一种基于CES-SLO机械臂逆运动学求解方法

本发明涉及机械臂逆运动学求解，尤其涉及一种机械臂逆运动学求解方法，适用于医疗超声扫查机器人以及其他多自由度串联机械臂。

背景技术：

1、机械臂控制过程中，需要根据末端执行器的目标位置和姿态确定各关节角，这一过程称为逆运动学求解。对于结构较复杂的机械臂，逆运动学可能不存在便于直接计算的闭式解析解，也可能存在多个满足同一末端位姿的关节角解，因此通常需要采用数值迭代或智能优化方法进行求解。

2、现有数值迭代方法在复杂运动学模型下可能存在收敛速度、初值敏感性和数值误差等问题；粒子群、遗传、正余弦等智能优化方法虽然具有一定的全局搜索能力，但在具体逆运动学任务中仍可能出现搜索稳定性不足、陷入局部最优以及关节限位条件下的边界问题。

3、扇贝运动优化算法slo以扇贝吸水、喷水和滑行运动为仿生原型，通过不同阶段的搜索行为实现全局探索和局部开发。但将其直接用于受关节角边界约束的逆运动学问题时，仍存在以下不足：一是吸水阶段参考个体的选择随机性较强，不能随迭代进度逐渐增强优质解的引导；二是部分位置更新依赖随机数，搜索结果具有随机波动；三是对于关节角上下边界，若简单采用截断等处理方式，可能使个体在边界附近聚集。

4、此外，在连续轨迹跟踪任务中，各轨迹点的逆运动学问题通常可以独立求解。由于同一末端位姿可能对应多个关节角解，相邻轨迹点即使均获得较小的位姿误差，也可能分别选择不同的逆运动学分支，从而造成相邻关节角大幅跳变。对于医疗超声扫查机器人，这种跳变可能导致探头运动不平稳。因此，需要一种既能准确求解目标位姿，又能在多解条件下优先保持相邻轨迹点关节运动连续性的求解方法。。

技术实现思路

1、针对现有机械臂逆运动学求解方法在实际应用中存在的求解精度不足、收敛速度较慢、容易陷入局部最优或边界局部最优，以及在逆运动学存在多组可行解时相邻轨迹点对应的关节角容易发生较大跳变等问题，本发明提出一种基于混沌精英策略改进扇贝运动优化算法（chaos elite strategy scallop locomotion optimizer，ces-slo）的机械臂逆运动学求解方法。该方法在保证机械臂末端达到期望位置和姿态的基础上，通过在适应度函数中引入关节角变化量罚函数，并结合混沌局部搜索、精英策略和边界处理策略，对扇贝运动优化算法进行改进，从而从多个可行逆运动学解中选择关节变化较小的解，使相邻轨迹点对应的机械臂关节角能够更加平滑地过渡。本方法尤其适用于医疗超声扫查机器人，也能够应用于其他多自由度串联机械臂的逆运动学求解。本发明的基本思想是将逆运动学求解划分为两个相互配合的部分：第一部分解决如何更稳定地搜索到满足目标位姿的解，采用ces-slo进行优化；第二部分解决多个可行解中选择哪一个，在适应度函数中加入相邻轨迹点关节角变化量的动态罚函数。

2、首先，建立待求解机械臂的运动学模型，将机械臂各关节的关节角作为逆运动学求解的优化变量。对于六自由度串联机械臂，可采用改进dh参数法建立各相邻连杆坐标系之间的关系，通过相邻坐标系之间的齐次变换矩阵依次连乘，获得机械臂末端相对于基坐标系的正运动学位姿。对于给定的关节角向量，将其代入所建立的运动学模型即可得到对应的末端齐次变换矩阵。由此，将原本需要求解解析逆运动学方程的问题转化为以关节角为优化变量的数值寻优问题。

3、其次，根据机械臂当前计算得到的正运动学位姿与期望位姿之间的差异构建适应度函数。设待求解关节角向量为

4、

5、将所述关节角向量代入运动学模型后得到当前位姿齐次变换矩阵，将目标位置和姿态表示为期望齐次变换矩阵。通过计算两个齐次变换矩阵对应元素之间的欧氏距离，对机械臂末端当前位姿与期望位姿之间的误差进行统一评价，即：

6、

7、其中，表示期望位姿矩阵第行、第列元素，表示当前关节角对应的正运动学位姿矩阵第行、第列元素。通过同时考虑齐次变换矩阵中的位置和姿态信息，可以避免将位置误差和姿态误差完全割裂处理。原始技术材料将该函数定义为期望位姿矩阵与当前位姿矩阵之间的欧氏距离，并以此作为逆运动学优化的主要评价指标。

8、由于机械臂逆运动学通常可能存在多个满足同一期望位姿的关节角解，如果仅以末端位姿误差作为评价指标，优化算法可能从多个可行解中任意选择一个解，从而造成相邻轨迹点之间的关节角发生较大变化。尤其对于连续超声扫查任务，关节角的大幅跳变会降低机械臂运动过程的平稳性。因此，本发明在上述位姿误差函数的基础上增加用于约束相邻轨迹点关节角变化量的罚函数项。所述罚函数为：

9、

10、其中，为当前待求解轨迹点对应的关节角向量，为上一相邻轨迹点已经求得的关节角向量，为动态惩罚系数。通过上述罚函数，使优化过程不仅关注当前轨迹点的末端位姿误差，而且同时考虑当前关节角解与上一轨迹点关节角解之间的变化程度，从多个可行解中倾向于选择关节变化量较小的解。

11、进一步地，为避免固定惩罚系数过大造成求解结果过度偏向上一轨迹点，以及惩罚系数过小导致其无法有效约束关节变化，本发明采用动态惩罚系数。所述动态惩罚系数可按照当前适应度评价次数与最大评价次数之间的关系进行调整：

12、

13、在优化初期，取较大值，使搜索结果快速趋向于上一轨迹点关节角解附近，从而增强相邻轨迹点之间的连续性；随着迭代和适应度评价次数增加，逐渐减小，并在达到预设阶段后变为零，使优化后期主要依据末端位姿误差进行精细寻优，以兼顾关节运动平滑性和逆运动学求解精度。原始材料明确采用这种动态变化方式，以避免固定惩罚系数对最终求解精度造成过度影响。

14、最终，将位姿误差函数与动态罚函数进行组合，构建待优化的综合适应度函数：

15、

16、以所述综合适应度函数最小化为优化目标。

17、在获得适应度函数后，本发明采用ces-slo算法进行迭代寻优。ces-slo以扇贝运动优化算法的吸水阶段、喷水阶段和滑行阶段三阶段迭代框架为基础，并针对原算法的搜索随机性、参考个体选择方式以及越界问题进行改进。具体而言，在吸水阶段，通过个体与参考个体、全局最优个体以及优势个体之间的信息交互，使种群逐步向优良区域移动；在喷水阶段，通过转向运动及跳跃运动扩大搜索范围；在滑行阶段，使个体围绕当前全局最优个体进行进一步搜索，从而实现从全局探索向局部开发的逐步过渡。上述三阶段框架属于slo的基础迭代机制，本发明重点在于将混沌策略、精英策略和边界处理策略组合应用于机械臂逆运动学求解过程。

18、其中，第一项改进为混沌局部搜索策略。采用混沌算子产生具有较好遍历性的混沌序列，并利用该混沌序列替代原扇贝运动优化算法位置更新过程中的部分随机数。所述混沌变量可通过logistic混沌映射递推得到：

19、

20、其中，为第次迭代产生的混沌变量。通过将混沌序列引入种群位置更新，可以改变原算法随机搜索过程中完全依赖随机数的方式，使搜索过程具有更好的遍历特性，同时使混沌序列具有确定的递推关系。

21、第二项改进为自适应精英策略。在吸水阶段参考个体的选取过程中，不再单纯从精英个体集合中随机选取参考个体，而是根据当前迭代进度动态确定采用当前种群最优个体或者随机个体作为参考个体。定义自适应参数：

22、

23、其中，为当前迭代次数，为最大迭代次数。当大于随机数时，选取当前种群中的全局最优个体作为参考个体；否则从种群中随机选择个体作为参考个体。由于随迭代进行逐渐增大，因此随着搜索过程推进，采用当前最优个体作为参考个体的概率逐渐增加，使算法在前期具有较好的探索能力，在后期逐步增强对优良解区域的开发能力。

24、第三项改进为边界处理策略。由于关节角存在物理允许范围，在种群位置更新过程中，部分个体可能超出对应关节的上、下边界。对于任一个体，当其任一维度的关节角取值超出预设边界时，不直接将该维度简单截断，而是将该个体的适应度值设置为预设常数，使越界个体在后续选择过程中处于不利位置，并促使优化过程重新搜索正常解空间中的可行个体。该处理方式能够避免越界解对后续迭代产生不合理影响。

25、在完成上述初始化及改进策略设置后，对机械臂关节角种群进行迭代计算。首先根据各关节角上下限随机生成初始种群，并计算各个体对应的正运动学位姿及综合适应度；随后按照当前迭代阶段执行吸水、喷水或滑行位置更新，并在位置更新过程中引入混沌序列和自适应精英策略；对更新后的个体进行边界检查及适应度计算，并持续更新当前最优个体。达到预设最大迭代次数或者满足终止条件后，将当前全局最优个体作为当前期望位姿对应的逆运动学关节角解输出。对于连续轨迹，则将当前轨迹点获得的关节角解作为下一轨迹点求解时的，重复上述过程，从而使连续轨迹上的关节角保持较小变化。该技术路线能够将机械臂末端位姿精度与关节运动连续性统一到同一优化框架中。

26、本发明的有益效果在于：

27、其一，通过在逆运动学位姿误差函数中增加动态关节角变化罚函数，使优化算法能够从多个满足期望位姿的可行解中优先选择与上一轨迹点关节角变化较小的解，有利于降低连续轨迹中的关节角跳变；其二，通过混沌局部搜索策略替代部分随机搜索过程，有利于改善种群搜索的遍历性；其三，通过自适应精英策略，使算法随着迭代推进逐步增加对当前优良解的利用，有利于兼顾前期全局探索和后期局部开发；其四，通过边界处理策略降低越界个体对搜索过程的不利影响；其五，将上述策略与slo的吸水、喷水和滑行三阶段搜索机制相结合，使其能够用于多自由度串联机械臂的数值逆运动学求解。上述技术方案尤其适用于需要连续、平稳运动的医疗超声扫查机器人，同时也可推广至其他具有多解特性的机械臂逆运动学求解场景。