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

说明书

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

技术领域

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

背景技术

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

发明内容

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

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

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

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

第一步：运动学正解。

①建立机械臂结基座标系{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的变换矩阵为

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