[发明专利]五自由度旋转链式码垛机械臂的运动控制方法

权利要求书

1.一种五自由度旋转链式码垛机械臂的运动控制方法，其特征在于，包括如下步骤：

1)获取机械臂末端运动轨迹曲线，在该运动轨迹曲线上，连续选取包含起始点和终止点的n个坐标点，记为P_i(x_i,y_i,z_i)，其中i＝1,...,n；

2)对选取的每一坐标点P_i(x_i,y_i,z_i)进行运动学反解计算，得到该坐标点对应的各驱动关节处的转动角度信息；从而获得运动轨迹曲线与各臂转动角度之间的对应关系；

3)将上述对应关系下发至运动控制卡，通过伺服驱动系统驱动各臂转动，通过传感器检测并反馈转动信号，各臂联动完成给定的运动轨迹。

2.根据权利要求1所述的五自由度旋转链式码垛机械臂的运动控制方法，其特征在于，步骤2)中所述的对坐标点P_i(x_i,y_i,z_i)进行运动学反解计算，具体包括如下步骤：

1)将选取的坐标点P_i(x_i,y_i,z_i)由末端执行器5所在的坐标系{5}移动到臂4所在的坐标系{4}中，得到运动轨迹曲线上的点在坐标系{4}中相应的坐标(x_i+l₄sinθ₁,y_i-l₄cosθ₁,z_i)；其中l₄为臂4的长度，θ₁为臂1的转动角度；

2)不考虑机械臂末端执行器转动角度时，坐标系{4}处的齐次变换矩阵可表示为：

F′=nx′ox′ax′xi+l4sinθ1ny′oy′ay′yi-l4cosθ1nz′oz′az′zi0001]]>

其中，n′、o′、a′分别为法向向量、方位向量和接近向量，用于描述臂4的姿态，

由10T(θ1)-1F′=21T(θ2)32T(θ3)43T(θ4),]]>其中为连杆k相对连杆k-1的变换矩阵，得：

(xi+l4sinθ1)cosθ1+(yi-l4cosθ1)sinθ1=l2cosθ2+l3cos(θ2+θ3)---(1)(xi+l4sinθ1)sinθ1-(yi-l4cosθ1)cosθ1=d2-d3-d4---(2)zi=-l2sinθ2-l3sin(θ2+θ3)---(3)]]>

其中，l₂、l₃分别为臂2和臂3的长度，θ₂、θ₃分别为臂2、臂3的转动角度，d₂、d₃、d₄分别为臂2与1间的距离、臂3与2间的距离、臂4与3间的距离，

由(2)式可得，

tan(θ1-α)=(d2-d3-d4-l4)/xi2+yi2-(d2-d3-d4-l4)2,]]>其中tanα＝y_i/x_i，求解得到坐标点P_i(x_i,y_i,z_i)相应的臂1的转动角度θ₁；3)由步骤2)中(1)、(3)式可得，

tanθ3=(2l2l3)2-(((xicosθ1+yisinθ1)2+zi2)-(l22+l32))2((xicosθ1+yisinθ1)2+zi2)-(l22+l32),]]>求解得到坐标点P_i(x_i,y_i,z_i)相应的臂3的转动角度θ₃；

4)由步骤2)中(3)式可得，

tan(θ2+β)=-zil22+l32+2l2l3cosθ3-zi2,]]>

其中tanβ=l2sinθ3(l2+l3cosθ3),]]>

求解得到坐标点P_i(x_i,y_i,z_i)相应的臂2的转动角度θ₂；

5)根据θ₄＝θ₃-θ₂，得到坐标点P_i(x_i,y_i,z_i)相应的臂4的转动角度θ₄；

6)考虑机械臂末端执行器的转动角度时，码垛机械臂末端执行器位姿的齐次变换矩阵可表示为：

F=nxoxaxxinyoyayyinzozazzi0001,]]>

其中n、o、a分别为法向向量、方位向量和接近向量，用于描述末端执行器5的姿态；

根据与连杆变换方程联立，码垛机械臂的运动学方程可表示为

F=10T(θ1)21T(θ2)32T(θ3)43T(θ4)54T(θ5),]]>

得到：

tan(θ5+γ)=xicosθ12+cos(θ2+θ3+θ4)2+sinθ12-ox2,]]>其中tanγ=sinθ1cosθ1cos(θ2+θ3+θ4),]]>

求解得到坐标点P_i(x_i,y_i,z_i)相应的臂5的转动角度θ₅。