7 自由度仿人机械臂工作空间求解的降密蒙特卡洛法

窦汝桐 ，于慎波 ，孙 凤 ，夏鹏澎 ，横井浩史 ，姜银来

（1.沈阳工业大学机械工程学院，辽宁 沈阳 110870；2.电气通信大学机械与智能系统工程学院，东京 182-8585）

随着老龄化社会的加剧，仿人机械臂的发展可以弥补年轻劳动力的短缺，解决老龄化社会带来的家庭服务和医疗陪护等社会问题[1-2].

工作空间作为机械臂运动学分析中必不可少的环节，其大小代表了机械臂的工作范围，是衡量机械臂工作性能的重要指标[3].Yin 等[4]采用工作空间描述的方式，筛选出液压挖掘机反铲臂的最优设计方案.东辉等[5]利用提出的工作空间密度函数，通过迭代的方式求解平面冗余机器人的逆运动学解.Xu等[6]提出一种5 自由度（DOF）机械臂，并依据工作空间为机械臂设置轨迹规划任务.赵智远等[7]基于工作空间分析，为9-DOF 超冗余机械臂确定出36 种构型方案.因此，机械臂工作空间的研究对机械臂设计与任务规划具有重要意义.

目前，工作空间的求解分为几何法、解析法和数值法：几何法可以获得工作空间的剖截面或剖截线，具有简单、直观的特点，适用于平面机械臂工作空间的求解，但对多自由度机械臂工作空间的描述不够准确[8]；解析法是基于雅克比矩阵奇异性与工作空间边界之间的关系，获得工作空间边界曲面的方法，可以准确地计算出工作空间的边界及其表达式，但求解过程十分复杂，一般适用于关节数少于3 的机械臂[9]；数值法则是基于概率统计学，采用随机抽取尽可能多的关节变量组合，获得机械臂末端位置的点集，进而近似代替机械臂的工作空间[10].

蒙特卡洛法作为工作空间求解的一种数值方法，具有算法简单和通用性强等优点[11-12].蒙特卡洛法在计算机械臂工作空间时存在随机点分布不均的缺点[13-14]，尤其是机械臂工作空间的边界位置，使得机械臂工作空间的边界位置模糊，造成机械臂工作空间的缺失.为提高机械臂工作空间的精度，需要大量新增随机点云去充实机械臂工作空间的边界，但新增的随机点多数落入了工作空间的内部，造成了新增随机点云的大量浪费[15]，并对机械臂工作空间的精度提升并不明显.针对蒙特卡洛法的不足，一种改进蒙特卡洛法被提出[16-17]，该方法利用蒙特卡洛法生成一个种子空间，在工作空间内点云数量较少的区域内部随机选取一个参考点，结合正态分布方法扩展并加密该参考点处随机点云的数量，提高工作空间的精度.该方法在工作空间求解中，由于参考点为随机选取，不一定位于区域中心位置，并且扩展了该点处加密点云的范围，使得该区域内存在点云数量得不到充分加密和外溢点云的浪费问题.

针对蒙特卡洛法在求解机械臂工作空间时存在的不足，提出一种降密蒙特卡洛法，并研究该方法中各参数设置对工作空间精度的影响.降密蒙特卡洛法通过准确加密每个区域内点云数量，均匀提高工作空间内部点云密度，使工作空间内部与边界分明；并针对边界点云数量少和边界模糊的缺点，采用扩展各关节角度范围的方式，通过多次迭代提高工作空间边界点云的密度和扩展工作空间边界范围，最终达到在保证工作空间精度的同时，减少新增点云浪费的目的，从而提高求解工作空间的效率.

1 仿人机械臂运动学分析

1.1 仿人机械臂结构

仿人机械臂作为机器人的执行机构，具有工作空间范围大、灵活性高、较好的避障性和拟人化的执行动作等优点，被学者们广泛关注.为了提高人机交互的舒适性和亲和力[18-19]，本课题组提出了一种具有人体手臂构型的7-DOF 仿人机械臂，如图1 所示，该机械臂由肩、肘和腕关节模块组成，θi为对应关节的角度值，i=1,2,···,D，D为机械臂关节数.肩关节模块由3 个电机并联驱动3 个关节，肘和腕关节模块分别由2 个电机并联驱动2 个关节.各模块之间采用并行通讯，相互独立工作，共同完成机械臂的规划任务.由于仿人机械臂各关节模块采用电机加钢丝绳的绳索驱动方式以及多电机并联驱动多关节的布局方式，使得该机械臂具有重量轻、结构紧凑、输出比高和拟人化运动程度高等优点.该机械臂拥有7 个电机和7 个自由度，全长690 mm，自重2.2 kg，可夹持重量1.5 kg，其自重与承载比高达22∶15.

仿人机械臂各关节的关节限位如表1 所示，其各关节的正方向如图1 中标注所示.

表1 仿人机械臂关节限位Tab.1 Joint limit of humanoid robot arm

1.2 运动学分析

仿人机械臂的运动学分析是机械臂工作空间求解和运动学控制的基础.正运动学是已知机械臂的各关节角度去推算机械臂末端执行器在笛卡尔坐标系下的位置与姿态.根据7-DOF 仿人机械臂构型和标准D-H（Denavit-Hartenberg）建模方法，建立仿人机械臂运动学坐标系如图2 所示.图2 中：dbs为肩关节到基座的距离，dse为肘关节到肩关节的距离，dew为腕关节到肘关节的距离，awt为机械臂末端到腕关节的距离.

图2 仿人机械臂运动学坐标系Fig.2 Kinematics coordinate system of humanoid robot arm

根据仿人机械臂各连杆之间的坐标系关系，推出机械臂各连杆D-H 参数如表2 所示.其中：di为相邻2 个关节之间的连杆偏移量，ai为相邻2 个关节之间的连杆长度，αi为相邻2 个连杆坐标系之间的扭转角度.

表2 仿人机械臂D-H 参数Tab.2 D-H parameters for humanoid robot arm

仿人机械臂相邻两连杆之间的位姿变换矩阵为

式 中：cθi=cos θi，sθi=sin θi，cαi=cos αi，sαi=sin αi.

仿人机械臂的正运动学模型为

式中：[nxnynz]T为机械臂末端执行器坐标系的x7轴相对于基坐标系中的方向矢量，[oxoyoz]T为机械臂末端执行器坐标系的y7轴相对于基坐标系中的方向矢量，[axayaz]T为机械臂末端执行器坐标系的z7轴相对于基坐标系中的方向矢量，Px、Py和Pz分别为机械臂末端执行器在笛卡尔坐标系中的空间位置点，该位置点的集合即为机械臂工作空间.

2 蒙特卡洛法

蒙特卡洛法是一种基于概率统计的非确定性数值计算方法，因其误差与计算维数无关，常被学者们用来求解多关节机械臂的工作空间.蒙特卡洛法求解步骤如下：

步骤1建立机械臂正运动学数学模型T，并确定机械臂各关节限位[θimin,θimax].

步骤2基于随机函数rand（·）生成N个[0,1]间的随机变量，并计算出各关节的随机关节角度为

步骤3将各关节随机值自由匹配为N组，代入正运动学方程，获得机械臂末端执行器参考点的位置坐标P（x,y,z）.

步骤4基于机械臂基座标，绘制每个参考点的位置坐标，获得机械臂工作空间的点云图.

3 降密蒙特卡洛法

3.1 体素网格法的实现

根据蒙特卡洛法生成的机械臂工作空间初始随机点云Pinit以及体素网格法[20]，将初始点云分割为若干矩形区域，每个矩形区域被称为体素.体素网格法的具体步骤如下：

步骤1建立随机点云的包围盒.根据随机点云Pinit的位置坐标获得Pinit在各轴向的极值xmax、ymax、zmax、xmin、ymin和zmin，并计算出点云各轴向长度l=|xmax-xmin|、w=|ymax-ymin| 和h=|zmax-zmin| .为了避免初始随机点云没有覆盖到工作空间的边界，造成工作空间的缺失，本文引入扩展因子 φ 扩大随机点云的边界范围，如图3（a）所示，并通过式（3）确定随机点云的扩展范围长度d.

图3 随机点云的划分和索引标注Fig.3 Division of random point clouds and voxel labeling

式中：φ ∈[0,1]，当φ=0 时，d=0，此时包围盒不扩展，当φ=1 时，d为包围盒最小边长的1/4.

扩展因子 φ 的选择与初始随机点云的范围相关，当初始随机点云数量少，覆盖工作空间范围小时，φ 值应取较大值，有利于更全面地覆盖工作空间的实际边界；反之亦然.

通过扩展范围长度d确定包围盒的实际边界值，如式（4）所示，并建立包围盒，如图3（b）所示.

步骤2分割包围盒为若干体素.根据包围盒各轴向极值范围，将包围盒分别沿x、y和z轴分割为nx、ny和nz等份，如图3（c）所示，计算出各轴向体素的边长为lb、wb和hb，如式（5）所示，并推算出每个体素的边界极值坐标，为后续初始随机点云的划分奠定基础.

步骤3标记体素索引n.为了更好地识别每个体素的位置，提出了体素索引n的标记规则，其中n∈{1,2,3,···}.体素标记采用统一编号规则：首先，沿x轴正方向，从1 开始标记直到最后一个体素nx；其次，沿y轴正方向移动一个体素，继续沿x轴顺序标注，直到标记整个底层的最后一个体素nxny；然后，沿z轴正方向移动一个体素，继续前2 步操作，并顺序标注索引；依此类推直至标记最后一个体素nxnynz，如图3（d）所示.

3.2 均匀加密工作空间内部点云的密度

在工作空间加密过程中，通过设置体素最低精度阈值Nε以及准确加密体素内点云数量的方法，使加密后机械臂工作空间内部点云密度均衡的同时，减少新增点云数量的浪费，算法1 步骤如下：

步骤1依照体素索引n，搜索体素内初始随机点云数量低于精度阈值Nε的体素，并记录该体素内的点云数量Nn.将体素索引值n的点云坐标和点云数量Nn保存在数据库Database_1 中；同时，将体素n内点云坐标所对应的机械臂各关节的角度值θ1、θ2、···、θi保存到数据库Database_2 中（第1～10 行）.

步骤2针对数据库Database_1 中保留的体素索引n及该体素内包含的点云数量Nn，确定出体素n所需加密点云数量为Nneed=Nε-Nn（第17 行）.为了准确加密体素内点云数量，减少新增点云的浪费，提取数据库Database_2 中体素n对应所有点云的关节角度，并按照关节顺序依次探索出体素n内各关节的极值（第18 行）.通过体素n所需加密点云数量Nneed和各关节的极值θimin和θimax获得机械臂关节i的Nneed个新增关节角度值（第19 行）为

根据自由匹配原则，将式（6）随机生成的关节角 度匹配成Nneed组，并将Nneed组关节角度值代入式（2）中，获得机械臂正运动学解（第20 行）.

步骤3判断新增点云的坐标是否包含在体素n内，如果包括在内，则按照索引n保存在数据Database_3 中，并将点云所对应各关节角度按照索引n并入数据库Database_2（第22～25 行）；如果不包括，则重复之前操作.为了避免工作空间边界处的体素始终无法达到所需点数而造成的资源浪费，设置重复加密次数为2 次后停止加密运算（第27～29 行）.

算法1：均匀加密工作空间内部点云密度

3.3 加密工作空间边界随机点云的密度

由于机械臂关节极值范围的约束，机械臂工作空间边界体素的实际有效加密范围小于体素实际范围，因此，边界体素在平均加密点云过程中实际增加点数较少，造成机械臂工作空间的精度不够准确.为了提高机械臂工作空间边界的点云密度和扩展初始点云中没有涉及到的机械臂工作空间边界，提出了扩展关节角度Je参数和最大循环次数Cm参数.该加密工作空间边界点云密度算法（算法2）步骤如下：

步骤1针对加密后体素内点云数量仍然少于Nε的体素视为工作空间的边界体素.依据数据库Database_1 和Database_3 中边界体素n所包含的点云数量，确定边界体素n所需加密点云数量Nnneed（第3 行）.如果体素n内点云数量为1 时，提取出体素n在数据库Database_2 中的各关节值θi作为被扩展关节；否则，探索出该体素在Database_2 中各关节的极值θimin和θimax，并作为被扩展关节的最大值和最小值（第9、13 行）.通过扩展关节角度Je获得各关节扩展后的极值θi_newmin和θi_newmax（第10、11、14、15 行）为

将各关节扩展后的极值θi_newmin和θi_newmax代入式（6），获得Nnneed组机械臂新增关节角度（第17 行）.

步骤2由于新增关节角度是通过扩展关节获得，所以Nnneed组内各关节角度不一定都符合机械臂的各关节极值范围（如表1 所示）.对于符合关节极值范围的关节组，运用式（2）求解机械臂正运动学解，并将运动学解的坐标和解的数量按照索引n并入数据Database_3 中；对于不符合关节极值范围的关节组，则放弃求解正运动学解（第18～24 行）.

步骤3由于边界体素n受到机械臂关节极值的约束以及实际有效加密范围小于体素极值范围的影响，导致边界体素的有效新增点云数量较少.因此，通过设置最大循环次数Cm，并结合实际增加点云数量与所需点数量Nnneed的差值，通过多次迭代增加边界体素内的点云密度及实际有效范围，并将新增点云并入数据库Database_3 中，直到实际增加点数等于所需点数，或者达到最大循环次数Cm停止（第5～24 行）.最大循环次数Cm还可以有效避免因扩展关节范围导致新增关节角度始终不符合机械臂关节极值范围，而造成的算法无效循环.

算法2：加密工作空间边界点云密度

通过降密蒙特卡洛法可以解决工作空间内随机点云的分布不均匀和工作空间边界模糊问题，最终达到在保证工作空间精度的同时，减少新增点云浪费的目的，从而提高求解工作空间的效率.

4 降密蒙特卡洛法的参数研究

以本课题组提出的7-DOF 仿人机械臂为例，研究了降密蒙特卡洛法中各参数对机械臂工作空间精度的影响.为了获得仿人机械臂在各轴向的实际边界极值作为降密蒙特卡洛法的求解参照，利用蒙特卡洛法经过多次采样计算去趋近机械臂工作空间的边界极限，如图4 所示.当随机点云数量低于5 ×107个时，工作空间各轴向极值点不具备一定的规律性，这也符合蒙特卡洛法的随机特性；当随机点云数量大于5 × 107个时，各轴向极值点趋于一条直线，基本可以达到真实的机械臂边界值，但在小范围内仍然有随机性波动.即使采样随机点数量达到1 ×108个时，采样工作空间的各轴向极值点也均未达到机械臂工作空间的实际极值点.

图4 蒙特卡洛法趋近工作空间边界Fig.4 Workspace boundary approaching by Monte Carlo method

为了获得准确的工作空间边界范围，将图4 中所有采样测试点云在各轴向极值点的极值作为仿人机械臂工作空间的实际极值点，如表3 所示.

表3 机械臂实际工作空间范围Tab.3 Actual workspace range of robot arm m

降密蒙特卡洛法中各参数对机械臂工作空间精度影响的研究方法为通过固定其他参数数值，逐步递增单一参数数值，获得降密蒙特卡洛法中机械臂工作空间的各轴向极值范围，并与表3 中实际极值范围对比，获得平均误差率 εa，如式（8）.

式中：xMax、xMin、yMax、yMin、zMax、zMin为机械臂工作空间各轴向实际极值点.

通过平均误差率 εa的变化规律来判断该参数对机械臂工作空间范围的影响程度.由于包围盒的扩展因子φ与机械臂的工作空间范围相关，所以选取包围盒扩展因子φ为固定值0.3.

首先，研究初始随机点云Pinit参数对工作空间精度的影响.根据工作空间的初始范围，初步将空间延各轴向划分为10 个体素，即各轴向分割体素nx、ny和nz均为10；根据分割体素空间的大小，保证每个体素内的随机点云数量不少于600 个，即精度阈值Nε为600；设置最大循环次数Cm为5 次，保证边界体素内随机点云数量能更好地满足精度阈值Nε的要求；扩展关节角度Je为0°；并分别设置初始随机点云Pinit为5×103、1×104、5 ×104、1× 105个.通过降密蒙特卡洛法获得机械臂工作空间各轴向的极值点，如表4所示，其中，初始随机点云Pinit为5×103、1 × 104、5×104、1×105个，对应总随机点云数量分别为228 000、230 300、240 006、255 988 个.根据表4中数据可知，随初始随机点云Pinit数量的逐步增加，其平均误差率 εa并不具备一定的规律性，且总随机点云数量增幅最小.因此，初始随机点云Pinit对机械臂工作空间精度的影响不大.

表4 初始点云数量对工作空间精度的影响Tab.4 Influence of initial point cloud quantity on workspace precision

其次，研究包围盒各轴向分割体素nx、ny和nz参数对工作空间精度的影响.根据Pinit对工作空间的影响不大，综合考虑计算效率与初始工作空间点云覆盖率，确定参数Pinit为5 × 104个；参数Nε、Cm和Je保持原有设置，依次为600、5 次、0°；并设置参数nx、ny和nz同时依次均为6、10、14 和18.通过降密蒙特卡洛法获得机械臂工作空间各轴向的极值点，如表5 所示，其中各轴向分割体素6、10、14、18 对应总随机点云数量分别为1.5 × 105、2.4 × 105、7.9 × 105、1.5 × 106个.根据表5 中随各轴向分割体素数量的逐步增加，其平均误差率则表现出大幅减小的趋势，且总随机点云数量增加幅度较小.因此，增加各轴向分割体素nx、ny和nz数量显著提高机械臂工作空间精度.

表5 体素数量对工作空间精度的影响Tab.5 Influence of voxel quantity on workspace precision

再次，研究精度阈值Nε参数对工作空间精度的影响.参数Pinit仍为5 ×104；根据参数nx、ny和nz对工作空间精度的影响较大，选取nx、ny和nz均为18；Cm和Je分别保持为5次和0°；分别设置Nε为300、600 和900.获得机械臂工作空间的各轴向极值点如表6 所示，其中，精度阈值为300、600、900 对应总随机点云数量分别为7.6 × 105、1.5 × 106、2.3 × 106个.由表6 可知，随精度阈值的逐步增加，其平均误差率表现出逐渐减小的趋势，但总随机点云数量增幅较大.因此，精度阈值Nε参数对机械臂工作空间精度有一定提高.

表6 精度阈值对工作空间精度的影响Tab.6 Influence of precision threshold on workspace precision

然后，研究最大循环次数Cm对工作空间精度的 影响.Pinit仍 为5 × 104，根据Nε对工作空间精度有一定的提高，选取Nε为600；nx、ny、nz和Je保持为18 和0°，分别设置Cm为5、8、16 次.获得机械臂工作空间的各轴向极值点，如表7 所示，其中最大 循环次数5、8、16 次对应总随机点云数量分别为1 539 000、1 544 400、1 555 800 个.根据表7，随最大循环次数的逐步增加，其平均误差率逐渐减小，且总随机点云数量的增加并不明显.因此，最大循环次数Cm对机械臂工作空间精度有较小的提高.

表7 最大循环次数对工作空间精度的影响Tab.7 Influence of maximum cycle number on workspace precision

最后，研究扩展关节角度Je对工作空间精度的影响.固定Pinit=5 × 104，nx、ny和nz均为18，Nε为600；考虑到参数Cm对工作空间的影响较小，选取Cm为5 次，分别设置Je为0°、2.5°、5.0°、7.5°、10.0°和12.5°.通过3 次样条曲线插值方法，获得相应参数机械臂工作空间的平均误差率曲线和总随机点云数量Nall曲线如图5 所示，根据总随机点云数量曲线可知，随着扩展角度的增大，总随机点云数量不断减少，这是因为随着扩展角度的不断增大，新增的关节角度组中不符合关节极限范围的关节组就越多（如算法2），导致总体数量减少；根据平均误差率曲线可知，随着扩展角度的增大，在0°～4° 内平均误差率变小，可以提高机械臂工作空间精度，但超过4° 后误差率逐步增大，对工作空间精度提升起到反向作用.因此，扩展关节角度Je在0°～4° 内对机械臂工作空间精度提高较大.

图5 扩展关节角度对工作空间精度的影响Fig.5 Influence of extended joint angle on workspace precision

通过降密蒙特卡洛法中各参数对工作空间精度影响的研究可知：各轴向分割体素nx、ny和nz与扩展关节角度Je对工作空间精度影响较大，其次是精度阈值Nε，最后是最大循环次数Cm和初始随机点云Pinit，并不是扩展角度Je参数越大对工作空间精度提升越明显.因此，在使用该方法求解工作空间时，需要优先确定各轴向分割体素nx、ny、nz与扩展关节角度Je，通过提高分割体素nx、ny、nz与设置较小的扩展关节角度Je，使工作空间的解快速接近于精确值；其次，通过提高精度阈值Nε，增加工作空间整体密度，使工作空间的解更加接近于精确值；最后确定最大循环次数Cm和初始随机点云Pinit，通过小幅调节这2 个参数，使工作空间的解趋近于精确值.该方法中参数优先级的确定和调节方式为降密蒙特卡洛法快速、准确求解工作空间提供了优选策略.

5 仿真验证

根据降密蒙特卡洛法中各参数对工作空间精度的影响程度，设置参数Pinit为1 × 105，nx、ny和nz均为23，Nε为1 200，Cm为16 次，Je为2.5°，验证降密蒙特卡洛法的有效性.降密蒙特卡洛法中仿人机械臂工作空间的极值范围如表8 所示，其总随机点云为6.11 × 106个.相比于蒙特卡洛法中工作空间各轴向极值的极值点，降密蒙特卡洛法的平均误差率εa=0.022 42%，其各轴向极值点的误差保持在10-4左右，精度可达0.1 mm 级，在ymin和zmin2 处极值点已经超过了表3 中的参照极值点，但总随机点数却少于1 × 108个，降幅为93.89%.根据降密蒙特卡洛法中各参数对机械臂工作空间精度影响的研究可知，随着机械臂工作空间平均误差率的升高，该算法所需的总随机点云将会大幅减少，其优势更加明显.因此，降密蒙特卡洛法与蒙特卡洛方法相比，在保证工作空间精度的同时，可以大幅减少点云数量，提高工作空间的求解效率，且通过平均误差率验证了降密蒙特卡洛法的有效性.

表8 降密蒙特卡洛法中工作空间范围及其误差率Tab.8 Workspace range and error rate by density-reducing Monte Carlo method

根据降密蒙特卡洛法求得仿人机械臂工作空间示意如图6 所示，从图6 可知：7-DOF 仿人机械臂工作空间接近于球体，具有较大的工作范围，各平面投影无空洞与空腔，工作空间边界线平滑连续.

图6 仿人机械臂工作空间示意Fig.6 Humanoid robot arm workspace

相比于改进蒙特卡洛法，通过设置相同的求解工作空间参数，对比降密蒙特卡洛法与改进蒙特卡洛法的差异.2 种方法中Pinit为5 × 104，nx、ny和nz均为10，Nε为600；降密蒙特卡洛法中Je为2.5°，改进蒙特卡洛法采用体元内的随机点作为正态分布中心点，方差为2.5°；最大循环次数Cm分别为2 次和4 次，工作空间的求解结果如表9 所示.根据表9 可知：降密蒙特卡洛法相比于改进蒙特卡洛法在循环次数为2 次和4 次时，工作空间的平均误差率分别降低0.138 53%和0.113 29%，总随机点云数量降幅分别为44.83%和64.52%，其求解耗时也大幅下降.因此，降密蒙特卡洛法相比于改进蒙特卡洛法在保证工作空间精度的同时，有效减少随机点云数量的浪费，提高了求解机械臂工作空间的工作效率.

表9 降密蒙特卡洛法与改进蒙特卡洛法对比Tab.9 Comparison between density-reducing Monte Carlo method and improved Monte Carlo method

6 结论

1）分析了蒙特卡洛法和改进蒙特卡洛法在求解机械臂工作空间时存在的不足，提出了一种降密蒙特卡洛法，该方法在保证机械臂工作空间精度的同时，可以减少工作空间随机点云的数量，提高求解工作空间的效率.

2）研究了降密蒙特卡洛法中各参数对机械臂工作空间的影响程度，解释说明了各轴向分割体素nx、ny和nz与扩展关节角度Je对工作空间精度的影响较大，需要优先确定其参数值；精度阈值Nε参数对工作空间精度有一定的提高；最后设置最大循环次数Cm和初始随机点云Pinit参数.所提出方法中参数的确定优先级，为降密蒙特卡洛法快速、准确求解工作空间提供了优选策略.

3）以7-DOF 仿人机械臂例，验证了降密蒙特卡洛法的有效性，为后续仿人机械臂任务规划奠定了理论基础.