[发明专利]基于臂形角区间的冗余自由度机械臂运动规划及评价方法

权利要求书

1.一种基于臂形角区间的冗余自由度机械臂运动规划及评价方法，其特征在于：该方法具体内容包括如下步骤：

步骤1：采用D-H法建立坐标系，将7R机械臂基座的前三个关节坐标进行旋转，重合于以第三个关节坐标系原点为球心的球铰，前三个关节等效为肩关节S；同样，将7R机械臂的末端最后三个关节坐标进行旋转，重合于以第六个坐标系原点为球心的球铰，后三个关节等效成腕关节W；第四个关节等效成肘关节E；末端执行器等效成手掌T；

步骤2：由给定的机械臂末端位姿，给定肩关节S、肘关节E和腕关节W的控制量，逆解求机械臂各关节角度；如果有解，设定为目标状态；如果无解，则无法对机械臂进行运动规划；

步骤3：求避关节极限的臂形角空间和避障碍物的臂形角空间，并求两空间交集；若交集不为空集，将此交集上的任意值带入运动学逆解模型，得到在给定机械臂末端位姿下避关节极限且避障碍物的所有可行的机械臂运动学逆解；从而针对末端执行器位姿，对关节工作空间运动轨迹进行规划；

步骤4：对所规划的关节工作空间进行采样，采用“成本收益比”指标对样点进行评价，提高运动规划效率，优化规划的路径更符合人的手臂运动形态。

2.根据权利要求1所述的一种基于臂形角区间的冗余自由度机械臂运动规划及评价方法，其特征在于：在步骤3中，所述求避关节极限的臂形角空间和避障碍物的臂形角空间交集，其具体求解方法如下：

对于给定的末端位姿，输入末端位姿的6个约束，并输入3个控制参数arm，elbow，wrist，求解避关节极限的臂形角区间和避障碍物的臂形角区间；

(1)避关节极限

将臂形角Ψ映射到关节转角，输入给定的末端位姿约束和每个关节转角的极限值，计算满足关节转角极限的臂形角Ψ的取值区间；

设前三个关节转角分别为q₁,q₂,q₃，当前三个关节角都为0时，记为Σ_o

Σo=[XΣoYΣoZΣo]...(1)]]>

记坐标系Σ_arm为Σ_D

ΣD=ΣDΨ=0×Rot(x,Ψ)...(2)]]>

记肩膀S变换矩阵为R_S

RS=ΣD×ΣoT=ΣDΨ=0×Rot(x,Ψ)×ΣoT...(3)]]>

由

Rot(x,Ψ)=1000cosΨ-sinΨ0sinΨcosΨ...(4)]]>

得

Rot(x,Ψ)＝I₃+sinΨ[x×]+(1-cosΨ)[x×]²………………………(5)

式中：I₃是3×3单位矩阵，[x×]是单位向量x构成的反对称阵；展开式(5)得到关于Ψ的变换函数，带入式(2)得到Σ_D关于Ψ的直观表达式，将Σ_D带入式(3)得

R_S(q₁,q₂,q₃)＝A_ssinΨ+B_scosΨ+C_s………………………(6)

式中：记A_s(i,j)为a_sij；

记B_s(i,j)为b_sij；

C_s＝(x×x^T)×Σ_o^T，记C_s(i,j)为c_sij。

从球铰变换公式有

Rs(q1,q2,q3)=**C1S2**S1S2-C3S2S3S2C2...(7)]]>

式中：S_i指sin(q_i)，C_i指cos(q_i)，i＝1,2,3，*表示忽略该项；

将系数矩阵A_s、B_s和C_s求出代入式(6)，并与式(7)对比[3,3]位置的值得：

cosq₂＝a_s33sinΨ+b_s33cosΨ+c_s33………………………(8)

从q₂的范围就能够推导出cosq₂的范围，进而计算出满足q₂极限范围Ψ的取值范围；同理，分别求出满足q₁、q₃、q₅、q₆和q₇极限范围Ψ的取值范围；

对于七个关节，除去第四个关节，求交集得避关节极限的Ψ区间为

Ψ＝{Ψ₁}∩{Ψ₂}∩{Ψ₃}∩{Ψ₅}∩{Ψ₆}∩{Ψ₇}………………………(9)

式中，Ψ_i(i＝1,2,3,,5,6,7)是避关节q_i极限的区间；

(2)避障碍物

将臂形角Ψ在取值范围上离散，输入末端位姿约束、臂形角Ψ和控制参数elbow，得到肘关节E的位置；则连杆SE和连杆EW位置已知，运用碰撞检测算法对每一个Ψ的取值，测试连杆SE和连杆EW是否与障碍物发生碰撞；从所有测试结果中，解算出没有发生碰撞的臂形角Ψ的若干个连续区间的并集；

给定末端位姿T_d将末端位置沿着Z₇相反方向移动d_WT即得到W点位置；记W点位置为p_w，则

pW1=Td*00-dWT1...(10)]]>

对给定末端位姿T_d，肩关节S和腕关节W在空间的位置已经固定，过点E在直线SW上的垂足为O_e，求出设为H。，以O_e为坐标系原点，以为X轴正方向，以O_eE为Y轴正方向，建立坐标系O_e-X_eY_eZ_e，

Xe=SW→/||SW→||Ye=(Z×Xe)/||Z×Xe||Ze=Xe×Ye...(11)]]>

向量

向量

已知O_e-X_eY_eZ_e的齐次变换矩阵T_e

在中位置取为Ψ的起始线，E在O_e-X_eY_eZ_e中的坐标如式(14)

EZe=H cosΨEYe=-H sinΨEXe=0...(15)]]>

求出E在O-XYZ中的坐标E：

E1=TeEXeEYeEze1...(16)]]>

则S、E、W、T四个点的位置确定，从而确定连杆SE、连杆EW和连杆WT位置，利用碰撞检测算法判断连杆是否与障碍物发生碰撞；

综上所述，将避关节极限子空间和避障碍物子空间的臂形角求交集，得到在给定机械臂末端位姿下避关节极限且避障碍物的所有可行的机械臂运动学逆解。

3.根据权利要求1所述的一种基于臂形角区间的冗余自由度机械臂运动规划及评价方法，其特征在于：在步骤4中，所述对所规划的关节工作空间进行采样，采用“成本收益比”指标对采样点进行评价，其具体实现方法如下：

对机械臂的腕关节位置空间进行采样，对每一个采样点x_new判断腕关节到机械臂末端之间的连杆是否碰撞，如果碰撞则放弃该采样点；如果没有碰撞则求解该位置对应的避关节极限形角区间和避障碍物臂形角区间的交集，若交集不为空集，则该采样点成为一个X_free中的采样点，并记录每个采样点的臂形角区间；

对所加入的每一个采样点，采用“成本收益比ratio”进行评价：

ratio=ln(|costPathproProject|+1)+ln(|||XsXg||-proProject||XsXg|||+1)]]>

式中，成本costPath是根节点到该采样点的欧氏距离路程；收益proProject是从根节点到该采样点在根节点到目标点连线上的投影；X_s的ratio为0，x₂的路程成本为(d₁+d₂)，X₂的收益为p₂。