一种基于RES-SCA算法的逆运动学实时解析解优化方法与流程

本发明涉及基于res-sca算法的逆运动学实时解析解优化。用于在人机遥操作系统中实时求解机器人末端位置和姿态的逆运动学解，确保机器人能够稳定和精确的完成遥操作指令。

背景技术：

在遥操作场景中，为了让机器人能够像人类一样灵活抓取或操作目标物体，通常需要机器人手臂要有运动学上的冗余。因此，许多机器人制造商制造了具有七自由度的机械臂来模仿人类手臂。然而，对于每个给定的末端目标，冗余将导致无穷解。这些无穷解的集合被称为封闭解。封闭解可用于避免关节限制、躲避障碍、奇异配置以及优化机械臂动力学等。

然而，想要得到冗余机械臂的运动学逆解是困难的。求解运动学逆解的常见方法是使用雅可比矩阵的数值迭代方法。然而，这种方法需要计算雅可比矩阵的逆，这不是一项简单的任务，尤其是对于冗余机械臂而言，此时的雅可比矩阵将不再是方阵。也有其它不依赖于雅可比矩阵的迭代方法，但依然需要多次迭代来确定逆运动学解，而且给出的解可能不是最优，甚至不是理想的。并且，数值迭代法只能获得一个解。因此，如何得到封闭形式的解析解成为研究的热点。

与数值迭代法不同，解析法通过根据机械臂连杆和关节的几何构型建立冗余参数来提供封闭解。lee等人提出了一种联合参数化技术来导出封闭解(详见：redundantarmkinematiccontrolbasedonparameteriza-tion[c]//proceedings.1991ieeeinternationalconferenceonroboticsandautomation.ieee,1991:458-465.)。tolani和badler通过指定肘部位置和固定肩或腕关节导出了封闭解(详见：real-timeinversekinematicsofthehumanarm[j].presence:tele-operators&virtualenvironments,1996,5(4):393-401.)。dahm和joublin认为，对于任何给定的位姿(位置和方向)，机器人的肘部都可以自由地沿着称为冗余圆的圆移动(详见：closedformsolutionfortheinversekinematicsofaredundantrobotarm[m].ruhr-univ.,inst.fürneuroinformatik,1997.)。moradi等人在dahm和joublin工作的基础上提出了一种基于闭合形式逆运动学的闭合形式解，并实现了机器人肘部运动的最小化(详见：jointlimitanalysisandelbowmovementminimizationforredundantmanipulatorsusingclosedformmethod[c]//internationalconferenceonintelligentcom-puting.springer,berlin,heidelberg,2005:423-432.)。针对含关节偏移的七自由度机械臂运动学逆解问题，sinha和chakraborty通过利用冗余机械臂几何形状，使用双参数搜索来计算多个逆运动学解(详见：geometricsearch-basedinversekinematicsof7-dofredundantmanipulatorwithmultiplejointoffsets[c]//2019internationalconferenceonroboticsandautomation(icra).ieee,2019:5592-5598.)。

然而，如何能够实时地寻找到封闭解中最优解是实现遥操作系统从端控制平稳和实时的关键。最常用的方法是传统的穷举搜索算法——网格搜索法，即通过遍历冗余参数(此处为冗余圆冗余角)来寻找出最适合的冗余参数。这种方法计算代价过大且粗糙，如果设定参数遍历间隔越小，迭代次数将越多越耗时，如果设定遍历间隔过大，则会忽略间隔间的潜在最优解。

正余弦算法(sca)是由mirjalili在2016年提出的一种新颖的元启发式算法(详见：sca:asinecosinealgorithmforsolvingoptimizationproblems[j].knowledge-basedsystems,2016,96:120-133.)。受数学技术的启发，sca根据正弦和余弦函数的数学模型对每个个体进行随机初始化，然后进行更新，也就是说，个体可以向外波动或向全局最优解波动来探索搜索空间。作为一种有竞争力的元启发算法，sca被广泛应用于许多邻域，如图像处理，特征选择，电力控制系统，多目标优化，神经网络，电力市场规划问题和太阳能装置等。然而，在最初的sca算法中，所有候选解都通过向迄今为止在整个群体中获得的全局最优候选解学习，这容易导致早熟收敛。

综上所述可知，如何从封闭式解析解中寻找最优解是实现遥操作系统从端控制平稳和实时的关键。传统的网格搜索法，通过设置间隔来形成搜索网格，寻找最优解。然而，网格的存在会遗漏搜索空间中的大部分潜在解从而导致精度不足，并且穷举的方式是耗时的。sca算法是一种高效的随机优化元启发算法，然而，原始的sca算法不能很好的平衡勘探与开发能力，因此存在收敛速度慢，容易陷入局部最优等问题。

技术实现要素：

本发明公开了一种基于res-sca算法的逆运动学实时解析解优化方法。针对逆运动学封闭式解析解实时寻优，提供了一种改进的正余弦算法：再平衡强化搜索sca(res-sca)算法。该算法通过结合再平衡策略(rs)和强化搜索策略(es)实现了平衡勘探与开发的同时，强化群体搜索能力，提高了收敛速度，确保了封闭式解析解寻优的快速和精确。

本发明是通过以下技术方案实现的。

步骤一、建立冗余圆冗余角与对应下一个机械臂末端位置ptool_next、姿态rtool_next逆运动学封闭式解析解θnext关系：

其中，θnext为k维关节角矢量，k为机械臂自由度，为机械臂逆运动学封闭解析解求解法的函数表达；

步骤二、建立解析解优化目标方程。获取当前机械臂所有关节角度θcur。以寻找最近邻关节角，即封闭解集中与上一个机械臂关节角的变化最小化作为目标，建立如下目标方程：

步骤三、初始化res-sca算法参数：最大迭代数t，种群大小n，候选解维度d，正余弦波动因子(λ1、λ2、λ3和λ4)，平衡因子候选解边界(ub、lb)以及常量ρ、

步骤四、按照候选解边界随机初始化n个候选解。对于第i个候选解xi，代表第i个可能的冗余圆冗余角其中i∈{1,2,…,n}；

步骤五、按照目标函数式(2)以及结合公式(1)求解n个候选解适应度值fitness(xi)，并记录当前迭代次数t时每个候选解的历史最优值以及全局历史最优候选解的值gbest^t；

步骤六、使用再平衡策略(rs)和强化搜索策略(es)更新每个候选解，具体表述如下：

(a)使用rs策略，对于每次迭代，如果当前迭代次数t小于或等于则res-sca按照如下公式更新候选解：

否则，res-sca按照如下公式更新候选解：

其中为使用rs策略更新后的候选解；

(b)并进一步使用es来产生最终更新：

其中，η＝t/t，n(0,1)和c(0,1)为高斯和柯西随机数；

步骤七、使用边界约束候选解的值。当个体更新后超出边界时按照下面公式对个体进行约束：

xi＝(xi<lb)×lb+(xi>ub)×ub(6)

步骤八、终止条件：当迭代过程满足终止条件时，也就是算法迭代次数满足条件t≥t，停止迭代并输出最终的最优解及其对应解析解θnext。否则，重复执行步骤五至步骤七。

本发明的优点：公开了一种针对逆运动学封闭式解析解实时优化方法。提供的res-sca算法很好的平衡了算法的勘探与开发能力，拥有收敛速度快，精度高等特点。基于res-sca算法的逆运动学解析解优化方法在提供高实时性的同时，确保了收敛的精确性，实现了机械臂逆运动学求解的精确、实时。

附图说明

图1为基于res-sca算法的逆运动学实时解析解优化原理；

图2为barrettwam七自由度机械臂简化几何结构；

图3为基于res-sca算法的解析解优化结果图；

图4为基于res-sca算法的解析解优化机械臂自运动图。

具体实施方式

本发明以barrettwam七自由度机械臂为实例平台作进一步说明。

步骤一、建立冗余圆冗余角与对应下一个机械臂末端位置ptool_next、姿态rtool_next逆运动学封闭式解析解θnext关系：

其中，θnext为七维关节角矢量，为机械臂逆运动学封闭解析解求解法的函数表达，以barrettwam机械臂为例，的具体内容如下：

(a)简化barrettwam的几何描述：barrettwam机械手臂是具有连杆偏移的七自由度冗余机械臂。其简化的几何结构由肩部关节s，带连杆偏移的肘部关节组合e1、e、e2，以及腕部关节w组成。两个连杆偏移结构e1e和ee2的设置将wam的肘关节分为e1、e、e2三个结构组合。在固定腕部关节的情况下，带有连杆偏移的上臂和前臂自由相交的e点分布集合依然构成了冗余圆。详细结构见图2。

(b)冗余圆求解：由于连杆偏移的存在，wam上臂和前臂在几何中的杆长发生了变化：

wam上臂及前臂和水平线l的夹角：

wam臂冗余圆dc和rc：

dc＝nl1×cos(α1)(12)

rc＝nl1×sin(α1)(13)

因此，有冗余圆：

其中为冗余圆冗余角，其取值范围为0～360°。在冗余圆上，每一个都对应了一组运动学逆解。按照腕部归一化旋转矩阵rwz旋转冗余圆，最终冗余圆为：

cr＝circle×rwz(15)

(c)关节角计算：机械臂运动学逆解是在给定位置矢量ptool＝[x,y,z]和旋转矩阵rtool＝[trx,try,trz]描述期望机械臂手部(或工具)位姿时，给出所需的关节角度θ＝[θ1,θ2,θ3,θ4,θ5,θ6,θ7]。前四个关节角，即θ1、θ2、θ3和θ4给定手腕位置，后三个θ5、θ6和θ7关节角定位手部(或工具)姿态。在冗余圆已知的前提下，要求得定位手腕位置的前四个关节角[θ1,θ2,θ3,θ4]，需要先求出连杆偏移后肘部e1、e2节点中的一个。以e1节点位置pe1为例：

ecr＝cr/||cr||2(16)

encw＝cr×pw/||cr×pw||2(17)

因为前两个关节指定了上臂从肩关节(基座)开始的方位角(θ1)和仰角(θ2)，所以可以通过定位的上臂pe1可以确定θ1和θ2：

第三个关节是上臂的扭转自由度

ne1z＝(pe1×ez)×pe1(21)

第四个关节对应于从肘部角度的前臂标高(θ4)，再加上前三个关节角，这四个关节角最终可以确定腕部的位置。wam运动学逆解问题的所有可能姿势都由冗余圆描述。

然而，由于连杆偏移的加入，冗余圆上的每个点都有两种可能的解决方案，被标记为肘外姿势和肘内姿势。此处仅考虑肘外姿势的情况，有：

第五和第六个关节提供手部工具的方位角(θ5)和仰角(θ6)。到目前为止，前四个关节角[θ1,θ2,θ3,θ4]是已知的。这些可以用来获得旋转矩阵如下所示：

由于定位腕部的前四个关节角已知，因此以腕部pw为基准构建局部手部末端坐标：

因此有：

第七个关节角θ7提供手部旋转。在已知前六个关节角，即[θ1,θ2,θ3,θ4,θ5,θ6]的情况下，可以参照式(24)直接获得腕部旋转矩阵进而可以获得手部旋转关节角：

θ7＝arccos(wrx·try)(28)

步骤二、建立解析解优化目标方程。获取当前机械臂所有关节角度θcur。以寻找最近邻关节角，即封闭解集中与上一个机械臂关节角的变化最小化作为目标，建立如下目标方程：

步骤三、初始化res-sca算法参数：最大迭代数t＝20，种群大小n＝10，候选解维度d＝1，正余弦波动因子(λ1＝1-0、λ2＝2π*rand、λ3＝2*rand和λ4＝1*rand，其中rand为[0,1]之间的随机数)，常量ρ＝1。其中，λ1表示为：

平衡因子常量δ＝0.1。对于候选解边界(ub、lb)，因最优解对应的运动学逆解为上一个逆解的最邻近解，因此最优解应围绕上一个最优参数前后20°(经过实验获得，近邻解存在于上一个最优解前后20°)范围展开搜索，因此有初始化边界：

步骤四、按照候选解边界随机初始化n个候选解。对于第i个候选解xi：

xi＝lb+rand*(ub-lb)(32)

其中，i∈{1,2,…,n}，rand为[0,1]之间的随机数。

步骤五、按照目标函数式(29)以及结合公式(7)求解n个候选解适应度值fitness(xi)，并记录当前迭代次数t时每个候选解的历史最优值以及全局历史最优候选解的值gbest^t。

步骤六、使用再平衡策略(rs)和强化搜索策略(es)更新每个候选解，具体表述如下：

(aa)使用rs策略，对于每次迭代，如果当前迭代次数t小于或等于则res-sca按照如下公式更新候选解：

否则，res-sca按照如下公式更新候选解：

其中为使用rs策略更新后的候选解；

(bb)并进一步使用es来产生最终更新：

其中，η＝t/t，n(0,1)和c(0,1)为高斯和柯西随机数。

步骤七、使用边界约束候选解的值。当个体更新后超出边界时按照下面公式对个体进行约束：

xi＝(xi<lb)×lb+(xi>ub)×ub(36)

步骤八、终止条件：当迭代过程满足终止条件时，也就是算法迭代次数满足条件t≥t，停止迭代并输出最终的最优解及其对应解析解θnext。否则，重复执行步骤五至步骤七。

在本实例中，以wam肩部为空间坐标原点，固定末端z轴方向，期望wam机械臂在垂直水平面且离机械臂y轴方向-0.7m空间墙壁上绘制圆心为(0m,-0.7m,0.1m)，半径为0.25m的圆。

在本实例中，基于res-sca算法的逆运动学实时解析解优化方法获得的结果如图3、4所示，其平滑稳定的完成了笛卡尔空间绘圆任务。为了更进一步体现本发明的优势，将本发明方法与传统的网格搜索法(gs)，sca算法及其变种cgsca算法进行了比较。表1结果显示，基于res-sca算法的逆运动学实时解析解优化方法提供了最快的求解速度。

表1平均求解并优化每个机械臂位姿耗时比较

以上所述仅表达了本发明的优选实施方式，其描述较为具体和详细，但并不能因此而理解为对本发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形、改进及替代，这些都属于本发明的保护范围。因此，本发明专利的保护范围应以所附权利要求为准。