CN112757306B - 一种机械臂逆解多解选择和时间最优轨迹规划算法 - Google Patents

Abstract

本发明涉及足式机器人机械臂领域，特别涉及一种机械臂逆解多解选择和时间最优轨迹规划算法。该算法可以根据当前的机械臂位姿迅速从逆解多解中选择最合适的一组解，有效规避奇异位型，实现不同逆解之间的平滑切换，保证机械臂控制和动作的连续性和不振荡；并利用七次多项式和条件比例控制相结合的轨迹规划方法规划出时间最优的轨迹，保证关节角度、角速度、角加速度、角加加速度的平滑连续可导，同时满足多目标多约束条件，满足执行器规范，含关节角速度最大值约束或关节角加速度最大值约束等。该算法大大减小了多解选择和轨迹规划的计算量，可以实现实时高效的时间最优轨迹规划，并可推广到低算力控制器上应用。

Description

一种机械臂逆解多解选择和时间最优轨迹规划算法

技术领域

本发明涉及足式机器人机械臂领域，特别涉及一种机械臂逆解多解选择和时间最优轨迹规划算法。

背景技术

机械臂是高精度，多输入多输出、高度非线性、强耦合的复杂***。因其独特的操作灵活性，已在工业装配，安全防爆等领域得到广泛应用。机械臂是一个复杂***，存在着参数摄动、外界干扰及未建模动态等不确定性，因而机械臂的建模模型也存在着不确定性。机器臂数学模型包括运动学和动力学，运动学和动力学均包含正解和逆解。通常，多自由度机械臂运动逆解存在多解的情况，对于不同的任务，需要规划机械臂笛卡尔空间或者关节空间的轨迹，作为机械臂目标给定信号进行控制。如何简洁高效的从逆解多解中选择最合适的一组解并规划时间最优的轨迹，在理论研究和实践应用上都是非常备受关注的。本发明提出的一种机械臂逆解多解选择和时间最优轨迹规划算法，可以根据当前的机械臂位姿迅速从逆解多解中选择最合适的一组解，有效规避奇异位型，实现不同逆解之间的平滑切换，保证机械臂控制和动作的连续性和不振荡；并利用七次多项式和条件比例控制相结合的轨迹规划方法规划出时间最优的轨迹，保证关节角度、角速度、角加速度、角加加速度的平滑连续可导，同时满足多目标多约束条件，满足执行器规范，含关节角速度最大值约束或关节角加速度最大值约束等。该算法大大减小了多解选择和轨迹规划的计算量，可以实现实时高效的时间最优轨迹规划，并可推广到低算力控制器上应用。

发明内容

本发明要解决的问题是提出一种机械臂逆解多解选择和时间最优轨迹规划算法，其算法架构简洁高效、具备快速从逆解多解中选择最合适的一组解并规划时间最优轨迹的能力。

本发明解决其技术问题采用的技术方案是：

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

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

第一步：运动学正解。

①建立机械臂结基座标系{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＝[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₀；其次，基于七次多项式插值规划轨迹：p,

再次利用得到的逆解求出各关节角度：θ,

最后判定是否满足情形1：

如果满足，则以式(7)来减小规划时间T＝t_f-t₀重新规划轨迹；如果不满足，则进入情形2。在情形2中，与情形1相同，首先基于七次多项式插值规划轨迹：p,

再次利用得到的逆解求出各关节角度：θ,

最后判定是否满足情形2：如果满足，则以式(7)来增大规划时间T＝t_f-t₀重新规划轨迹；如果不满足，则退出情形2。两种情形讨论结束。

③输出θ,

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

该算法中，多项式插值轨迹规划的阶次由具体的轨迹规划需求决定，不仅限于七次；在关节空间内，在连续轨迹规划中，该算法可以得到同样的效果；该算法也适用于其他逆解存在多解的多自由度机械臂或串联机器人。

本发明和已有技术相比所具有的有益效果：本发明提出的一种机械臂逆解多解选择和时间最优轨迹规划算法，没有采用迭代原理，其算法架构简洁高效、具备快速从逆解多解中选择最合适的一组解并规划时间最优轨迹的能力，可以根据当前的机械臂位姿迅速从逆解多解中选择最合适的一组解，有效规避奇异位型，实现不同逆解之间的平滑切换，保证机械臂控制和动作的连续性和不振荡；并利用七次多项式和条件比例控制相结合的轨迹规划方法规划出时间最优的轨迹，保证关节角度、角速度、角加速度、角加加速度的平滑连续可导，同时满足多目标多约束条件，满足执行器规范，如关节角速度最大值约束或关节角加速度最大值约束等。该算法大大减小了多解选择和轨迹规划的计算量，可以实现实时高效的时间最优轨迹规划，并可推广到低算力控制器上应用。

附图说明

图1五自由度机械臂示意图；

图2五自由度机械臂结构参数与坐标系示意图；

图3算法流程图；

图4轨迹优化后的机械臂各关节角度；

图5轨迹优化后的机械臂各关节角速度；

图6轨迹优化后的机械臂各关节角加速度；

图7轨迹优化后的机械臂各关节角加加速度；

图8轨迹优化后的机械臂末端位置；

图9轨迹优化后的机械臂末端姿态；

图10初始规划时间10s的时间优化过程示意图；

图11初始规划时间20s的时间优化过程示意图；

具体实施方式

结合附图对本发明做进一步说明。

本发明要解决的问题是提出一种机械臂逆解多解选择和时间最优轨迹规划算法，其算法架构简洁高效、具备快速从逆解多解中选择最合适的一组解并规划时间最优轨迹的能力。

本发明解决其技术问题采用的技术方案是：

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

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

第一步：运动学正解。

①如图2所示，建立机械臂结基座标系{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＝[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₀；其次，基于七次多项式插值规划轨迹：p,

再次利用得到的逆解求出各关节角度：θ,

最后判定是否满足情形1：

如果满足，则以式(7)来减小规划时间T＝t_f-t₀重新规划轨迹；如果不满足，则进入情形2。在情形2中，与情形1相同，首先基于七次多项式插值规划轨迹：p,

再次利用得到的逆解求出各关节角度：θ,

最后判定是否满足情形2：如果满足，则以式(7)来增大规划时间T＝t_f-t₀重新规划轨迹；如果不满足，则退出情形2。两种情形讨论结束。

③输出θ,

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

算法流程图如图3所示。图4-9为笛卡尔空间点到点的轨迹规划优化结果，分别为：各关节角度、各关节角速度、各关节角加速度，各关节角加加速度，末端位置、末端姿态。图10-11分别为初始规划时间分别为10s和20s的时间优化过程。

该算法中，多项式插值轨迹规划的阶次由具体的轨迹规划需求决定，不仅限于七次；在关节空间内，在连续轨迹规划中，该算法可以得到同样的效果；该算法也适用于其他逆解存在多解的多自由度机械臂或串联机器人。

本发明和已有技术相比所具有的有益效果：本发明提出的一种机械臂逆解多解选择和时间最优轨迹规划算法，没有采用迭代原理，其算法架构简洁高效、具备快速从逆解多解中选择最合适的一组解并规划时间最优轨迹的能力，可以根据当前的机械臂位姿迅速从逆解多解中选择最合适的一组解，有效规避奇异位型，实现不同逆解之间的平滑切换，保证机械臂控制和动作的连续性和不振荡；并利用七次多项式和条件比例控制相结合的轨迹规划方法规划出时间最优的轨迹，保证关节角度、角速度、角加速度、角加加速度的平滑连续可导，同时满足多目标多约束条件，满足执行器规范，如关节角速度最大值约束或关节角加速度最大值约束等。该算法大大减小了多解选择和轨迹规划的计算量，可以实现实时高效的时间最优轨迹规划，并可推广到低算力控制器上应用。

Claims (3)

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；两种情形讨论结束；

③输出

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

2.权利要求1所述的一种机械臂逆解多解选择和时间最优轨迹规划算法，其特征在于：该算法中，多项式插值轨迹规划的阶次由具体的轨迹规划需求决定，不仅限于七次；在关节空间内，在连续轨迹规划中，该算法可以得到同样的效果；该算法也适用于其他逆解存在多解的多自由度机械臂或串联机器人。

3.权利要求1所述的一种机械臂逆解多解选择和时间最优轨迹规划算法，其特征在于：本算法没有采用迭代原理，其算法架构简洁高效、具备快速从逆解多解中选择最合适的一组解并规划时间最优轨迹的能力，可以根据当前的机械臂位姿迅速从逆解多解中选择最合适的一组解，有效规避奇异位型，实现不同逆解之间的平滑切换，保证机械臂控制和动作的连续性和不振荡；并利用七次多项式和条件比例控制相结合的轨迹规划方法规划出时间最优的轨迹，保证关节角度、角速度、角加速度、角加加速度的平滑连续可导，同时满足多目标多约束条件，满足执行器规范，如关节角速度最大值约束或关节角加速度最大值约束等；该算法大大减小了多解选择和轨迹规划的计算量，可以实现实时高效的时间最优轨迹规划，并可推广到低算力控制器上应用。

Images