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

说明书

技术领域

本发明属于机械臂运动规划领域，具体涉及一种基于臂形角区间的冗余自由度机械臂运动规划及评价方法。

背景技术

机械臂运动规划的重要研究内容是规划出一条满足各类指标的最优路径。冗余自由度机械臂关节空间具有自运动特性，逆解具有多解性，可通过满足避关节极限或避障碍物指标求得冗余自由度机械臂逆解空间。

针对避关节极限问题，目前多数方法是基于雅可比矩阵零空间的关节速度向量进行求解，用关节极限值构造出排斥因子或用关节极限平均值构造出吸引因子来约束关节转角。该类方法仅能得到唯一可行解，没有充分利用避关节极限区间，并且在奇异值附近求解广义逆运算量大，存在积分漂移误差，易出现有解但无法求出的问题。

针对避障碍物问题，多数方法是在机械臂上选取关键点，用关键点到障碍物的距离构建势场排斥机械臂的位置。该类方法存在机械臂运动过程中关键点不容易选取，复杂情况下易陷入局部极值问题。另外也有文献通过引入神经网络算法求解基于人工势场建立的模型，研究冗余自由度机械臂的避关节极限和避障碍物问题，该类方法过于复杂，无法实时在线进行运动规划。

发明内容

为解决现有冗余自由度机械臂运动规划运算量大、存在积分漂移误差和易陷入局部极值的问题，本发明提出一种基于臂形角区间的冗余自由度机械臂运动规划及评价方法，该方法引入臂形角约束，在关节位置级上显式地应用自运动特性，求解满足避关节极限和避障碍物的所有可行的运动学逆解；在所规划的工作空间进行采样，并对采样点进行评价，以提高运动规划的效率和精度，使优化规划的路径更符合人的手臂运动形态。

本发明为达到上述目的，采取如下技术方案：

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

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

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

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

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

进一步，在步骤3中，所述求避关节极限的臂形角空间和避障碍物的臂形角空间交集，其具体求解方法如下：

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

(1)避关节极限

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

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

记坐标系Σ_arm为Σ_D

记肩膀S变换矩阵为R_S

由

得

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)