一种七自由度冗余机械臂的轨迹规划方法和系统

本发明属于运动控制领域，具体涉及一种七自由度冗余机械臂的轨迹规划方法和系统。

背景技术：

1、随着工业自动化和空间技术的发展，对机械臂的操作灵活性和功能性提出了更高的要求。冗余机械臂因其多余的自由度能够在执行复杂任务时提供更多的选择和灵活性。在空间站遥操作、水下作业或其他高风险环境中，冗余机械臂可以减少对人工操作的依赖，降低作业风险，提高系统的安全性和可靠性。

2、冗余机械臂在轨迹规划时需要考虑避开障碍物和机械臂的奇异位置，以保证机械臂的运动安全和避免运动学不确定性。冗余机械臂的轨迹规划不仅仅是为了达到目标位置，还需要考虑能量消耗、运动时间、平滑性等多个优化目标，提高机械臂的运动效率和性能。冗余机械臂的逆运动学问题通常具有多个解，这为轨迹规划提供了更多的可能性，但同时也增加了规划的复杂性。研究者需要深入分析机械臂的运动学和动力学特性，以设计出有效的轨迹规划算法。冗余机械臂在实际应用中，如航天器的对接、大型设备的维护、精密装配等任务中，其轨迹规划技术的发展对于提高操作的精确度和效率具有重要意义。随着技术的不断进步，冗余机械臂轨迹规划将继续朝着智能化、自动化和高效化的方向发展。

3、冗余机械臂轨迹规划的现有技术主要包括以下几种方法：

4、(1)逆运动学方法。通过逆运动学原理获取机械臂的轨迹插值点，然后采用多项式对轨迹进行插值，以实现机械臂的轨迹规划。

5、(2)改进粒子群优化方法。利用改进的粒子群算法对机械臂的运动轨迹进行优化，以实现时间最优的轨迹规划。改进的pso算法通过混沌序列初始化、自适应调整惯性权重和动态调整学习因子等策略，提高算法的全局搜索和局部搜索能力。

6、(3)多目标优化方法。针对冗余机械臂轨迹规划过程中构型不唯一的特点，建立减小机械臂动作幅度、能量消耗和关节运动冲击等指标的多目标优化模型。宋成与袁杰2020年在一种冗余机械臂多目标轨迹优化方法中提到了：通过改进的双模式混合差分进化算法(dhde)，优化机械臂运动轨迹并获得逆运动学解的数值解。

7、(4)构形平面方法。王安琪、魏延辉、韩寒等人2018年在基于构形平面的冗余机械臂轨迹规划方法及余秋蕾2024年5月在七自由度冗余机械臂轨迹规划与跟踪技术研究中提到：从构形平面入手，利用空间几何的方法对冗余机器人进行空间轨迹规划。通过空间矢量引导、避障路径的比较，快速找到空间优化的路径，实现多目标轨迹规划方法。该方法不依赖具体的机械臂工作构形，适用于更多自由度的冗余机械臂。

8、(5)基于三维点云场景重建的方法。针对受限空间下双臂机器人协同操作的轨迹规划问题，提出一种基于三维点云场景重建的复杂约束环境模型。将安全作业和灵巧操作任务抽象为数学约束形式，构建双臂协调优化模型，实现双臂机器人在复杂受限空间下的自主避碰和最优灵巧操作。

9、(6)其他进化算法。如遗传算法、差分进化算法等，用于优化机械臂的轨迹，以达到减少能量消耗、提高运动速度和避免关节极限等目的。

10、(7)a*算法和其他启发式搜索算法。在某些研究中，a*算法和其他启发式搜索算法被用于机械臂的路径规划，以寻找从起点到终点的最优路径。

11、这些技术在提高冗余机械臂轨迹规划的效率、准确性和鲁棒性方面发挥了重要作用。每种方法都有其特定的应用场景和优缺点，研究人员通常会根据实际问题的具体需求选择最合适的轨迹规划技术。

12、冗余机械臂在进行轨迹规划时，逆运动学问题是首要需要解决的。由于冗余机械臂具有多个自由度，逆运动学问题通常有多个解，这增加了选择合适关节配置的复杂性。此外，冗余机械臂需要在避开障碍物的同时避免奇异构型，奇异构型会严重影响机械臂的性能和安全性。最后，路径的平滑性也是冗余机械臂在进行轨迹规划时不可忽视的问题。如果规划的路径不够平滑，会导致机械臂运行时出现抖动或急动，影响其性能和寿命。

技术实现思路

1、本发明的目的在于现有技术在狭窄空间中冗余机械臂无法进行平滑轨迹规划的问题，提出了一种七自由度冗余机械臂的轨迹规划方法和系统。

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

3、一种七自由度冗余机械臂的轨迹规划方法，包括以下步骤：

4、根据七自由度冗余机械臂的局部坐标系确定dh参数，通过dh参数建立基座到末端位姿的正运动学模型，求解基座到末端位姿的正运动学模型，得到七自由度冗余机械臂末端位姿；

5、通过七自由度冗余机械臂末端位姿建立逆运动学模型，求解逆运动学模型得到七自由度冗余机械臂的关节转动角；

6、结合正运动学模型、逆运动学模型与rrt*算法对七自由度冗余机械臂进行连续轨迹规划，采用三次样条插值方法对rrt*算法规划的七自由度机械臂连续轨迹进行平滑处理。

7、进一步地，通过dh参数建立基座到末端位姿的正运动学模型的相邻坐标系间的变换矩阵如下所示：

8、

9、进一步地，将相邻坐标系间的变换矩阵代入每个关节连乘，得到通过方向和位置向量表示的基座到末端位姿的变换矩阵，如下式所示：

10、

11、其中，n、o、a分别表示末端坐标系x、y、z相对于基座坐标系姿态的单位矢量，p表示末端坐标系相对于基座坐标系的位置矢量。

12、进一步地，利用dh参数和通过方向和位置向量表示的基座到末端位姿的变换矩阵得到七自由度冗余机械臂的关节转动角包括θ1、θ2、θ3、θ4、θ5、θ6、θ7，具体如下所示：

13、

14、θ2＝0

15、

16、其中，

17、进一步地，采用三次样条插值方法对rrt*算法规划的七自由度机械臂连续轨迹进行平滑处理，具体为：利用基于三次样条函数的轨迹曲线在每个节点处的加速度公式，结合给定的区间两端的加速度和基于三次样条函数的轨迹曲线方程，计算出轨迹曲线在区间任意时刻的位置、速度和加速度，获得平滑的七自由度冗余机械臂末端连续轨迹，包括七自由度冗余机械臂的每个关节在移动过程中的位置、速度和加速度。

18、进一步地，轨迹曲线方程如下式所示：

19、qi(t)＝a0+a1(t-ti)+a2(t-ti)2+a3(t-ti)3

20、将轨迹曲线分段后，使用首尾端点的二阶导表示在插补周期的段样条曲线，则基于三次样条函数的轨迹曲线表示如下式所示：

21、

22、三次样条函数在插补周期区间两端的速度通过对基于三次样条函数的轨迹曲线求一阶导数获得，如下式所示：

23、

24、三次样条函数在插补周期区间两端的加速度通过对基于三次样条函数的轨迹曲线求二阶导数获得，如下式所示：

25、

26、其中，[ti,ti+1]为三次样条插值方法的插补周期。

27、进一步地，基于三次样条函数的轨迹曲线在每个节点处的加速度公式如下式所示：

28、aω＝b

29、ω＝[ω1,ω2,…,ωn-2,ωn-1]t

30、其中，a为(n+1)×(n+1)的三对角矩阵；ω为节点加速度，b为元素均已知的向量。

31、一种七自由度冗余机械臂的轨迹规划系统，包括正运动学模块，用于根据七自由度冗余机械臂的局部坐标系确定dh参数，通过dh参数建立基座到末端位姿的正运动学模型，求解基座到末端位姿的正运动学模型，得到七自由度冗余机械臂末端位姿；

32、逆运动学模块，用于通过七自由度冗余机械臂末端位姿建立逆运动学模型，求解逆运动学模型得到七自由度冗余机械臂的关节转动角；

33、轨迹规划模块，用于结合正运动学模型、逆运动学模型与rrt*算法对七自由度冗余机械臂进行连续轨迹规划，采用三次样条插值方法对rrt*算法规划的七自由度机械臂连续轨迹进行平滑处理。

34、一种电子设备，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现以上所述的一种七自由度冗余机械臂的轨迹规划方法。

35、一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现以上所述的一种七自由度冗余机械臂的轨迹规划方法。

36、与现有技术相比，本发明具有以下有益的技术效果：

37、本发明提供的一种七自由度冗余机械臂的轨迹规划方法，通过正运动学和逆运动学求解七自由度机械臂；连续轨迹规划考虑平滑轨迹，减少了机械臂的震荡现象。

38、进一步地，在机械臂轨迹规划中，结合rrt*算法和三次样条插值可以最终得到一条平滑且优化的机械臂运动轨迹。rrt*算法是一种用于解决路径规划问题的高效算法，它能快速找到从起点到终点的可行路径，并且随着迭代的进行，该路径会越来越优化，趋向于最优解。然而，rrt*算法生成的路径可能在某些情况下不够平滑，特别是当路径需要通过具有复杂几何形状的狭窄空间时。三次样条插值是一种常用的数学工具，用于生成平滑的曲线，它能够保证曲线在给定的控制点之间不仅位置连续，而且一阶导数的速度和二阶导数的加速度也是连续的。在机械臂轨迹规划中，这意味着可以生成一条不仅连接所有预定路径点，而且具有连续速度和加速度的轨迹，从而使得机械臂的运动更加平滑和自然。

39、进一步地，将rrt*算法与三次样条插值结合使用，解决了以下实际问题：

40、(1)通过三次样条插值优化rrt*算法生成的路径，减少机械臂在运动过程中可能出现的振动和冲击，提高运动的平稳性。

41、(2)rrt*算法能够找到接近最优的路径，结合三次样条插值可以进一步优化路径，使得机械臂在运动过程中的能耗降低，提高运动效率。

42、(3)相比于直接使用高阶多项式或其他复杂的插值方法，三次样条插值在保证足够平滑性的同时，计算量相对较小，适合实时系统应用。

43、(4)在存在障碍物和狭窄空间的环境中，rrt*都能够找到可行路径，结合三次样条插值可以生成更加适合机械臂运动学特性的轨迹，提高避障能力和适应性。

44、(5)平滑的轨迹有助于提高机械臂末端执行器的定位精度，对于需要高精度操作的应用场景尤为重要。

45、进一步地，rrt*算法结合三次样条插值在机械臂轨迹规划中的应用，能够生成既平滑又优化的轨迹，解决机械臂在复杂环境中的运动规划问题，提高运动的平稳性、效率和精度。

46、进一步地，正运动学和逆运动学是机器人学中两个重要的概念，它们与轨迹规划紧密相关，共同构成了机器人运动控制的基础。正运动学与轨迹规划的关系体现在路径的生成和碰撞检测：在轨迹规划中，正运动学用于计算在给定关节空间路径下，末端执行器在操作空间中的运动路径；通过正运动学，可以预测末端执行器在空间中的位置，帮助进行碰撞检测和避免。逆运动学与轨迹规划的关系体现在目标达成和路径优化两方面：逆运动学用于确定如何调整机器人的关节，以使末端执行器达到预定的目标位置和姿态；在轨迹规划中，逆运动学可以帮助优化关节空间中的路径，使得末端执行器能够以更有效或更舒适的方式达到目标。

47、进一步地，轨迹规划通常需要正运动学在预测末端执行器的路径，以及逆运动学来计算达到特定路径点所需的关节配置。在动态环境中，轨迹规划可能需要实时调整，此时逆运动学可以帮助快速重新计算关节配置，以适应环境变化。轨迹规划中的平滑性要求可以通过正运动学来评估，逆运动学则可以用于优化关节运动，减少震动和提高运动效率。

48、进一步地，正运动学和逆运动学是轨迹规划的两个关键组成部分。正运动学帮助确定末端执行器的路径，而逆运动学则用于计算实现该路径所需的关节运动。两者共同确保机器人能够高效、安全地完成任务。