随着仿人机器人进入家庭的需求增多，越来越多的研究开始关注如何控制仿人机器人完成一些实际的家务，如倒茶、提水、开门等[1]．在这些任务中，仿人机器人不仅会受到关节限位、碰撞等固有约束的影响，而且会受到保持特定位姿等任务约束的影响，同时还须要保持拟人构型以增强人机交互的舒适性．因此，研究机器人任务约束下的拟人运动规划问题对推动仿人机器人进入家庭具有重要意义．现有方法将仿人机械臂任务约束下的路径规划问题与运动构型拟人化问题单独考虑．基于随机采样的规划算法适用于仿人机械臂高维运动规划，但是任务约束的存在导致随机采样算法的采样点位于约束流形中的概率趋于零[2]．随后，松弛、投影、切空间[3]和地图集[4]等方法被用来与基于随机采样的算法相结合，处理约束路径规划问题．基于松弛的方法通过增加约束函数的允许公差来放松约束流形的表面[5]；基于投影的方法使用投影算子将空间样本点映射到约束流形上[6]；基于重参数化的方法重新参数化机器人构型空间，直接采样满足约束条件的构型，然后映射回机器人原本构型空间[7]．上述约束满足的方法须要构建约束流形的近似空间或移动构型空间的点到约束流形上，其计算复杂、规划效率低，而且没有考虑仿人机械臂运动拟人问题．针对运动构型拟人的问题，研究人员通常最小化一个成本指标，将其引入运动学模型来生成拟人构型，包括肌肉力量最小指标[8]、势能最小指标[9]和快速上肢评估准则[10]等，但是这种基于指标的方法通常伴随着运动生成的假设，可能会限制运动的可变性，且这类方法通常计算复杂．近年来，国内外学者通过学习人臂运动然后映射至仿人机械臂．文献[11]将从人类演示中获得的轨迹转换为空间上的引力场来实现拟人运动．文献[12]采用多元线性回归学习人体手臂内外坐标之间的关系，实现可达运动中的拟人臂构型．另外，人工神经网络[13]与最小二乘法[14]也被应用于生成拟人臂构型，但是当机械臂模型或环境发生改变时，须要重新对数据进行采集和训练．之前的研究考虑了人臂长度差异，使用高斯过程学习的方法得到末端手部位姿与拟人臂构型间的关系，提高了模型适用性[15]．本研究提出一种任务约束下的仿人机械臂拟人运动规划方法，能够同时解决仿人机械臂的任务约束运动规划和运动拟人化问题；使用人臂高斯过程回归模型，解决冗余机械臂拟人运动规划问题，实现随机采样规划算法直接在任务空间进行采样，避免采用任务约束满足的方法，提高了算法规划效率；对自运动流形进行完整描述，保证规划算法的概率完备性．1 算法描述由于七自由度仿人机械臂的冗余特性，即末端任务空间的一个位姿点对应关节空间无数个构型，因此运动规划算法多在机械臂关节空间进行规划．在末端执行器存在任务约束的情况下，关节空间采样点须要通过任务约束满足的方法避免采样点位于约束流形上概率趋于零的问题，且约束越多，流形越复杂，带来的额外计算也越大．任务约束下基于高斯过程的仿人机械臂运动规划方法采用高斯过程回归的方法学习人臂运动，并建立任务空间到关节空间的映射关系，实现在仿人机械臂任务空间中直接采样．规划空间(6-k维)的复杂程度随着任务约束的增加而降低，其中k为末端任务约束的维度．由于人臂模型与仿人机械臂模型关节限位的不同及障碍物的存在，人臂预测模型得到的拟人臂构型不一定满足仿人机械臂固有约束，为保证算法的概率完备性，提高规划成功率，本方法完整描述了不满足固有约束采样点的自运动流形，并在该自运动流形中求解次拟人臂构型．本研究所述算法以机械臂初始构型qinit、目标位姿xgoal及任务约束集F为输入，生成一条机械臂拟人运动轨迹T．具体步骤如下．步骤 1 调用正运动学计算初始构型qinit的初始末端位姿xinit，初始化生长树T，其中包括树的采样点集合V，连接采样点边的集合E(初始为ϕ)及采样点的拟人臂构型集合Q，以(xinit,ϕ,qinit)为生长树的根节点．步骤 2 在6-k维无碰撞任务空间χfree中产生随机采样点xrand，找到T上距离采样点最近的生长点xnearest，并向xrand扩展一个步长Ssize，得到生长点xnew．步骤 3 将xnew输入基于高斯过程回归的预测模型，输出最优臂角参数φopt，由基于臂角的逆运动学得到拟人臂构型qopt，判断qopt是否满足关节限位与避碰撞的固有约束，若满足则将xnew和qopt添加到生长树T，并执行步骤6，否则执行步骤4．步骤 4 拟人臂构型qopt不满足固有约束，完整描述xnew固有约束下的自运动流形ψf，若ψf为空，则xnew无效，返回步骤2，否则执行步骤5．步骤 5 基于最小欧几里得距离计算自运动流形ψf中的次拟人臂构型qsubopt，并将xnew和qsubopt添加到生长树T．步骤 6 以xnew为圆心、r为半径，搜索所有近邻节点Xnear，分别计算每个近邻节点xnear的代价完成重连接，判断是否到达目标点xgoal，若到达则执行步骤7，否则执行步骤2，进入下一个迭代．步骤 7 从目标点xgoal回溯到初始点xinit，返回生长树T．2 基于高斯过程的拟人臂构型模型2.1　人臂运动数据采集如图1所示，采用红外运动捕捉仪采集20名受试人员(身高在150～195 cm之间，体重在39～95 kg之间)分别到达40个目标位姿的运动数据，将Mark标记球分别固定在受试人员肩、肘、腕与手掌中心获得各关节的位置(x,y,z)、姿态(α,β,γ)及上下臂的长度(d1,d2)．人臂末端目标位姿的采集通过定点放置云台，调整手柄刻度实现．10.13245/j.hust.230510.F001图1使用Vicon红外运动捕捉系统采集人臂运动数据随后计算800组数据对应臂角值，分析具有不同上下臂长度的受试人员数据，并从上述800组数据中选取600组作为训练输入集M，相应训练输出集ψ，建立模型训练集为D=(M,ψ)，剩余200组作为测试输入集M*，相应测试输出集ψ*，建立模型测试集为D*=(M*,ψ*)．2.2　模型训练如图2所示，简化人臂为S-R-S型7自由度运动学模型，令肩关节S、肘关节E和腕关节W三点确定的平面为臂平面，特殊情况当3关节为0°时的臂平面为参考平面，设E0表示参考平面的肘关节，qi为第i个关节．10.13245/j.hust.230510.F002图2人臂简化模型示意图采用臂角参数φ(臂平面与参考平面之间的夹角)来描述人臂及仿人机械臂的冗余特性[16]，φ的计算式为φ=sgn(φ)arccos(e×w)(e0×w0)e×we0×w0；sgn(φ)=-sgn(e0×w0⋅e)，式中：e为臂平面中肩关节到肘关节的向量；w为臂平面中肘关节到腕关节的向量；e0为参考平面中从肩关节到肘关节的向量；w0为参考平面中肘关节到腕关节的向量．拟人臂构型预测模型为φ=g(m)+ε，式中：m=[x,y,z,α,β,λ,d1,d2]T为输入向量；g为m到相应臂角参数φ的映射函数；ε~N(0,σn2)为红外运动捕捉系统的噪声．在模型训练阶段，选择各向异性马特恩(Matern)协方差函数、各向异性平方指数(SE)协方差函数等组成的复合协方差函数对人臂运动拟人臂构型进行建模，并采用最大似然估计法学习最优超参数．这样，当给定人臂末端位姿与臂角φ时，可由基于臂角的逆运动学得到拟人臂构型qopt．3 自运动流形完整描述及次拟人臂构型选取在一个特定的末端位姿下，仿人机械臂可以围绕连接肩关节与腕关节的轴进行自运动，其受到关节限位及碰撞的影响．当拟人臂构型qopt不满足固有约束时，基于零空间完整描述其自运动流形选取次拟人臂构型qsubopt．3.1　避关节限位下的自运动流形通过直接求解关节限位不等式与臂角参数得到关节限位下的自运动流形．关节2和6与臂角关系的运动学方程一般用余弦函数表示，即cosθi=asinφ+bcosφ+c    (i=2,6)，式中：a，b和c分别为运动学方程的相应因子；θi为i关节角度．因此，在[-π,π]区域内满足两个关节限位自运动流形的计算方法为：cosθil≤asinΨi-+bcosΨi-+c≤1;cosθiu≤asinΨi++bcosΨi++c≤1,式中Ψi-和Ψi+分别为关节2与关节6在θi∈θil,0和θi∈0,θiu两个区域对应的自运动流形，其中θiu和θil分别为关节的上、下限位．其他关节的运动学方程一般可以表示为tanθj=sgn(sinθi)(ansinΨj+bncosΨj+cn)sgn(sinθi)(adsinΨj+bdcosΨj+cd)(i=2,6;j=1,3,5,7),式中：an，bn，cn，ad，bd和cd分别为运动学方程的相应因子；Ψj为对应关节1，3，5，7的自运动流形．可以看出：构造这些关节限位与臂角参数之间的不等式受关节2和关节6的符号的影响．因此，根据关节2和关节6的正负，将自运动流形分为4个子流形ΨrkL(k=1,2,3,4)，在每个子流形中，臂角与逆运动学解之间的关系为一一对应，即Ψr1L包括θ2和θ6小于0的所有解，Ψr2L包括θ2小于0和θ6大于0的所有解，Ψr3L包括θ2和θ6大于0的所有解，Ψr4L包括θ2大于0和θ6小于0的所有解，则仿人机械臂在关节限位下的自运动流形描述为ΨL=∪k=14ΨrkL．3.2　避碰撞下的自运动流形首先采用球扫线建立机械臂的碰撞检测模型，对于机器人本体和障碍物的其余部分，采用球扫掠凸体构建其碰撞检测模型；然后通过计算机械臂连杆球扫线的扫掠体得到关节限位下自运动形成的近似扫掠体，并将非凸扫掠体近似为凸扫掠体，机械臂肘部轨迹包含在两个球扫掠凸体中．在自运动流形的扫掠体与其他碰撞检测模型之间构造碰撞检测对，利用GJK(Gilbert-Johnson-Keerthi)算法计算每个碰撞检测对的最近距离，当欧几里得距离小于零时，碰撞校验对发生碰撞．一旦获得这些不可行的自运动流形ΨrkC，就将其从自运动流形中剔除，最后得到避关节限位与避碰撞自运动流形为Ψf=∪k=14(ΨrkL-ΨrkC)．3.3　基于零空间的次拟人臂构型选取求解次拟人臂构型是在完整描述的末端点自运动流形上，求解距离拟人臂构型qopt∉Ψf最近的臂构型，满足方程φsubopt=min{|φ-φopt|}    (φ∈Ψf)．如图3所示，彩环表示拟人臂构型不满足固有约束的仿人机械臂零空间Ψf (假设拥有4个臂角区域Ψr1，Ψr2，Ψr3和Ψr4)，圆环白色部分表示不满足关节限位的臂角范围，灰色部分表示发生碰撞的臂角范围．例如当最优拟人臂构型qopt位于不满足关节限位和避碰撞的臂角范围内时，选取靠近最优臂角值且位于有效零空间内的次优解为qsubopt．10.13245/j.hust.230510.F003图3自运动流形中的次拟人臂构型选取4 实验验证为验证本研究所述方法的有效性与优越性，进行如下三个实验：a．通过与基于最小二乘法[14]及神经网络[13]的模型进行对比，验证高斯过程拟人臂构型预测模型的有效性；b．搭建仿真环境，与RRT*及基于投影的方法进行对比，验证本方法的优越性；c．搭建仿人双臂机器人实验平台，对其进行避障运动规划，验证本方法的有效性．4.1　拟人臂构型预测模型有效性分析及对比实验将测试集D*=(M*,ψ*)输入基于高斯过程回归的臂构型预测模型，结果与真实臂角对比如图4所示．可以看出：使用高斯过程预测的臂角与真实臂角值比较接近，预测模型有效．10.13245/j.hust.230510.F004图4高斯过程预测模型的预测结果与真实值对比使用绝对平均误差(MAE)与均方根误差(RMSE)作为评价指标分别对基于高斯过程、最小二乘法及神经网络的模型进行评估，结果如表1所示．可以看出：高斯过程预测臂角的MAE与RMSE值远小于另外两种模型．10.13245/j.hust.230510.T001表1不同模型的预测精度对比模型MAERMSE高斯过程0.054 10.004 3最小二乘法0.145 20.026 3神经网络0.096 40.017 64.2　任务约束下虚拟双臂机器人运动规划及对比分析计算机配置为Intel i5 10400F，6核心12线程，主频2.9 GHz，16 GiB内存．在Windows10系统下使用Visual Studio 2017与OpenInventor搭建可视化仿真环境，建立虚拟仿人双臂机器人扫掠体碰撞模型如图5所示，本研究主要对其右臂进行规划研究．10.13245/j.hust.230510.F005图5虚拟双臂机器人在任务约束下的末端规划为验证本方法的有效性，约束机械臂末端的运动姿态，模拟人在端水杯的情况下避开障碍物时的运动过程．将算法采样数设定为1 500，在满足约束的机械臂末端位姿空间中为其规划一条无碰撞的渐进最优全局路径，初始位姿与目标位姿如图5所示，绿色线条即为最优路径．在运动过程中，虚拟右臂的每一个末端位姿对应一个拟人臂构型或次拟人臂构型，在实现避障的同时始终保持末端夹爪处于水平姿态，如图6所示．10.13245/j.hust.230510.F006图6任务约束下虚拟双臂机器人运动规划在相同场景中将本算法与关节空间的RRT*算法及基于投影的方法进行对比实验．为避免随机性，设定采样数为1 500，分别运行20次，并记录规划成功的次数、所花费的总时间及路径的长度．所有成功运行的数据平均如表2所示，花费时间分别降低39%与68%，路径长度分别减少55%与38%．10.13245/j.hust.230510.T002表2仿人机械臂在两个场景中的规划结果方法空间维度平均时间/s平均路径/m是否满足约束是否拟人规划成功率RRT*χ715.388 64.181 4否否20/20基于投影方法χ729.468 53.048 9是否19/20本文方法χ49.286 31.873 8是是20/20由于协作机械臂有7个自由度，传统RRT*与基于投影等方法的规划空间均为χ7关节空间，而本方法随着位姿约束的增加，空间维度随之降低，在上述场景中规划空间为χ4=T3×R1(3维平移和1维旋转)任务空间，因此速度更快，且路径更短．4.3　仿人机械臂拟人运动规划搭建仿人双臂机器人实验平台如图7所示，其在4.2节的任务场景中运动过程与人臂实际端水杯运动过程表明机械臂的运动符合人臂运动机理．10.13245/j.hust.230510.F007图7任务约束下仿人双臂机器人与实际人臂运动对比机器人运动过程中末端路径如图5所示，末端姿态变化与臂角参数变化如图8所示，可知机械臂在χ4任务空间中规划得到的路径能够避开障碍物并始终满足末端姿态的约束(见图8(a))，运动过程中只有不影响水杯倾斜的姿态α发生了改变．10.13245/j.hust.230510.F008图8运动中姿态与臂角参数变化5 结语针对现有基于随机采样的运动规划算法在任务约束下计算量大、规划时间长且不考虑拟人的问题，提出一种基于高斯过程的七自由度仿人机械臂运动规划方法．实验结果表明：本方法在任务约束下的规划效率更高，与RRT*算法及基于投影的方法相比，本方法规划的路径长度分别减少了55%与38%，速度分别增加了39%与68%，且运动过程更加拟人．