相比于传统六自由度机械臂，冗余机械臂具有更好的灵活性，使其在满足目标任务的约束之外还能够满足其他约束条件，如避关节极限[1]、避障[2]和运动拟人化[3]等，但同时也带来了逆运动学更为复杂的问题[4]．逆运动学是机械臂运动规划的基础，目前主要的逆运动学求解方法包括数值解法和解析解法[5]．数值解法主要包括结合雅可比矩阵求伪逆的迭代求解[6]和二次规划迭代求解[7]，其中运动学与动力学库KDL[8]、改进的数值解法Trac-IK[9]都是常用的数值解方法．数值解法通用性好，适合各种机械臂构型，但是存在求解效率较低、精度差及只能得到单组解等问题[10]．解析解法通过推导机械臂关节的位置关系得到关节空间和任务空间的映射关系，求解效率高且精度较高，能得到所有的解，便于后续选择与优化．冗余机械臂的逆运动学解析解将六自由度任务空间映射到七自由度关节空间，映射关系往往是一对多，所得逆解为无穷组，构成了零空间[11-12]．在实际应用中，须要通过参数化变量来消除冗余自由度．优化参数化变量值从而得到唯一逆解成为了研究的重点课题．根据参数化方法不同，解析解又分为臂形角参数化法和关节角参数化法[13]，其中臂形角[14]参数化法由于几何意义明显，因此研究与应用较多．为了优化臂形角参数，一些学者考虑了避关节极限的约束条件[15]．针对臂形角退化问题，一种双臂形角的冗余控制方法被提出[16]，但不适用于连续轨迹跟踪．还有学者综合考虑了避奇异点、避关节极限等约束，提出了臂形角优化的逆解方法[17]．此外，还有考虑关节速度与加速度的实时多目标优化逆解方法[12]与避关节极限和能耗最优的逆解优化方法[10]．臂形角参数法便于臂形控制，适用于仿人构型研究等，但通用性差、计算复杂，只适用于典型腕部和肩部无偏置的机械臂构型．关节角参数化法以冗余关节作为参数化关节，再求解非冗余关节，推导过程简单直观，适用多种构型，实用性强．传统的关节角参数化未对参数关节值进行选择与优化[18]，因此许多优化策略被提出，例如：以减少最大关节位移的优化目标确定参数化关节的策略[5]、针对带有移动关节的机械臂以最小化位姿误差为优化目标的策略[19]、针对结构偏置的冗余机械臂，优化关节限位和关节扭矩的策略[20]，以及针对八自由度机械臂，考虑运动连续性与避关节极限的多目标优化策略[21]，但该策略求解效率较低，不适合七自由度机械臂的实时求解．简单直观的关节角参数化方法能快速实现工程应用，缩短开发周期，但目前缺乏在此基础上综合考虑不同工作状态下的多目标约束的逆运动学求解与优化研究．基于此，本研究通过关节角参数化法建立冗余机械臂解析解，综合考虑避关节极限和运动连续性约束建立优化目标，通过动态调整目标权重在不同的工作状态下确定最优关节角参数，从而得到最优逆解，实验结果证明了本方法的有效性与实用性．1 冗余机械臂正运动学冗余机械臂的经典结构为“肩-肘-腕”的拟人臂结构[22]，前三个关节形成球关节，对应肩部，第四关节对应肘部，后三个关节形成球关节，对应腕部，整体构成了球副-旋转副-球副(S-R-S)构型．用标准DH(Denavit-Hartenberg)法建立冗余机械臂的连杆坐标系如图1所示，图中：dbs为基坐标系与肩关节坐标系的距离；dse为肩关节与肘关节的距离；dew为肘关节到腕关节的距离；dwf为腕关节到工具坐标系的距离．10.13245/j.hust.240371.F001图1冗余机械臂连杆坐标系连杆坐标系的DH参数如表1所示，表中：ai为连杆i的偏置距离；αi为连杆i的偏角；θi为连杆i的关节角度；di为连杆之间的距离．根据连杆坐标系可以建立相邻连杆的坐标变换关系，即        ii-1T=cos θi-sin θicos αisin θisin αiaicos θisin θicos θicos αi-cos θisin αiaisin θi0sin αicos αidi0001．(1)10.13245/j.hust.240371.T001表1连杆坐标系DH参数表连杆iaiαidiθi10-π/2dbsθ120-π/20θ230π/2dseθ340-π/20θ450π/2dewθ560-π/20θ6700dwfθ7由式(1)可以得到正运动学方程为70T=T10T21T32T43T54T65T76．(2)2 冗余机械臂逆运动学2.1　关节角参数化关节角参数化法用以求解冗余机械臂的逆运动学．使用参数化描述七自由度机械臂的冗余性，关节角参数化的个数即为冗余自由度的个数，将关节角参数化后，参数化关节θp即被确定为关节限位中的某确定值，其他非参数化关节θnp可利用逆运动学求解得到θnp=ik(T70,θp)．(3)2.2　逆运动学解析方程本研究以运动特征明显的θ1为参数化关节，求解冗余机械臂的逆运动学．S-R-S构型的冗余机械臂可简化为肩(S)-肘(E)-腕(W)三个关节[23]，如图2所示，S-E-W构成了三角形几何关系．10.13245/j.hust.240371.F002图2S-R-S冗余机械臂构型对于给定70T都可以确定θ4为θ4=π±arccosd32+d52-|sw|22d3d5，式中sw为由S指向W的向量．将ijT中1~3行的4个列向量分别替换为ijx,yij,zij,pij，令ijR=[xij,yij,zij]，则由式(1)得到40T=T10T21T32T43=40x40y40z40p0001．(4)结合式(4)及给定的70T，可以得到47T=07Rx4007Ry4007Rz4007Rp40+p070001．(5)将冗余机械臂分解为肘-腕部分和肩-肘部分，由肘-腕的几何关系，结合式(5)整理可以得到40p+d5z40=-R70([00d7]+p07)．(6)式(6)左边为关于关节角变量θ1，θ2，θ3和θ4的三角函数关系，已知θ4，给定参数化关节角θ1的值，即可求解θ3和θ2．在得到θ1，θ2，θ3和θ4之后，式(5)各量为已知量，由式(1)和式(2)可得47T=f(θ5,θ6,θ7)，联立式(5)即可求解得到θ5,θ6和θ7．由运动学逆解方程推导可知：θ4的求解与其他角度无关，θ1，θ2和θ3只须要知道其中一个即可求解其他两个，θ5，θ6和θ7可以依赖前项条件求出．本研究以θ1为参数化关节，确定θ1后可解出其他关节变量，因此参数化关节不同的取值决定了机械臂运动状态．3 优化目标与约束条件3.1　约束条件在实际的作业任务中，冗余机械臂的冗余特性使得机械臂在实现作业任务要求的目标约束以外还能够满足各种冗余约束条件．通常，机械臂末端位置和姿态约束为目标约束，机械臂各个关节满足避开关节极限约束和保证运动的连续性为冗余约束，其中避关节极限又可分为两个方面：一是严格避开关节上下限以免造成硬件损伤；二是尽量远离关节上下限位置以免产生奇异构型．运动连续性包括笛卡尔空间的运动连续和关节空间的运动连续，关节空间的运动连续性可认为是前后两次运动间隔中各个关节角度变化应尽量小．机械臂最主要的约束是目标位姿约束，虽然解析算法计算得到的逆解理论上会严格符合目标位置和姿态，但是由于各种精度问题存在位姿误差，因此提出位置和姿态约束，即ΔP≤εp；ΔR≤εr，式中：ΔP为目标位置向量与逆解结果再正解得到的位置向量的差值；ΔR为姿态向量的差值；εp和εr为允许的最大误差．关节极限约束分为硬约束和软约束．硬约束是指关节取值严格不得超过对应关节上下限范围，有θimin≤θi≤θimax  (i∈1,2,…,n)．软约束是指尽量远离关节上下限位置以免产生奇异构型，通过最小化目标函数值来保证逆解结果尽量远离关节极限值，由于各个关节限位不同，因此须要将关节值映射为统一的衡量区间，构造目标函数为fi(θ)=1-(θimax-θi)(θi-θimin)(θimax-θimin)2/4，式中fiθ为θi偏离中心位置的程度，其值越大，表示关节i越靠近限位．显然对于任意关节，当取值为θimax或θimin时，fiθ值为1，当取值为(θimax+θimin)/2时，fiθ取得最小值0，因此该目标函数将关节限位映射为[0，1]．关节运动连续性约束为冗余约束条件，使得机械臂在满足基本作业目标以外能够最小化机械臂运动时所消耗的能量．通过构造目标函数使得当前构型接近于上次构型，从而将整体关节角度变化最小化，构造目标函数为g(θ)=1-e-|kΔθ|2，式中：Δθ为关节向量的变化量；k为权重向量，其综合考虑了不同关节运动时消耗的能量不同；g(θ)取值为[0，1)，关节角度变化越大其值越大．3.2　优化目标结合以上约束与目标函数，为综合考虑关节限位与运动连续性，使得机械臂满足运动条件，提出总目标函数，以避开关节极限目标函数与运动连续性目标函数的线性组合的方式产生，即Fθ=w11n∑infi(θ)+w2g(θ)，式中w1和w2分别为避关节极限目标和运动连续性目标的权重．根据任务过程中两目标的重要程度的不同，w1和w2也有所不同，当任务中距离关节极限较远时，应当着重满足运动连续性目标；当任务过程中离关节极限较近时，应当着重满足避关节极限目标，因此须要动态调整权重．将关节限位映射为[0，1]区间后，可以认为：当fi(θ)∈[0,0.8)时，关节i处于合理运动区间，此时应当优先满足运动连续性目标；当fi(θ)∈[0.8,1]时，关节i处于危险运动区间，此时应当优先满足避关节极限目标，由此确定动态调整权重的条件．至此，逆运动学优化问题可通过选取合适的参数化关节变量值得出对应的逆解，从而最小化目标函数F(θ)．3.3　优化目标求解整体算法流程图如图3所示．10.13245/j.hust.240371.F003图3算法流程图本研究使用带惯性权重的粒子群算法在参数化关节的限位范围内选取最优关节角与对应逆解，从而最小化上述目标函数，第i个粒子在d步的位置和速度迭代过程为xi(d)=xi(d-1)+vi(d-1)t；vi(d)=wvi(d-1)+c1r[bi(d)-xi(d)]+c2r[gb(d)-xi(d)],式中：i=1，2，…，n，其中n为粒子个数；c1和c2为学习因子，分别表示粒子对自身认知和社会全局认知的学习权重，是调整局部最优解和全局最优解的重要参数；r为[0，1]间均匀分布的随机数；bi为粒子i当步最优值；gb为所有粒子的最优值；w为惯性权重，用来描述粒子速度惯性的影响，影响着算法的收敛状况，可表示为w=wmax-dwmax/dmax，其中，wmax为最大权重系数，dmax为最大迭代次数，d为当前迭代次数．随着迭代次数的增加，惯性权重系数会逐渐衰减至0，意味着速度惯性的影响逐渐变小．4 实验验证本研究使用自研S-R-S构型的七自由度冗余机械臂进行实验验证，分别针对解析逆运动学求解、单一末端位姿逆解优化和连续轨迹跟踪逆解优化效果进行对比验证．冗余机械臂的结构如图1所示，连杆参数为：dbs=-0.09 m，dse=0.247 m，dew=0.211 m，dwf=0.210 m．逆解优化算法使用C++编程，运行于外置计算机(Intel i7-11700，2.50 GHz)．通过机器人操作系统ROS将计算结果发送到机械臂控制系统，控制系统通信方式为CAN通信，频率为100 Hz．4.1　逆运动学求解验证为验证关节角参数化的逆运动学解析解的完备性，在空间中选取一个位姿为P=[0.300，-0.125，-0.500]；R=[0.707，0.000，0.707，0.000]，式中：P为位置坐标；R为四元数形式的姿态．使用2.2节中的逆运动学算法求解，选取参数化关节角度为θ1=-0.58 rad，不考虑关节限位，求解得到八组关节角度如表2所示．由结果可知：根据给定的末端位姿，所提出的逆运动学解法可以得到所有的八组逆解结果．10.13245/j.hust.240371.T002表2关节角度逆解结果序号θ1θ2θ3θ4θ5θ6θ71-0.58-0.090.49-0.591.951.23-2.542-0.58-0.582.65-0.59-0.151.68-2.893-0.58-0.58-0.490.592.991.68-2.894-0.58-0.09-2.650.59-1.191.23-2.545-0.58-0.090.49-0.59-1.19-1.230.606-0.58-0.582.65-0.592.99-1.680.257-0.58-0.58-0.490.59-0.15-1.680.258-0.58-0.09-2.650.591.95-1.230.60rad为进一步验证逆运动学有效性，采用正反解交叉验证求解的精度，任意给定工作空间中的目标位姿，通过逆运动学求解得到八组解，然后再代入正解求得位姿，将该位姿的位置量与目标位姿位置量在笛卡尔空间求欧氏距离差值，姿态量的处理是通过旋转矩阵转化为轴角法的角度差值．随机生成300个工作空间的位姿，统计位置绝对误差与姿态绝对误差如图4所示，从结果可以看出：位置绝对误差和姿态绝对误差分别在10 nm和1×10-7 rad以内，充分说明了逆运动学求解的有效性．10.13245/j.hust.240371.F004图4位置和姿态误差曲线逆运动学是后续轨迹规划的基础，其求解的时间性能决定了能否进行实时轨迹规划，因此须要衡量逆运动学求解的效率，本研究将提出的逆运动学求解与几种经典的逆解算法进行时间效率对比，包括通用的运动学与动力学库KDL[8]、改进的数值解法Trac-IK[9]和通用解析求解IKFast[24]，分别在工作空间随机求1×104次逆运动学，统计其时间消耗的均值，最大值和标准差如表3所示．10.13245/j.hust.240371.T003表3不同逆解时间对比逆解方法均值最大值标准差KDL116.3481169.52Trac-IK433.005 117796.09IKFast2.22651.26本研究IK1.19250.89μs从统计结果可以看出：本研究提出的逆解算法耗时以两个数量级的优势远胜于KDL和Trac-IK解法，即使和同为解析解的IKFast解法相比，效率也提高了46.4%；另外，从标准差也可以看到求解效率的稳定性也是最好的，且远优于数值解法，这在实时控制过程中有着更优胜的稳定性能．4.2　单一末端位姿逆解优化验证为验证加入目标优化后的逆运动学求解与优化效果，设计了逆运动学优化实验，机械臂关节1到关节7的限位分别为±1.57，±1.04，±2.09，±1.57，±2.09，±1.57，±2.09 rad．保持与上节相同的末端位姿为初始位姿，初始各关节角度为θt0=[1.50,0.22,1.81,-0.54,-0.36,-1.08,0.46]．结合关节限位与3.2节的关节变量映射区间，当前的f1(θ)=0.91，θ1处于危险运动区域，优先执行避关节极限目标，优化关节角度尽量避免这种关节位置．设置关节权重向量为k=[0.3,0.2,0.1,0.1,0.1,0.1,0.1]．通过粒子群算法依据总优化目标函数寻求最优关节角参数值，粒子群算法的参数设置为：粒子个数n=15；最大迭代次数dmax=20；学习因子c1和c2均为1.65；最大权重系数wmax=0.002 5．终点的姿态与初始姿态相同，位置沿z轴正方向增加0.01 m，求解得到的最优关节角参数值为θ1=1.18 rad，此时对应的最优逆解为θbt1=[1.18,0.32,1.23,-0.72,-0.04,-0.97,0.32]．针对上述相同条件，KDL求解的逆解为θkt1=[1.24,0.17,1.70,-0.68,-0.53,-1.02,0.53]．可以看出：机械臂关节1角度值通过目标优化能够远离危险运动区，且相比于KDL算法求解结果的目标函数值F(θkt1)=0.25，本研究逆解优化求解结果的函数值F(θbt1)=0.21，整体的优化效果更好．初始构型、KDL求解的逆解构型和本研究逆解优化算法求解的构型如图5所示．10.13245/j.hust.240371.F005图5机械臂构型对比4.3　连续路径跟踪逆解优化验证为了进一步验证逆运动学优化在连续路径跟踪中的效果，设计了直线路径跟踪实验与圆弧路径跟踪实验．两种路径的初始位姿与上两节的相同，在直线路径中，终点姿态相对初始姿态保持不变，终点位置在初始位置的y轴正方向偏移0.25 m．圆弧路径中，初始点为左端点，半径为0.08 m．在两种路径中均匀插入100个路径点，分别利用本研究提出的逆解优化方法求解和KDL求解得到的关节角度驱动机械臂运动，记录关节角度变化分别如图6和图7所示，记录末端实际运动轨迹如图8和图9的绿色轨迹所示．10.13245/j.hust.240371.F006图6直线路径的关节角度变化10.13245/j.hust.240371.F007图7圆弧路径的关节角度变化10.13245/j.hust.240371.F008图8直线路径的末端实际运动轨迹10.13245/j.hust.240371.F009图9圆弧路径的末端实际运动轨迹从图6和图7中可以看出：本研究提出的逆解优化方法与KDL相比，各关节远离关节极限，整体的关节运动连续性更平滑．在直线路径中，逆解优化得到的关节1和3相对于KDL更偏离极限位置，且整个路径跟踪过程中大大减少了关节3运动量．在圆弧路径中，逆解优化得到的关节3的避关节极限效果更加明显，对比KDL，关节1和3的运动量都得以减少．由图8和图9的末端实际运动轨迹可知：本研究提出的逆解优化方法得到的末端运动轨迹相比于KDL更平滑，KDL得到的轨迹前端存在突变抖动的情况．在直线和圆弧的路径跟踪过程中，本研究逆解优化方法的平均目标函数值分别为0.103和0.131，对比KDL的平均目标函数值0.326与0.312，分别提高了68.4%和58.0%的优化效果，说明本研究逆解优化的有效性，且在真实机器人上的应用也满足了实时性要求．5 结语本研究提出一种基于关节角参数化的冗余机械臂逆运动学求解优化方法．通过关节角参数化法建立冗余机械臂的解析解，在参数化关节值确定的情况下能求解得到所有关节角度值．结合实际作业过程中须要满足的冗余约束条件，提出了避关节极限目标与运动连续性目标，在不同的工作状态下动态调整两目标的权重．通过改进粒子群优化算法求解总目标函数，从而快速得到满足工作需求的最优逆解．实验结果表明：本研究提出的逆运动学求解与优化方法绝对定位精度高，误差在10 nm以内，时间效率满足实时规划的要求，能有效避开关节极限并显著提高运动连续性，在实际冗余机械臂运动规划中有较强的应用价值．进一步的研究可考虑避障、避关节奇异等其他冗余条件，以及在带偏置的机械臂上的应用．