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

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

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

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

【技术特征摘要】
1.一种机械臂逆解多解选择和时间最优轨迹规划算法，其特征在于：在运动学正解和逆解的基础上，它的总体算法架构由：逆解多解选择算法和时间最优轨迹规划算法，两大部分组成；其中，解多解选择算法的总体思路为：根据当前的机械臂位姿进行正-逆解或者逆-正解转换，取转换结果与当前机械臂位姿的差的范数最小的逆解作为最合适的一组解；时间最优轨迹规划算法的总体思路为：利用七次多项式轨迹规划方法进行轨迹规划，计算该轨迹下的各约束条件对应的值，并验证该值是否等于约束条件阈值，若不等于，则取该值与对应约束条件阈值的差值进行比例控制负反馈到原来的轨迹规划时间上，以缩小该差值，同时利用激活条件来区分和决定不同约束条件是否进行负反馈；
以一个五自由度机械臂在笛卡尔空间进行点到点的轨迹规划为例，算法的步骤分为以下四步：
第一步：运动学正解；
①建立机械臂结基座标系{0}、各关节坐标系{1,2.3.4}和末端坐标系{5}，根据机械臂结构参数建立机械臂D-H参数表；
表1.1机械臂D-H参数表



②根据建立的坐标系和机械臂D-H参数表，得出各坐标之间的齐次变换矩阵；从坐标系oi-1-xi-1yi-1zi-1到坐标系oi-xiyizi的变换矩阵为



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



得出机械臂末端相对于基座标系的位置和机械臂末端相对于基座标系的姿态，即机械臂的运动学正解p＝Forward_kinematics(θ)，其中，p＝[xyzαβγ]为已知得机械臂末端位姿，θ＝[θ1θ2θ3θ4θ5]为所求的机器人各关节角度；
第二步：运动学逆解；
①由(2)式可得



令式(2)的第一列和第四列相等，可得六个等式；
②根据以上得到的六个等式，可依次求解得θ1的两个解，每个θ1可求得θ5的两个解，每组(θ1,θ5)可求得θ3的两个解，再求得θ1的一个解和θ4的一个解；
③综上，总共可得到八个解，即机械臂的运动学逆解θ＝Inverse_kinematics(i,p),(i∈[1,8])；
第三步：基于七次多项式的轨迹规划；
①为了使得加速度也连续，避免机械臂启动和停止时加速度突变产生震动，可采用七次多项式进行优化，优化目标为保证加速度连续可导，即约束轨迹规划起点和终点的加加速度大小；具体机械臂末端轨迹规划公式为



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



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



即完成了以加速度连续可导为目标的七次多项式轨迹规划；
第四步：轨迹优化；
①逆解多解选择算法：首先，在机械臂逆解的八组解中依次选择一组解i＝1,(i∈[1,8])；然后，读取机械臂当前各关节角θ＝[θ1θ2θ3θ4θ5]，根据当前的机械臂位姿进行正解得到当前的末端位姿，再根据得到当前的末端位姿进行逆解得到第i组逆解下的各关节角度θi＝[θi1θi2θi3θi4θi5]，最后比较|θi-θ|≤εθ，其中εθ为自定义角度阈值，用于从八组逆解中区分出最合适的解，若|θi-θ|≤εθ满足，则该解为最优解；若不满足...

【专利技术属性】
技术研发人员：陈光荣，郭盛，曲海波，陈亚琼，
申请(专利权)人：北京交通大学，
类型：发明
国别省市：北京;11