[发明专利]一种机械臂逆解多解选择和时间最优轨迹规划算法

权利要求书

1.一种机械臂逆解多解选择和时间最优轨迹规划算法，其特征在于：在运动学正解和逆解的基础上，它的总体算法架构由：逆解多解选择算法和时间最优轨迹规划算法，两大部分组成；其中，解多解选择算法的总体思路为：根据当前的机械臂位姿进行正-逆解或者逆-正解转换，取转换结果与当前机械臂位姿的差的范数最小的逆解作为最合适的一组解；时间最优轨迹规划算法的总体思路为：利用七次多项式轨迹规划方法进行轨迹规划，计算该轨迹下的各约束条件对应的值，并验证该值是否等于约束条件阈值，若不等于，则取该值与对应约束条件阈值的差值进行比例控制负反馈到原来的轨迹规划时间上，以缩小该差值，同时利用激活条件来区分和决定不同约束条件是否进行负反馈；

以一个五自由度机械臂在笛卡尔空间进行点到点的轨迹规划为例，算法的步骤分为以下四步：

第一步：运动学正解；

①建立机械臂结基座标系{0}、各关节坐标系{1,2.3.4}和末端坐标系{5}，根据机械臂结构参数建立机械臂D-H参数表；

表1.1机械臂D-H参数表

②根据建立的坐标系和机械臂D-H参数表，得出各坐标之间的齐次变换矩阵；从坐标系o_i-1-x_i-1y_i-1z_i-1到坐标系o_i-x_iy_iz_i的变换矩阵为

③根据机械臂结基座标系和末端坐标系之间的齐次变换矩阵

得出机械臂末端相对于基座标系的位置和机械臂末端相对于基座标系的姿态，即机械臂的运动学正解p＝Forward_kinematics(θ)，其中，p＝[x y z α β γ]为已知的机械臂末端位姿，θ＝[θ₁ θ₂ θ₃ θ₄ θ₅]为所求的机器人各关节角度；

第二步：运动学逆解；

①由(2)式可得

令式(2)的第一列和第四列相等，可得六个等式；

②根据以上得到的六个等式，可依次求解得θ₁的两个解，每个θ₁可求得θ₅的两个解，每组(θ₁,θ₅)可求得θ₃的两个解，再求得θ₁的一个解和θ₄的一个解；

③综上，总共可得到八个解，即机械臂的运动学逆解θ＝Inverse_kinematics(i,p),(i∈[1,8])；

第三步：基于七次多项式的轨迹规划；

①为了使得加速度也连续，避免机械臂启动和停止时加速度突变产生震动，可采用七次多项式进行优化，优化目标为保证加速度连续可导，即约束轨迹规划起点和终点的加加速度大小；具体机械臂末端位姿p(t)的轨迹规划公式为

其中，p(t)为已知的机械臂末端位姿p＝[x y z α β γ]关于时间t变化的轨迹函数，分别为p(t)的一阶、二阶和三阶导数；

②给定约束条件，即起点和终点的位置，速度，加速度，加加速度分别为：

③将式(5)代入式(4)可求得七次多项式系数分别为

即完成了以加速度连续可导为目标的七次多项式轨迹规划；

第四步：轨迹优化；

①逆解多解选择算法：首先，在机械臂逆解的八组解中依次选择一组解i＝1,(i∈[1,8])；然后，读取机械臂当前各关节角θ＝[θ₁ θ₂ θ₃ θ₄ θ₅]，根据当前的机械臂位姿进行正解得到当前的末端位姿，再根据得到当前的末端位姿进行逆解得到第i组逆解下的各关节角度θ_i＝[θ_i1 θ_i2 θ_i3 θ_i4 θ_i5]，最后比较|θ_i-θ|≤ε_θ，其中ε_θ为自定义角度阈值，用于从八组逆解中区分出最合适的解，若|θ_i-θ|≤ε_θ满足，则该解为最优解；若不满足，则取另一组逆解重新计算并比较；

②基于条件比例控制的时间最优轨迹优化算法

该算法可分为两种情形：

情形1：第三步得到的规划轨迹的角速度和角加速度虽然都满足执行器规范，即关节可执行的最大角速度和角加速度但其规划时间T＝t_f-t₀比较长，并不是最优的，其中t₀,t_f分别为经过起始点和终止点的时间；此时，需要缩小规划时间以达到时间最优同时又满足执行器规范；

情形2：第三步得到的规划轨迹的角速度和角加速度虽然大于执行器规范，即关节可执行的最大角速度和角加速度此时，最简单的解决方式就是增加规划时间以达到时间最优同时又满足执行器规范；

根据以上两种情形，结合比例-积分-微分(PID)控制原理，可设计没有迭代的、简单易实现的、基于条件比例控制的时间最有控制器：

其中分别为时间最优控制器中速度和加速度的误差的调节参数，类似PID控制原理中的比例参数；式(7)中的比例控制是否被激活取决于其对应的执行器规范是否得到满足，即条件比例控制的由来；特别地，和可以由各关节或各轴单独定义；在理想条件下，规划时间T＝t_f-t₀达到最优时，规划轨迹的角速度和角加速度与执行器规范允许的最大角速度和角加速度相等，即此外，根据多约束和多目标的需求，式(7)右式需要加入最大加加速度项，其他运动学和动力学约束项；为了提高时间最优控制器的瞬态过渡性能，也需要参考PID控制加入积分控制和微分控制；

该算法必须先讨论情形1在讨论情形2以保证执行器规范等约束完全得到满足；首先，初始化规划参数：规划起始点p₀，规划终止点p_f，规划运动时间T＝t_f-t₀；其次，基于七次多项式插值规划轨迹：再次利用得到的逆解求出各关节角度：最后判定是否满足情形1：如果满足，则以式(7)来减小规划时间T＝t_f-t₀重新规划轨迹；如果不满足，则进入情形2；在情形2中，与情形1相同，首先基于七次多项式插值规划轨迹：再次利用得到的逆解求出各关节角度：最后判定是否满足情形2：如果满足，则以式(7)来增大规划时间T＝t_f-t₀重新规划轨迹；如果不满足，则退出情形2；两种情形讨论结束；

③输出作为关节控制的给定，算法结束。