一种基于RS-CMSA算法的机器人逆运动学求解方法

摘要

本发明公开一种基于RS‑CMSA算法的机器人逆运动学求解方法。包括以下步骤：(1)基于PoE公式对串联机器人进行正运动学建模；(2)将机器人逆运动学求解问题建模成非线性多模态优化问题；(3)利用RS‑CMSA算法对非线性多模态优化问题进行求解，获得给定位姿对应的所有符合约束的逆解。本发明针对一般的串联机器人，不限自由度数，不限关节种类，与一般的智能算法只能求解出一个逆运动学解不同，本发明基于RS‑CMSA算法进行串联机器人逆运动学求解，可避免优化过程陷入局部收敛，保证了收敛精度，收敛精度为1e‑6mm，并且可获得给定位姿对应的所有符合约束的逆解，为后续的运动规划和运动控制提供基础。

说明书

技术领域

本发明涉及机器人逆运动学求解领域，具体设计一种基于RS-CMSA算法的机器人逆运动学求解方法。

背景技术

机器人的逆运动学求解一直是机器人运动学研究的主要问题之一，与机器人的离线编程、轨迹规划和运动控制等研究工作密不可分。多年来经过许多学者的不断研究，发现机器人的逆运动学问题具有以下两个主要特性：1、对于非冗余自由度串联机器人，通常具有有限个逆解个数(SICILIANO B,KHATIB O.Springer Handbook of Robotics[M].Springer,2016.)，即一个末端位姿一般对应着机器人的多组关节变量值；2、逆运动学问题的本质是一个多元非线性超越方程组求解问题，常利用代数性质和符号公式将问题规约为一元高次方程求根问题。目前关于机器人的逆运动学求解方法一般分为：封闭解法、数值解法及智能算法。

封闭解法分为几何法和代数法。只有几何结构满足Pieper准则的机器人才有封闭解。封闭解法可以根据公式或几何结构推导出关节角度的表达式，因此可以求出所有逆解，并且计算速度快，但是封闭解法只适合特定结构的机器人，不具备通用性，并且推导过程复杂，需要丰富的经验。

数值解法常用的是迭代算法，利用雅克比矩阵建立迭代公式，给定初始值不断进行迭代来逼近精确解。后续为了解决在奇异位置雅克比矩阵无法求逆并且求逆计算量庞大的问题，衍生出很多相关算法。如引入伪逆近似雅克比矩阵求逆解决无法求逆问题，引入阻尼最小二乘法来改善在奇异点处稳定性差的缺陷，使用雅克比转置矩阵代替雅克比逆矩阵，减少计算量。但是数值解法只能求解出一个在初始关节值附近的逆解，甚至当初始值设定不当时，无法收敛。

智能算法的主要思想是将机器人逆运动学求解问题建模成以最小化位姿误差为目标的优化问题来解决，对机器人结构不做要求，通用性强。主要包括粒子群算法、遗传算法、退火算法和神经网络算法等。一般智能算法对高维、非线性和多模态问题的求解并不理想，目前关于智能算法求解机器人逆解的研究中，通常只能求解出一个逆解。但是在一些场景下，研究可获得机器人所有逆解的通用性算法是十分必要的，例如，在进行机器人的运动规划时，获得所有逆解后，通过能耗、避障要求和可操作度等约束选择较优的逆解进行运动规划；在基于任务驱动的最优机器人构型生成及模块化机器人的装配构型优化(吴文强.可重构模块化机器人建模、优化与控制[D/OL].华南理工大学,2013[2020–10–23].)时，需要通用的可求出所有逆解的机器人求逆算法来判断生成的机器人构型是否为任务可达的。李光等(李光,肖帆,杨加超,等.基于唯一域方法的机器人逆向运动学求解[J].农业机械学报,2019,50(10):386–394.)将机器人的关节空间划分为与逆运动学多解数目一致的唯一域，在每个唯一域中利用CMA-ES算法搜索逆解，但唯一域的划分与机器人结构有关；Tabandeh等利用自适应小生境遗传算法进行机器人逆运动学求解，可求得六自由度机器人的8组逆解，但算法容易陷入局部收敛甚至发生早熟，收敛精度无法保证。

发明内容

为了获得一般串联机器人的所有逆解，避免优化过程陷入局部收敛，保证收敛精度，本发明提出一种基于RS-CMSA(Covariance Matrix Self-Adaptation EvolutionStrategy with Repelling Subpopulations)算法的串联机器人逆运动学求解方法，该方法可用于任意串联机器人，不限制自由度数目和关节类型，可以求解出给定位姿对应的所有符合约束的逆解，并且具有较高的精度，从而为后续机器人的运动规划、控制提供基础。

本发明将其逆运动学求解问题抽象建模成一个非线性、多模态优化问题，最后利用RS-CMSA(Covariance Matrix Self-Adaptation Evolution Strategy with RepellingSubpopulations)算法进行多模态优化问题的求解，该方法可获得非冗余串联机器人的所有逆解，有效避免优化过程陷入局部收敛，并且保证收敛精度。

本发明至少通过如下技术方案之一实现。

一种基于RS-CMSA算法的机器人逆运动学求解方法，包括以下步骤：

(1)对串联机器人进行正运动学建模；

(2)将机器人逆运动学求解问题建模成非线性多模态优化问题；

(3)基于RS-CMSA算法对非线性多模态优化问题进行求解，获得给定位姿对应的所有符合约束的逆解；

将本发明提出的算法应用于PUMA560机器人逆运动学求解问题，验证算法的有效性和稳定性。

优选的，所述步骤(1)的正运动学建模包括建立基坐标系及末端坐标系，确定机器人初始位置时的末端位姿及各关节相对基坐标系进行螺旋运动的螺旋轴，然后利用PoE公式自动生成机器人正运动学方程。

优选的，在空间中定义一个基坐标系{s}，在末端执行器上定义一个末端坐标系{b}，并将机器人置于初始位置，指定每个关节正向转动的方向，末端坐标系相对于基坐标系的初始位姿用M∈SE(3)表示。

优选的，末端坐标系的位姿T表示成

优选的，相对基坐标系的目标位姿矩阵和当前位姿矩阵分别表示为T

优选的，相对末端坐标系的目标位姿为

优选的，机器人正运动学方程的生成方式为：将每个关节的螺旋运动施加给后续的连杆，具体的方程如下：

其中，M为机器人初始时的末端执行器相对基坐标系的位姿，S

优选的，所述步骤(2)基于对机器人逆运动学求解问题性质的分析：将机器人逆运动学求解问题建模成非线性多模态优化问题，其中确定优化问题的目标函数、设计变量、约束条件，具体描述如下：

min f(θ)

s.t.θ∈[θ

式中，θ为设计变量，属于关节空间，同时需要满足机器人关节活动范围的约束条件，θ

优选的，所述RS-CMSA算法包括以下步骤：

f)重启策略：每次重启后增加各个子种群个体数量，增加的策略如下：

其中，λ表示子种群个体数，N表示子种群数量，

g)精英保存策略：保存0≤N

h)自适应调整禁忌区域的形状与大小：不同的子种群禁忌区域不同，禁忌区域的形状及大小与最小值区域的形状及大小相适应；

i)禁忌点的关键度计算：优先检查关键度高的禁忌点；

j)峰谷函数检测机制：利用峰谷函数对终止的子种群进行分析，使用黄金分割法检查子种群搜索到的最优个体是否形成新的盆地，如果一个最优个体被确定可形成新的盆地，就将该最优个体添加到存档中，否则识别出收敛于哪一个盆地，当最优个体的适应度与存档中个体的适应度之差小于容忍误差∈

优选的，所述子种群的进化方式为：

步长控制：

对数递减权重：

均值更新：

协方差矩阵自适应：

其中，w

本发明可用于任意串联机器人，不限制自由度数目和关节类型，基于RS-CMSA算法求解，可避免优化过程陷入局部收敛，保证了收敛精度，收敛精度为1e-6mm，并且可获得给定位姿对应的所有符合约束的逆解，为后续的运动规划和运动控制提供基础。

与现有技术相比，本发明一种基于RS-CMSA算法的机器人逆运动学求解方法具有以下有益效果：

本发明将机器人逆运动学求解问题建模成一个非线性多模态的优化问题，利用RS-CMSA算法进行优化问题的求解，可以有效避免优化过程陷入局部收敛，保证了收敛精度，并且获得给定位姿对应的所有符合约束的高精度逆解。本发明针对的是一般串联机器人逆运动学求解问题，不限制关节类型和自由度数目，皆可准确求解出所有逆解。

附图说明

图1为本实施例一种基于RS-CMSA算法的机器人逆运动学求解方法流程图；

图2为本实施例一般串联机器人正运动学建模原理图；

图3为本实施例基于PoE公式的正运动学建模流程图；

图4为本实施例两个位姿误差的物理意义表示图；

图5为本实施例PUMA560机器人的结构图。

具体实施方式

为了更好地理解本发明，下面结合附图对本发明作进一步地描述。

本发明提出一种基于RS-CMSA算法的机器人逆运动学求解方法，用于机器人逆运动学求解领域，基于RS-CMSA算法，可获得给定位姿对应的所有符合约束的逆解，且具有较高的精度。具体实施步骤如图1所示，包括以下步骤：

(1)基于PoE公式对串联机器人进行正运动学建模；

图2是一般串联机器人正运动学建模原理图。基于指数积公式(PoE)建构一般串联机器人的正运动学模型。指数积公式是：将每个关节的螺旋运动施加给后续的连杆。如图2所示为一般的串联机器人，由n个单自由度关节串联而成。在空间中定义一个基坐标系{s}，在末端执行器上定义一个末端坐标系{b}，并将机器人置于初始位置(所有关节变化量为0)，指定每个关节正向转动的方向。末端坐标系相对于基坐标系的初始位姿用M∈SE(3)表示。

现在假定关节n对应的关节变量为θ

若假定关节n-1也允许发生变化，即给连杆n-1施加一个螺旋运动，该运动效果会传递至连杆n，因为连杆n与连杆n-1通过关节n连接。此时，末端执行器的位姿T为：

不断重复上述过程，可以得到当所有关节都发生变化的时候，末端执行器的位姿应为：

图3是基于PoE公式的正运动学建模流程图，根据上述建模原理，可知自动生成机器人正运动学方程的流程如下：先建立基坐标系{s}及末端坐标系{b}，随后确定初始位置时的末端位姿及各关节相对基坐标系进行螺旋运动的螺旋轴S

图4为两个位姿误差的物理意义表示图。{d}为目标坐标系，T

(2)将机器人逆运动学求解问题建模成非线性多模态优化问题；

一个末端位姿对应关节空间多个逆解，即目标函数具有非线性多模态性质，因此将机器人逆运动学求解问题建模成非线性多模态优化问题，一般串联机器人的逆运动学求解优化问题主要由设计变量、目标函数、约束条件三部分构成，具体描述如下：

min f(θ)

s.t.θ∈[θ

式中，θ为设计变量，属于关节空间，同时需要满足机器人关节活动范围的约束条件[θ

(3)基于RS-CMSA算法对非线性多模态优化问题进行求解，获得给定位姿对应的所有符合约束的逆解。

从机器人逆运动学求解问题的特性出发，一个末端位姿一般对应着机器人的多组关节变量值，其中，关节变量是连续的，目标函数是非线性方程且导数不存在，因此该问题为一个非线性多模态的优化问题。所以选择RS-CMSA算法进行问题的求解。

表1是RS-CMSA算法的伪代码

表1是RS-CMSA算法的伪代码，RS-CMSA是一种新的多模态小生境优化方法，即带排斥子种群的协方差矩阵自适应进化策略的多模态优化。整个RS-CMSA算法不需要依赖问题进行参数调试，只需要输入初始种群数量，问题维数和最大函数评估次数即可，后续的进化控制参数都不需要修改。具体的算法过程见表1的伪代码。其中，该RS-CMSA算法主要由五个核心部分组成：

a)重启策略：每次重启后增加各个子种群个体数量，增加的策略如下：

其中，λ表示子种群个体数，N表示子种群数量，

b)精英保存策略：保存0≤N

步长控制：

对数递减权重：

均值更新：

协方差矩阵自适应：

其中，w

c)自适应调整禁忌区域的形状与大小：不同的子种群禁忌区域不同，禁忌区域的形状及大小与最小值区域的形状及大小相适应；

d)禁忌点的关键度计算：优先检查关键度高的禁忌点，减少时间复杂度；

e)峰谷函数检测机制：利用峰谷函数对终止的子种群进行分析，使用黄金分割法检查子种群搜索到的最优个体是否形成新的盆地，如果一个最优个体被确定可形成新的盆地，就将该最优个体添加到存档中，否则识别出收敛于哪一个盆地，当最优个体的适应度与存档中个体的适应度之差小于容忍误差∈

通过上述五个核心策略，RS-CMSA算法利用禁忌点及禁忌区域实现小生境，同时设计关键性度量，从而优先检查关键禁忌点，减少复杂度；采用协方差矩阵自适应策略及精英保存策略进行子种群的进化；进行不断重启，逐渐增加子种群个体数量来寻找更难发现的最小值点；使用峰谷函数判断一个解是否为新的最小值点。进而有效的求解出所有符合约束的高精度逆解，实现了无半径参数的小生境方法来寻找给定位姿对应的所有的高精度逆解，为后续机器人的运动规划和决策提供基础。

(4)将本发明的方法应用于PUMA560机器人逆运动学求解问题，验证算法的有效性和稳定性。PUMA560机器人的结构如图5所示，各关节旋量坐标参数为表2所示。

表2 PUMA560各关节旋量坐标参数

结构参数：l

当θ＝0时，末端坐标系相对于基坐标系的变换矩阵为：

关节活动范围：

θ

θ

取工作空间中的一个点为目标位姿进行逆运动学求解：

求得目标位姿T

表3目标位姿对应的8组逆解

本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动，这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。