基于三维点云的双机器人协作打磨轨迹规划研究

周 震，姜笑言，韩家哺

（上海工程技术大学 机械与汽车工程学院，上海 201620）

随着现代智能化产业的日益发展，对生产领域的自动化程度提出了更高要求，机器人在工业应用中占据重要角色。智能化是工业机器人发展的方向，协作是未来工业机器人发展的必然选择［1］。由于多机器人协作在路径空间和灵活适应性等方面优于单机器人，且在同一系统中具有互相协作的优势，使其工程应用十分广泛。多机器人协作主要涉及协作机器人的基坐标系标定，轨迹规划及位置／力协同控制，控制与避碰等方向。而双机器人协作也有广泛关注和研究，Liu 等［2］以双臂搬运方块为研究对象，在双臂末端与物体之间分别设置位置约束、速度约束和加速度约束进行优化，实时补偿了运动误差，但由于较强的约束导致双臂运动范围极其有限；
Cohen 等［3］在高维度空间利用双臂构造搜索空间，采用启发式搜索算法，解决双臂末端轨迹光滑问题，但会产生抖动现象；
于广东等［4］对双机器人同步协调运动和相对协调运动做了详细的分析，并对这两类协调运动进行简单的实验；
Zhang 等［5］使用三维激光扫描仪扫描叶片形成了密集点云，并通过点云预处理算法和切片算法，生成机器人的磨抛轨迹，但点云数据处理的算法复杂；
侯仰强等［6］建立双机器人协调焊接系统，分析影响焊缝质量及机器人平稳性的各参数，建立了评价无干涉焊接路径的目标函数，但该方法只局限于焊接类似“马鞍式”焊缝的焊接任务。

本文在吸取借鉴前人研究的同时，打破了轮毂打磨设备灵活性差、复杂空间轨迹难以获取、方法适用领域单一等局限性，以期加深对多机器人协作系统及轨迹规划的研究。通过构建双机器人协作打磨坐标系和运动学约束关系，对采集的三维点云数据预处理，规范标准化姿态，进行轮毂表面特征线的提取，获得理想的空间打磨轨迹。对模型及方法进行仿真实验验证，分析双机器人相对运动协调打磨的可行性。

1.1 协作打磨各坐标系的建立

在主从式双机器人协作轮毂打磨过程中，夹持轮毂的机器人作为主机器人，从机器人夹持磨具，以相对协调的运动方式对预定轮毂轨迹点跟踪打磨。根据主机器人与从机器人末端位姿的相对变换矩阵是否时变，可将协调运动分为紧耦合和松耦合形式［7］。紧耦合协调运动时末端位姿矩阵相对不变，应用于协同货物搬运等任务；
松耦合协调运动时相对末端位姿矩阵时刻变化，运动轨迹较为复杂，应用于协作焊接、包装码垛、装配等任务。

双机器人系统坐标系如图1 所示，定义n ＝1 为夹持机器人，n ＝2 为打磨机器人，选取主机器人基座标系 [B1] 为世界坐标系 [S]，则 [B2] 为从机器人基坐标系。

[En] 为机器人的末端坐标系，[Tn] 为工具坐标系，[ w] 为打磨离散点坐标系。

图1 双机器人系统坐标系Fig.1 Coordinate system of dual robot systems

1.2 协作打磨任务运动学关系

双机器人相对运动协作打磨是两台机器人相互协调完成既定打磨任务，不仅存在末端位姿约束，还存在相对运动约束。在双机器人协作打磨系统中，两台机器人形成的一条闭合运动链，可解耦成两条开链形式［7］。一条运动链由夹持机器人的“夹持机器人基底-末端法兰-工具-轮毂-打磨轨迹”组成；
另一条由打磨机器人的“夹持机器人基底-打磨机器人基底-末端法兰-工具-工件-打磨轨迹”组成。研究双机器人运动学关系就是研究运动链的耦合和解耦。双机器人运动的表达形式就是用各个坐标系的转换找到坐标系之间矩阵变换关系实现的，根据相对协调运动时的打磨任务要求，整体的闭合运动链模型为

将式（1）所示运动链解耦，分别得到两组机器人的开链模型表达式为式（2）和式（3）：

2.1 空间复杂轮毂点云特征线的提取

在自动化打磨生产过程中，需要对轮毂表面不同区域进行路径分析。打磨路径一般可分解为直线或规则圆弧，通过插补算法将轨迹离散成一系列空间打磨轨迹点，机器人末端依次跟踪序列点完成打磨任务。而复杂不规则的空间曲线，由于轮毂制造厂商对于非标工件知识产权的维护，不提供相应的CAD／CAM 三维模型，则不能获得轮毂结构和空间曲面信息。本文通过激光扫描臂对轮毂精细扫描，逆向成型获得轮廓三维点云数据，并利用基于姿态标准化的点云特征线方法提取空间曲线特征信息。

2.1.1 构建轮毂的三维点云模型

要获得打磨作业时末端执行器运动轨迹和精准的位姿信息，前期点云模型构建的准确度非常关键。利用FARO 激光扫描臂，通过逆向工程的方式对无CAD 三维模型的轮毂结构进行数字化扫描。先用扫描仪器的扫描探头在轮毂中心轴的最低点采集3个点，构造一个平面，并以底部圆平面中心点的位置建立基准坐标系，随后手持扫描仪器对轮毂各个区域来回往复扫描。采集的轮毂点云模型总坐标点数为886 921 个，激光扫描臂实验过程和初始轮毂三维点云模型如图2、图3 所示。

图2 激光扫描臂实验Fig.2 Laser scanning arm experiment

图3 初始轮毂三维点云模型Fig.3 Initial Hub three-dimensional point cloud model

2.1.2 轮毂点云的姿态标准化

激光扫描臂采集的原始轮毂点云模型会存在点云数据过于密集或冗余、噪点等问题。利用扫描臂适配的PolyWorks 软件中的点云处理命令对模型进行简单预处理，过滤噪点，去除多余的扫描数据使点云数据均匀化。处理后的点云数据呈现无序状态，确定点云的姿态首先要快速搜索临近点云，提高后期计算效率。点云邻域搜索主要用KD-TREE 算法实现，通过在基准参考坐标系上构建的决策树搜索临近点云［8］。为了消除点云姿态对轮毂曲面拟合误差的影响，应先调整点云姿态，避免不同姿态导致的拟合误差问题。

设得到的临近点云数据为：A ＝｛（xi，yi，zi）｜i ＝1，2，3，…，n｝，将数据拟合成形如z ＝f（x，y）的目标曲面方程，则曲面拟合误差为

由式（6）可看出，当模型中各点云的坐标系方向不同时，目标方程会有所差别，且拟合误差也受影响，所以要规范姿态标准化。点云特征信息的主方向可以表达点云分布情况，当点云数据的主方向与基准坐标系Z轴重合，则在XOY平面区域分布更平均，姿态标准化后曲面拟合效果更好。临近点云A的主方向Dmain可用PCA（Principal Component Analysis）算法得到，先计算点云A的协方差矩阵，并对矩阵特征值分解，得出特征值λ1、λ2、λ3和特征向量α1、α2、α3，则点云主方向Dmain可表示为

利用Householder 矩阵求得转换矩阵R，使点云主方向Dmain与坐标系Z轴对准，则姿态标准化后的点云A为

2.1.3 点云特征线的提取

工件型面一般都有明显的几何特征，这些几何特征能反映工件表面轮廓的一些空间曲线、曲面等。本文点云特征线提取是基于轮毂表面结构的最大主曲率绝对值作为拟合曲面曲率的估计值，识别提取出表面轮廓边界的曲线、棱边、尖角、连接过渡等形状特征。工件的点云特征曲线同样也是轮毂分区域分块打磨的依据。

将姿态标准化后的坐标点云数据用二次曲面方程进行曲面拟合为

曲面拟合的系数为

令X ＝［a，b，c，d，e，k］T，则式（10）中的系数可表示为：

由于计算曲率的坐标点Ai（xi，yi，zi）不是拟合曲面上的点，则需将Ai沿着点云主方向向曲面位置投影，可得投影点At i的矩阵，式（11）：

令：

若存在式（12）：

当等式（12）成立时，主曲率值满足方程（13）［9］：

将式（12）、（13），解方程得曲面投影点的主方向曲率为k1，k2。结合轮廓点云特征，利用最大主曲率绝对值作为拟合曲面曲率的估计值，提高可区分性，使点云特征线与非特征线的曲率的估计值差别更大，更易识别提取点云，则曲率可表示为

将激光扫描臂实验采集的轮毂三维点云与处理后特征线提取的数据文件，导入到Matlab 中编写程序，进行可视化分析。初始轮毂点云模型与数据处理后模型如图4、图5 所示，对比结果表明所用算法能较精准清晰的表达轮毂特征线，为后续待打磨点的选择提供依据。

图4 初始轮毂点云模型Fig.4 Initial hub point cloud model

图5 特征线提取后点云模型Fig.5 Point cloud model after feature line extraction

2.2 轮毂空间打磨轨迹规划

在双机器人协作打磨过程中，选取待打磨点能在一定程度上表达空间轨迹的所有特征。完成高质量连续打磨要求不仅与打磨方案和工艺参数有关，更要建立协调运动中待打磨轨迹点与打磨工具位姿模型及相对位姿的关系。

2.2.1 打磨点位姿数学模型

从精简后的点云数据中，选取期望的20 个待打磨轨迹坐标点。利用多项式拟合获得关于时间t（0 ≤t≤10）的空间打磨点轨迹方程组，式（15）：

令打磨点位置矩阵Pi＝xi yi zi]T，其中t的单位为s，坐标系的单位为mm。

式（15）拟合的打磨轨迹是由离散点（待打磨点）连接而成。基于打磨点的空间位姿求取，在每个打磨点上建立坐标系，并通过世界坐标系与工件打磨点坐标系的相对位姿矩阵关系来确定系列打磨点的位姿。位置是该打磨点在轮毂工件坐标系下的相对位置，姿态是空间打磨轨迹在该打磨点上的切向和法向。以拟合的轮毂空间曲线为例，建立的待打磨点坐标系如图6 所示，［T1］为轮毂工件坐标系，［wi］为第i个打磨点坐标系。由机器人工件坐标系、待打磨点坐标系相对姿态关系作以下规定：空间离散打磨点的法向量u令作打磨点坐标系的X轴，打磨点的切向量v令作坐标系的Y轴并指向下一点离散点方向，法向量u和切向量v的矢量叉乘得到的矢量w是打磨点坐标系［wi］的Z轴。各个点不同的位姿信息共同构成协调打磨轨迹运动。

图6 待打磨点位姿坐标系Fig.6 Position of point to be polished system

空间姿态变化表明，对打磨点姿态调整就是通过机器人末端夹持对工件姿态的调整。利用ZYX序列的欧拉角表示从上一个打磨点到下一个打磨点坐标系调整后的旋转矩阵Ri为

由式（15）、（16）知，最终得到任意打磨点i在轮毂工件坐标系［T1］下的位姿矩阵可表示为

2.2.2 打磨工具位姿数学模型

在双机器人相对运动协调打磨任务中要获得好的打磨效果，打磨工具的姿态须与打磨点的矢量方向一致，打磨头也要和打磨点位置重合。在打磨时机器人夹持轮毂，经过轮毂位姿的调整达到每个待打磨点理想的位姿，打磨工具从上一个点到下个打磨点也要求调整至理想位姿。令打磨工具坐标系［T2］的Y轴方向与打磨轨迹上点的切向相同并指向下一点是工具的运动方向；
［T2］的X轴方向与轨迹上点的法向重合，同时是打磨力控的方向；
法向量u与切向量v的矢量叉乘值w是坐标系［T2］的Z轴向，控制工具运动方向。打磨工具中心轴线是Z轴，与打磨点坐标系的Z轴同向，打磨精确度高时打磨点与工件末端打磨头位置一致。所以打磨点坐标系［wi］与打磨工具坐标系［T2］相对姿态的关系就是以Z轴向不变的前提下，X、Y轴自转角α的调整，如图7 所示。

图7 待磨点和打磨工具坐标系相对姿态调整Fig.7 Adjust the relative posture of the coordinate system of the grinding point and the grinding tool

当打磨工具在某待磨点打磨瞬间，轮毂与打磨工具相对静止，待磨点与打磨头位置重合，设置合适的自转角α旋转即可调整相对位姿关系，调整后的打磨工具在世界坐标系下位姿矩阵为

本文双工业机器人协作打磨方式能去除复杂轮毂表面任意位置毛刺或凸起等不规则杂质，提高打磨精细化处理的能力。为验证协调运动学模型和点云特征线提取算法的有效性，基于SolidWorks／Simscape 联合仿真搭建双机器人物理模型。为实现机器人的控制，需要将生成的原始模型添加合适的组件模块，并设置属性参数；
编程实现双机器人协作打磨运动学模型和相关算法；
通过信号线连接Simulink 工具箱中模块，封装程序间的逻辑关系。

3.1 双机器人仿真系统原理

由式（4）、（5）可知，为得到夹持机器人和打磨机器人末端位姿，还需要知道打磨点坐标系在世界坐标系下的位姿矩阵变化，式（19）和式（20）：

其中，Rw是打磨点在世界坐标系下的旋转矩阵，由定义的坐标系方向可知；
Pw、Pi是打磨点在世界坐标系、轮毂工件坐标系下的位置矩阵；
Ri是磨点在轮毂工件坐标系下姿态矩阵。Pi、Ri由式（20）得到；
PT1是轮毂工件在世界坐标系下的位置，可由上位机读取进行转换。

依据式（4）、（5）、（19）和（20）可得双机器人的末端位姿矩阵并利用逆运动学算法得到机器人各轴关节角序列变化，控制协调打磨运动。双机器人运动学仿真系统的原理框架如图8 所示。

图8 仿真系统原理框架Fig.8 Principle framework of simulation system

3.2 仿真实验分析及结果

采用Matlab／Simulink 仿真环境平台，双机器人物理模型、运动学约束关系及正逆运动学求解算法都被封装成独立模块，并通过信号线和Simulink 库中的模块集连接。双机器人协作打磨运动学仿真模型如图9 所示。

图9 双机器人运动学仿真模型Fig.9 Kinematics simulation model of two robots

仿真实验所使用的打磨轨迹是轮毂点云数据经特征处理后，拟合而成的空间曲线。若有优秀的跟踪打磨表现，其研究方法也必能沿用到空间直线或平面曲线等其他打磨轨迹上。实验时夹持轮毂机器人进行空间位置和姿态的不断调整，打磨机器人跟踪20 个期望待打磨点进行打磨运动。双机器人协调打磨过程如图10 所示，仿真时间是10 s，图10（a）是打磨机器人磨头行驶到初始待磨点且磨头与待磨点位置重合，打磨工具轴线Z轴与待磨点法线矢量平行；
图10（b）、（c）、（d）是第3 s、7 s 和第9 s时的双机器人位姿调整示意图。双机器人位姿变化良好，能有效地进行协作打磨运动。

图10 双机器人协调打磨Fig.10 Coordinated grinding by dual robot

双机器人在协调运动中，机器人各关节角序列变化如图11 所示，曲线变化平滑无突变；
协调打磨工况下各个机器人末端的三维空间轨迹如图12 所示，打磨机器人的运动轨迹与期望打磨轨迹一致。

图11 机器人各关节角序列变化Fig.11 The sequence changes of robot joint angles

图12 各机器人末端轨迹Fig.12 End trajectories of each robot

仿真实验结果表明：双机器人在协作打磨运动过程中，磨具末端与待打磨点有良好的接触吻合，3D 动画显示打磨运动平稳未出现抖动现象，各关节角度曲线变化平滑未出现尖点，运动状态有较好的连续性，证明了协作打磨运动学模型和算法的可行性。

本文提出双机器人相对运动协调打磨方法及建立双机器人协作打磨坐标系、双机器人运动学模型和约束，能有效完成工件的复杂空间曲线打磨问题，增大工作空间并提高了操作的灵活性；
基于三维点云的姿态标准化，并用最大主曲率绝对值作为拟合曲面曲率的估计值，能提高特征点云可区分性，便于空间打磨点的提取，三维扫描技术结合机器人打磨为工程实际应用提供了新思路。针对复杂多变的工业环境，虽然本文提出的方法简单且可复制性强，但双机器人协作的力控制及提高变工件打磨效率下一步需解决。

猜你喜欢位姿轮毂姿态融合二维图像和三维点云的相机位姿估计光学精密工程(2022年22期)2022-11-28电驱动轮轮毂设计及有限元分析汽车实用技术(2022年5期)2022-04-02抗压痕透明粉在精车铝轮毂上的应用研究上海涂料(2021年5期)2022-01-15攀爬的姿态学生天地(2020年3期)2020-08-25船舶清理机器人定位基准位姿测量技术研究浙江海洋大学学报(自然科学版)(2020年5期)2020-06-19全新一代宋的新姿态汽车观察(2018年9期)2018-10-23跑与走的姿态中国自行车(2018年8期)2018-09-26基于CPS 的汽车轮毂制造系统设计制造技术与机床(2017年10期)2017-11-28基于ANSYS的轮毂支架结构设计制造业自动化(2017年2期)2017-03-20小型四旋翼飞行器位姿建模及其仿真湖北工业大学学报(2016年5期)2016-02-27