Velocity constraint equation A,=RA 市W=产+RA,S0,-i-RAs9,=0 A=RA RR=-1 Acceleration constraint equation 0=月+RA,s6+RA,0-月-R4,s"9,-R4s0 市》=月+RA,90-月-RAs西-Asy0}+As02=0 o-业a日i4e+4食-0 r》=g》日-y》=0i=(,p,） Right term of acceleration constraint equation yu.》=A,sy9p}-A,sp,j j j  A  RA i P j i i i Q j j j r i j     Φ  r  RA s  r  RA s ( , )  0 • Acceleration constraint equation  0   i P i i i P j i i i Q j j j Q j j j            Φ  r  RA s  RA s  r  RA s  RA s r RR  I ( , ) 2 2 i P j i i Q i j j P j i i i Q j j j r i j       Φ  r  RA s  r  RA s  A s  A s • Velocity constraint equation i ii  A  RA    0 r(i, j) r(i, j) r(i, j) Φ Φ q  q     T T T i  i j  j q  r  r  • Right term of acceleration constraint equation ( , ) 2 2 i P j i i Q j j r i j   A s   A s           0                 ( , ) ~ ~ 2 2 i P j i i Q j j i P i i i j Q j j j r i j            A s A s r I IA s r Φ I IA s