机械臂运动学

机械臂运动学基础1、机械臂的运动学模型机械臂运动学研究的是机械臂运动，而不考虑产生运动的力。运动学研究机械臂的位置，速度和加速度。机械臂的运动学的研究涉及到的几何和基于时间的内容，特别是各个关节彼此之间的关系以及随时间变化规律。典型的机械臂由一些串行连接的关节和连杆组成。每个关节具有一个自由度，平移或旋转。对于具有n个关节的机械臂，关节的编号从1到n，有n+1个连杆，编号从0到n。连杆0是机械臂的基础，一般是固定的，连杆n上带有末端执行器。关节i连接连杆i和连杆i-1。一个连杆可以被视为一个刚体，确定与它相邻的两个关节的坐标轴之间的相对位置。一个连杆可以用两个参数描述，连杆长度和连杆扭转，这两个量定义了与它相关的两个坐标轴在空间的相对位置。而第一连杆和最后一个连杆的参数没有意义，一般选择为0。一个关节用两个参数描述，一是连杆的偏移，是指从一个连杆到下一个连杆沿的关节轴线的距离。二是关节角度，指一个关节相对于下一个关节轴的旋转角度。为了便于描述的每一个关节的位置，我们在每一个关节设置一个坐标系，对于一个关节链，Denavit和Hartenberg提出了一种用矩阵表示各个关节之间关系的系统方法。对于转动关节i，规定它的转动平行于坐标轴zi-1，坐标轴xi-1对准从zi-1到zi的法线方向，如果zi-］与%相交，则xi-1取z」］xzi的方向。连杆，关节参数概括如下：连杆长度ai沿着xi轴从zi-1和zi轴之间的距离；•连杆扭转ai从zi-1轴到zi轴相对xi-1轴夹角；连杆偏移di从坐标系i-1的原点沿着zi-1轴到*轴的距离；•关节角度0ixi-1轴和xi轴之间关于zi-1轴的夹角。

对于一个转动关节。j是关节变量，q是常数。而移动关节d.是可变的次j是恒定的。为了统一，表示为运用Denavit-Hartenberg（DH）方法，可以将相邻的两个坐标系之间的变换关系表示为一个4x4的齐次变换矩阵i-1A=icos0isin0i0一sin0cos以iicos0cos以iisini-1A=icos0isin0i0一sin0cos以iicos0cos以iisin以i0sin0sin以一cos0sin以cos以i0acos0

asin0ii-1i其中0T表示坐标系i相对于世界坐标系0的位置与姿态，简称位姿。i2、正向和反向运动学对于一个n-轴刚性连接的机械臂，正向运动学的解给出的是最后一个连杆坐标系的位置和姿态。重复利用上式，得到0T=0AiAn-iA=K（q）n12n机械臂末端位姿在笛卡尔坐标系中有6个自由度，3个平移，3个旋转。所以，一般来说具有6个自由度的机械臂可以使末端实现任意的位姿。总的机械臂变换0T一般简写为Tn，对6个自由度的机械臂简写为T6。对于任意的机械臂，无论其它有多少个关节，具有什么结构，正向运动学解都是可以得到的。在机械臂的路径规划中，用到的是反向运动学的解q=K-1（0T），它给出了特定的末端位姿对应的机械臂的关节角度。一般来说，反向运动学的解不是唯一的，对具有某种结构的机械臂，封闭解可能不存在。对于6自由度的机器人而言，运动学逆解非常复杂，一般没有封闭解。只有在某些特殊情况下才可能得到封闭解。不过大多数工业机器人都满足封闭解的两个充分条件之一（Pieper准则）（1）三个相邻关节轴交于一点（2）三个相邻关节轴相互平行如果机械臂多于6个关节，称关节为冗余的，这时解是欠定的。如果对于机械臂某个特别的位姿，解不存在，称这个位姿为奇异位姿。机械臂的奇异性可能是由于机械臂中某些坐标轴的重合，或位置不能达到引起的。机械臂的奇异位姿分为两类：（1）边界奇异位姿，当机械臂的关节全部展开或折起时，使得末端处于操作空间的边界或边界附近，雅克比矩阵奇异，机械臂的运动受到物理结构的约束，这时机械臂的奇异位姿称为边界奇异位姿。（2）内部奇异位姿，两个或两个以上的关节轴线重合时，机械臂各个关节的运动相互抵消，不产生操作运动，这时机械臂的奇异位姿称为内部奇异位姿。机械臂运动学逆解的方法可以分为两类：封闭解和数值解、在进行逆解时总是力求得到封闭解。因为封闭解的计算速度快，效率高，便于实时控制。而数值解法不具有这些特点。机械臂运动学的封闭逆解可通过两种途径得到：代数法和几何法。一般而言，非零连杆参数越多，到达某一目标的方式也越多，即运动学逆解的数目也越多。在从多重解中选择解时，应根据具体情况，在避免碰撞的前提下通常按"最短行程”准则来选择。同时还应当兼顾"多移动小关节，少移动大关节"的原则。n个自由度的机械臂的末端位姿由n个关节变量所决定，这n个关节变量统称为n维关节矢量，记为q。所有的关节矢量构成的空间称为关节空间。机械臂末端的位姿用6个变量描述，3个平移仪归）和3个旋转（①/o，①J，记x=（x,y,乙气,气,％），x是机械臂末端在基xyzxyz坐标空间中的坐标，所有的矢量X构成的空间称为操作空间或作业定向空间。工作空间是操作臂的末端能够到达的空间范围，即末端能够到达的目标点集合。值得指出的是，工作空间应该严格地区分为两类：（1）灵活（工作）空间指机械臂末端能够以任意方位到达的目标点集合。因此，在灵活空间的每个点上，手爪的指向可任意规定。⑵可达（工作）空间指机械臂末端至少在一个方位上能够到达的目标点集合。机械臂各关节驱动器的位置组成的矢量称为驱动矢量s，由这些矢量构成的空间称为驱动空间正向运动学驱动空间4-关节空间4-工作空间运动学逆解3、Jacobian矩阵机械臂的Jacobian矩阵表示机械臂的操作空间与关节空间之间速度的线性映射关系，对于一个n轴的机械臂，机械臂末端在基坐标系中的速度是X=Jq其中x是6个元素的向量。••对于6个关节机械臂Jacobian矩阵是方阵，如果它是可逆的，则可以由机械臂的末端速度求出各个关节的速度。Jacobian矩阵在机械臂的奇异位姿上是不可逆的。在实际应用中，当机械臂的末端位置接近奇异位置时，Jacobian矩阵是病态的，可能导致关节速度不能正确地得到。上式解决的是正速度问题，即已知q和q求末端执行器的速度x。对于逆速度解问题，由上••

式可以得到速度逆解公式为q—J-1x,注意到此时需要求雅可比矩阵的逆，由线性方程组理论知上式对任意的X,q都有解的必要条件是雅可比矩阵的秩rank(J)=6，这意味着机械臂的自由度数n>6o这也说明了具有冗余自由度的机械臂，在末端位姿固定的条件下，能使关节在一个较大的关节空间的子空间中运动，有效地避开障碍或奇异位姿，并把关节位移限制在允许范围内，从而具有更大的运动灵活性。雅可比矩阵可以看成是从关节空间到操作空间运动速度的传动比同时也可用来表示两空间之间力的传递关系。对于冗余自由度机械臂，其雅可比矩阵是长方矩阵，因J满秩且方程个数少于未知数个数，所以有无穷多个解，这时，一般是求其中的最小范数解，或采用加权最小范数解也就是说使|\qTD^最小的解旗中D是对称正定加权矩阵。此时的解是使机械臂在能量消耗最小的情况下的解。这时，逆速度问题便转为：求q满足q—J-1x且使L—1qTDq最小。实际上等同于求性能-2指标L在约束条件q—J-1X下的极值。应用Lagrange乘子法，以上极值为题的解是q—D-1Jt(JD-iJt)-ix，当D=I时，雅可比矩阵是J+-Jt(JJt)-i,称为雅可比矩阵的伪逆。下面通过一个两自由度的平面机械臂说明雅可比矩阵的特性，根据右图中的几何关系容易求得x—lc+1cc—cos(0),c—cos(0+0)11212111212y—1s+1ss—sin(0),s—sin(0+0)11212111212两边微分后写成矩阵形式dxdydx801d01dxdF2d02」d0d012e「dx］「-1s-1s-1s］「d0即IIdxdydx801d01dxdF2d02」d0d012e「dx］「-1s-1s-1s］「d0即II—I11212212II1IdyII1c+1c1c|ld0112122122.—ls—ls—ls.,__,，由J=11212212可以看出，J阵的值随末端位置的不同而不同，即。1和02的lC+1clc1211212212改变会导致J的变化。对于关节空间的某些位姿，机械臂的雅可比矩阵的秩减少，这些位姿称为机械臂的奇异位姿。上例机械臂雅可比矩阵的行列式为：det(J)=〈孕讽气)，当02=0°或02=180°时，机械臂的雅可比行列式为0，矩阵的秩为1，这时机械臂处于奇异位姿。机械臂在操作空间的自由度将减少。如果机械臂的雅可比J是满秩的方阵，相应的关节速度即可求出，即Q=J-1x，上例平面1「lcls2R机械臂的逆雅可比矩阵J-1=占_*"_ls2—]s，显然，当02趋于0°(或122L1121211212」180°)时，机械臂接近奇异位姿，相应的关节速度将趋于无穷大。为了补偿机器人末端执行器位姿与目标物体之间的误差以及解决两个不同坐标系之间的微位移关系问题，需要讨论机器人连杆在作微小运动时的位姿变化。假设一变换的元素是某个变量的函数对该变换的微分就是该变换矩阵各元素对该变量的偏导数所组成的变换矩阵乘以该变量的微分。例如给定变换T为：一tttt一11121314T=tttt21222324tttt31323334tttt1-41424344若它的元素是变量x的函数，则变换T的微分为:

dT—dt—12dxdt——-dxdt—32dxdtdt—13dxdt—23dxdtdt—14dxdt―24dxdt—34dxdxdt—43dxdt下面讨论机械臂的微分运动，设机械臂某一连杆相对于基坐标系的位姿为T,dT—dt—12dxdt——-dxdt—32dxdtdt—13dxdt—23dxdtdt—14dxdt―24dxdt—34dxdxdt—43dxdt所以有一dT—[Trans(d,d,d)Rot(k,dO)-144]T根据齐次变换的对称性，若微运动是相对某个连杆坐标系i(动系)进行的(右乘)，则T+dT可以表示为T+dT=T-Trans(d,d,d)Rot(k,do)所以有一dT—T[Trans(d,d,d)Rot(k,dO)-144"令△-Trans(d,d,d)Rot(k,do)-1为微分算子-则相对基系有dT=A0T,相对i系xyz4x4有dT=TAi。这里A的下标不同是由于微运动相对不同坐标系进行的。在机械臂运动学中微分变换分为微分平移和微分旋转两类。微分平移变换与一般平移变换一样，其变换矩阵为:100dx010dyTrans(dx,dy,dz)—001dz0001

由于微分旋转时0-0，所以sine-de,cose-1将它们代入旋转变换通式中得微分旋转表达式:「1-kdekde0-kdez1y-kde0Rot(k,de)=zx-kdekde10yLox001_于是得到微分算子A=Trans(d,d,d)Rot(k,de)-1，即xyz4x4「0-kdekdedxkdez一yA=0-kdedyzkdex-kde0dzyxL0000微分旋转与有限旋转相比，有一些特殊的性质，下面分别说明。(1)微分旋转的无序性，当e—0时，有sine-de,cose—1.若令6x=dex,6y=dey,5z=d0z，则绕三个坐标轴的微分旋转矩阵分别为「1000「「106y0-「1-6z00_01-6x001006z100zRot(x,6x)=Rot(y,6y)=Rot(z,6z)=06x10-6y010001000010001_0001略去2次项，得到「106y0-「106y0"6x6y1-6x001-6x0「.Rot(x,6x)Rot(y,6y):=-6y6x10=-6y6x10l0001_l0001_「16x6y6y0-「106y0"01-6x001-6x0xRot(y,6y)Rot(x,6x)=-6y6x10—=-6y6x10L0001_一0001_两者结果相同，可见这里左乘与右乘等效。结论：微分旋转其结果与转动次序无关，这是与有限转动(一般旋转)的一个重要区别。(2)微分旋转的可加性，考虑两个微分旋转复合后的效果

Rot(x,8x)Rot(y,8Rot(x,8x)Rot(y,8y)Rot(z,8z)=-8z8y01-8x08x10若Rot(6x,6y,6z)和Rot(6x’，阿，6Z）表示两个不同的微分旋转，则两次连续转动的结果为：Rot(8x,8y,8z)Rot(8x\8y\8z')=-(8z+8z')8y+8y'01-(8x+8x')08x+8x'100018z+8z^-(8y+8y')由等效转轴和等效转角与Rot(x,8x)Rot(y,8y)Rot(z,8z)等效，有Rot(k,d。)=Rot(x,8x)Rot(y,8y)Rot(z,8z)kd。kd。z-kd。y0-kd。一z1kd。x0kd。y-kd。0]0_18z-8z18y-8x0-0x10—=-8y8x10010001所以有kxde=6x，kyde=6y，kzde=6z，将它们代入△得一0-88d一8z0-8ydxA=zxy-880dyxz0000可见，微分变换由两个部分组成6微分转动矢量，d微分平移矢量，合称为微分运动矢量,可表示为D=(d,d,d,8,8,8)tTOC\o"1-5"\h\zxyzxyz10501，相对固定系的微分平移矢量d=[100.5]，~001例：已知一个坐标系A=100010000微分旋转矢量6=[00.110501，相对固定系的微分平移矢量d=[100.5]，微分旋转矢量6=[00.10]，求微分变换dA。-0-88d--000.11"800-8d0000A=zxy=-880d-0.1000.5yxz_0000_0000~000.11-"00110"-00.101-000010050000...dA=AA=——-0.1000.5010000-0.1-0.5_0000_0001_0000_下面讨论两坐标系之间的微分关系，设第一个坐标系为i系，第二个坐标系为j系不失一般性，假定j系就是固定的0系。因为A=00

8z

-8y0因为A=00

8z

-8y0-8z08x05y-8x00dx

dy

dz

0所以A0T=0TA0iiinoaxxxnoayyynoazzz00008zi-8y0，A=0T-1A00T，整理得到PxPyPz1-8z8ydxiii0-8xdyii8x0dzii000=y((8xp=y((8xp)+d)=o((8xP)+d)=咄8xp)+d)ix8广oddddxnnn(P乂n)(pxn)(pxn)dxixyzxyz0dyooo(P乂o)(pxo)(pxo)dyixyzxyz0dzaaa(pxa)(pxa)(pxa)dzi=xyzxyz一08x000nn一n8xixyz08y000ooo8yixyz08zJ000aaa8zil_xyz0iyiz对于任何三维矢量p=[px,py,?』,其反对称矩阵s(p)定义为：0-ppzyp0-pz0-ppyxs(p)=xn0axxxn0axxxn0ayyyn0azzz0r=idi5i」0Rti0-0Rts(0p)0Rti类似地，任意两坐标系｛A｝和｛B｝之间广义速度的坐标变换为：bRA0-bRS(AP)bRAbRA0-bRS(AP)bRAaVaVaR,=BA^L0-aRS(BP)BVaRBB300110100501000001BO例：已知一个坐标系A=，相对固定系的微分平移矢量d=[100.5]，微分旋转矢量S=[00.10]，求A系中等价的微分平移矢量dA和微分旋转矢量皓解：将d=[100.5]和6=[00.10]代入得到d=[。0A5•广遗5L5广气5dx=〃』(5xp)+d)dy=0^((5xp)+d)d_=ag(5xp)+d)5.1L5=[0.10°LoA4、机械臂轨迹规划机械臂的轨迹规划可以在关节空间也可以在笛卡尔空间中进行或者说机械臂轨迹规划是指在关节空间或者笛卡尔空间中研究机械臂轨迹生成方法。简言之，机械臂轨迹规划是运动学逆解的实际应用，它描述了机械臂在多维空间中的运动路线。在知道末端位姿的前提下，通过运动学逆解得到各个关节在相应时刻的转动量或者平移量合理的规划指的是规划出的角位移曲线、角速度曲线以及角加速度曲线，可以有效地减少了机械臂在运动过程中的冲击和振动，使机械臂的工作寿命得以延长。得到d=[。0A5•广遗5L5广气5机械臂的关节角位移变化率比较小，能够有效地防止了机械臂工作时的振动和冲击。机械臂关节角速度和角加速度变化均平顺连续，从而有效避免了机械部件的磨损，能够保证整个机械臂系统的长期、稳定的运行，满足机械臂的工作要求。5、robotics工具箱中的相关函数link建立一个连杆对象,例如对于本次竞赛的机械臂，根据连杆参数得到L{1}=link([pi/20012000]);L{2}=link([pi/200000]);L{3}=link([-pi/200140.80pi]);L{4}=link([-pi/271.8000pi/2]);L{5}=link([+pi/271.8000pi]);L{6}=link([-pi/20000pi/2]);L{7}=link([000129.600]);robot建立一个机械臂对象R=robot(L)noname(7axis,RRRRRRR)grav=[0.000.009.81]standardD&HparametersalphaAthetaDR/P1.57080120R(std)1.5708.00R(std)-1.57080140.8R(std)-1.570871.80R(std)1.570871.80R(std)-1.570800R(std)00129.6R(std)drivebot用滑块控制的机械臂图形drivebot(R,ones(1,7)*pi)plot机械臂的图形显示plot(R,[pi/2pi/200000])fkine串联机械臂正向运动学计算tr=fkine(ROBOT,Q)ROBOT表示机械臂对象，Q机械臂关节坐标值。tr=fkine(R,[000pi/2000])tr=0.0000-0.00001.0000129.6000-0.00001.00000.0000-0.0000-1.0000-0.00000.0000-20.80000001.0000ikine串联机械臂逆向运动学计算q二ikine(ROBOT,T)q=ikine(ROBOT,T,Q)q=ikine(ROBOT,T,Q,M)输入变量ROBOT表示机械臂对象，T机械臂末端变换矩阵。输出变量q机械臂关节的角度(单位是弧度)，一般来说逆运动学的解不是唯一的，取决于初始值Q，缺省时是0向量。如果机械臂的自由度(DOF)小于6，由于解空间的维数大于机械臂的自由度，这时需要第4个输入量M来确定笛卡尔坐标(手腕对应的坐标系)中的哪些量在求解中被忽略。M中有6个元素，分别表示沿着x,y,z方向的平移和相对于x轴，y轴，z轴的旋转，值是0(忽略)或1。非零元素的个数应该等于机械臂的自由度。例如，对典型的有5个自由度的机械臂，一般是忽略相对手腕坐标的转动，这时M=[111110]。另外一种用法是qt=ikine(ROBOT,TG)qt=ikine(ROBOT,TG,Q)qt=ikine(ROBOT,TG,Q,M)输入变量ROBOT表示机械臂对象，TG是4x4xN机械臂末端变换矩阵。输出变量qt是一组(N个)TG对应的关节坐标。一行对应一个输入变换，每一步的初始值取上一步的值。求解使用机械臂Jacobian矩阵的伪逆，这是数值求解方法，对于特定机械臂逆运动学解(如果可能)应该尽量使用解析解。但是这种方法可以得到奇异点上的解，零空间中的关节角度可以任取。q=ikine(R,tr)q=0.00000.00000.00000.7854-0.0000-0.78540.0000注意：对于机械臂末端的一个位置与姿态，逆运动学计算不是唯一的，验证tr=fkine(R,q)tr=0.0000-0.00001.0000129.6000-0.00001.00000.0000-0.0000-1.0000-0.00000.0000-20.80000001.0000transl计算平移变换tr=transl(X,Y,Z)返回机械臂末端坐标X,Y,Z对应的齐次表换矩阵tr=transl(129.6,0,20.8)tr=1.000000129.600001.000000001.000020.80000001.0000[XYZ]'=transl(T)返回齐次表换表示中的平移值，作为一个3兀素的列向量xyz=transl(tr)xyz=129.6000020.8000ctraj计算工作空间中两点T0,T1之间的轨迹tc=ctraj(T0,T1,N)tc=ctraj(T0,T1,R)返回从T0到T1笛卡尔坐标系的轨迹TCN表示轨迹中的点数。在第1中情况下，轨迹中的点在T0到T1中等距离分配。在第2中情况下，向量R给出轨迹中每个点的距离，R中的元素取值为[01]。一个轨迹是4x4xN矩阵，最后一个下标表示点索引。旋转插值使用四元球形线性插值。tr0=fkine(R,[0000000])tr0=1.00000.0000-0.00001.0000-0.00000.0000-0.00000.0000-0.0000-0.00001.0000108.80000001.0000tr1=fkine(R,[pi/4pi/60pi/3000])tr1=0.6124-0.70710.353695.60080.61240.70710.353695.6008-0.5000-0.00000.8660110.30050001.0000tc(:,:,1)=1.000000-0.000001.000000.0000001.0000108.80000001.0000tc(:,:,2)=0.8976-0.38220.219847.80040.35710.92260.145847.8004-0.2585-0.05230.9646109.55030001.0000tc(:,:,3)=0.6124-0.70710.353695.60080.61240.70710.353695.6008-0.5000-0.00000.8660110.30050001.0000transl(tc)ans=-0.00000.0000108.800047.800447.8004109.550395.600895.6008110.3005jtraj计算关节中两点Q0,Q1之间的轨迹[QQDQDD]=jtraj(Q0,Q1,N)[QQDQDD]=jtraj(Q0,Q1,N,QD0,QD1)[QQDQDD]=jtraj(Q0,Q1,T)[QQDQDD]=jtraj(Q0,Q1,T,QD0,QD1)轨迹中的点数是N,或者是一个时间向量T。插值使用7次多项式，边界速度由QD0,QD1指定，缺省时边界速度和加速度为0。q0=[pipipipipipipi];q1=[pipi/2000pi/20];tr0=fkine(R,[pipipipipipipi]);tr1=fkine(R,[pipi/2000pi/20]);[QT,QD,QDD]=jtraj(q0,q1,30);figuresubplot(2,2,1),plot(R,QT)subplot(2,2,2),plot(QT),gridon,legendCq1','q2','q3','q4','q5','q6','q7','Location','NorthWest')subplot(2,2,3),plot(QD),gridonsubplot(2,2,4),plot(QDD),gridon%注意：其中有一些曲线重合jacob0计算机械臂在基坐标系中Jacobian矩阵J二jacob0(ROBOT,Q)tr2jac计算机械臂在基坐标系中Jacobian矩阵J=TR2JAC(T)diff2tr微分表示转换为齐次变换tr=diff2tr(D)返回表示微分平移与旋转的齐次变换矩阵，矩阵中包含一个反对称的旋转子矩阵。tr2diff转换为齐次变换转换为微分表示D=tr2diff⑴D=tr2diff(T1,T2)第一种形式将齐次表换矩阵表示转换为6-元素向量微分表示。第二种形式返回6-元素向量，表示从T1到T2的在基坐标系中需要的微分移动。J=jacob0(R,q1)%Jacobiananddifferentialmotiondemonstration%Adifferentialmotioncanberepresentedbya6-elementvectorwithelements%[dxdydzdrxdrydrz]%wherethefirst3elementsareadifferentialtranslation,andthelast3%areadifferentialrotation.Whendealingwithinfinitisimalrotations,%theorderbecomesunimportant.Thedifferentialmotioncouldbewritten%intermsofcompoundedtransforms%transl(dx,dy,dz)*trotx(drx)*troty(dry)*trotz(drz)%butamoredirectapproachistousethefunctiondiff2tr()D=[.1.20-.2.1.1]'diff2tr(D)T=fkine(R,q1)%thenthedifferentialmotioninthesecondframewouldbegivenbyDT=tr2jac(T)*D;DQ=pinv(J)*DT;vel=[100000]';%translationalmotionintheXdirectionqvel=pinv(J)*vel;ans=-0.00000.0000-0.00000.0039-0.0000-0.00390.0000%这是计算工作空间轨迹和求逆解的另外一种方法。但是，如果Jacobian矩阵奇异时%会失效。如果机械臂的自由度大于6，即是冗余的，采用Jacobian矩阵伪逆计算，或%对Jacobian矩阵进行奇异值分解。附录rpy角与euler角（1）rpy角rpy角是描述船舶航行时的姿态的一种方法，将船的行驶方向作为Z轴测绕Z轴旋转称为滚动（Roll）角a，将绕Y轴（与海面平行）方向的旋转称为俯仰（Pitch）角p，取X轴与海面垂直方向，将绕X轴的旋转称为偏转（Yaw）角丫。机械臂末端的定义类似，故习惯上称为rpy角。描述运动坐标系的规则是：首先使运动坐标系的初始方位与固定坐标系重合，将运动坐标系绕固定坐标系X轴转动y，再将运动坐标系绕固定坐标系Y轴转动p，最后将运动坐标系