行星采样机械臂的轨迹规划研究

张冰蔚 戚永康 齐超 申阿强 张鹏

（江苏科技大学机械工程学院， 江苏 镇江  212003）

摘  要： 为行星采样机械臂做轨迹规划，先根据D?H法建立机械臂的运动学模型，对其进行了笛卡尔空间直线插补和圆弧插补两种轨迹的规划，并在Matlab中进行了运动仿真。然后基于MFC和OpenGL设计一种针对采样机械臂的运动仿真程序。最后结合一种运动控制器进行直线插补和圆弧插补的轨迹规划的实验。实验结果表明，采样机械臂的实际运动轨迹与运动仿真程序中的轨迹一致，对其进行的正、逆运动学分析和轨迹规划是正确的，为行星采样机械臂的轨迹设计和分析提供了参考依据。

关键词： 采样机械臂; 运动学模型; 笛卡尔空间; 轨迹规划; 插补算法; Matlab仿真

中图分类号： TN101?34; TP242.3                文献标识码： A                       文章编号： 1004?373X（2020）14?0114?04

Research on trajectory planning of manipulator for sampling on planets

ZHANG Bingwei， QI Yongkang， QI Chao， SHEN Aqiang， ZHANG Peng

（College of Mechanical Engineering， Jiangsu University of Science and Technology， Zhenjiang 212003， China）

Abstract： In allusion to the trajectory planning of the manipulator for sampling on planets， the kinematics model of the manipulator is established according to the D?H method to perform the two kinds of trajectory planning of Cartesian space linear interpolation and circular interpolation， and its motion simulation is carried out with Matlab. A motion simulation program for the sampling manipulator is designed based on MFC and OpenGL. The experiment of trajectory planning of the linear interpolation and circular interpolation was conducted in combination with the motion controller. The experimental results show that the actual motion trajectory of the sampling manipulator is consistent with the trajectory in the motion simulation program， and the forward and inverse kinematics analysis and trajectory planning are correct， which provides a reference for the trajectory design and analysis of the manipulator for sampling on planets.

Keywords： sampling manipulator; trajectory planning; kinematical modeling; motion simulation; line interpolation; circular interpolation

0  引  言

人类在了解地球的同时也在不懈地探索外太空，近年来，中、美、俄等国家的航天机构陆续地向外太空发射了许多探测器。行星探测车是探测行星环境及其资源的重要工具，包括许多重要部件，而采样机械臂又是重中之重，用于对行星进行采样极其关键[1?3]。为此本文对实验室里的行星采样机械臂进行轨迹规划的研究，建立机械臂的数学模型、规划了机械臂的轨迹，然后设计程序进行运动仿真，结合控制器进行轨迹实验。结果表明，对机械臂进行的轨迹规划是符合预期的，为今后进一步的实验和分析提供了重要参考。

1  采样机械臂的结构

图1所示为本文行星采样机械臂的结构简图。它具有4个自由度，分别为腰、肩、肘和腕四个关节。腕关节的任务是挖掘行星表层的土壤，其余三个关节负责进行定位。

本文的采样机械臂主要应用在30～50 kg的微型漫游车上，所以其结构的设计有严格的标准，如表1所示为机械臂结构中各参数的设计要求。

2  采样机械臂的数学模型建立

如图2所示，依据经典的D?H法[4]定义采样机械臂的各关节坐标系。

表2为行星采样机械臂各连杆的4个运动学参数。

2.1  正运动学建模

根据机械臂的坐标变换法则，可得相邻坐标系之间的变换关系，如下[5]：

[i-1iT=cθi-sθi0              αi-1sθicαi-1cθicαi-1-sαi-1    -sαi-1disθisαi-1cθisαi-1cαi-1       cαi-1di     0                     0              0                 1]  （1）

式中：[cθi=cos θi;sθi=sin θi;cαi=cos αi;sαi=][sin αi]。

根据表2与式（1），可推导出机械臂末端执行器相对于基坐标系的变换矩阵：

[04T=01T12T23T34T     =c1c234-c1s234s1      c1（l3c23+c2l2）s1c234-s1s234-c1    s1（l3c23+c2l2）s234c234 0       l3s23+s2l2+d1    0              0           0                 1]    （2）

2.2  逆運动学建模

给定一个期望的末端位姿齐次变换可得对应的非线性方程[6]，通过代数求解法，可得采样机械臂的逆解，结果如下：

[θ1=atan2（px，py）θ2=atan2pz-d1，pxc1-atan2（l3s3，l3c3+l2）θ3=atan2（sin θ3，cos θ3）θ4=atan2（a31，a32）-θ2-θ3]  （3）

由逆解计算的结果可知，θ3具有正负两个解，如图3所示。就采样机械臂的末端执行机构需进行挖掘作业，且避免碰撞地面，故选择上肘位形式为最具有鲁棒性的解。

3  采样机械臂的轨迹规划

轨迹规划是通过预先设定期望的轨迹，并从此路径出发，从而生成相关变量的时间规律，如机械臂运动的位移、速度等[7]，本文对采样机械臂采用笛卡尔空间轨迹规划的方法[8?10]。

3.1  笛卡尔空间直线插补

假设需要机械臂的末端执行器从点A运动到点B，通过运动学的逆解，将所得的插值点坐标映射到关节空间中各关节所对应的角度。设从点A（0，3，2）运动到点B（7，0，5），插补点数N为30，可将具体的算法运用M语言编写，运用Matlab进行直线插补仿真。仿真结果如图4所示。

3.2  笛卡尔空间圆弧插补

假设三点P1，P2，P3不共线，通过逆运动学将需要插值点转换为对应的关节坐标值，对应点的关节角，驱动各个关节在关节空间的运动。假设机械臂的末端执行器在基坐标系下沿着圆弧路径从起点A（0，3，2），经过中间点B（7，0，5），到达终点C（5，0，7），这里插补总次数N取30。运用Matlab进行空间圆弧插补仿真，结果如图5所示[11]。

4  采样机械臂的运动仿真及轨迹规划实验

4.1  基于OpenGL的机械臂运动仿真

根据表2中各个连杆的具体参数，将采样机械臂各个值等比例缩小80倍，取d1=2.187 5 mm，l2=6.125 mm，l3=5.25 mm，通过OpenGL函数建立采样机械臂的简化三维模型，且根据轨迹算法得出各关节的角度变量，最后调用MFC框架类实现轨迹规划的仿真，程序界面如图6所示[12?14]。

图6的程序界面包括三维显示和运动控制两部分。右边的各个控制模块可以进行正、逆运动规划，手动输入关节角值和通信，从而控制左边界面中三维模型的各个关节的运动。本文仅对此机械臂进行相关的直线规划和圆弧规划试验。

4.2  搭建实验平台

以EMAC?200四轴运动控制器作为采样机械臂的下位机控制器，该控制器内部集成了Magellan运动处理芯片，除能精确地控制多种电机外还能支持多种程序运行方式，通过高速总线与通信端口、RMA等连接，且支持多种通信协议。

选用YZ?DSCD3605直流伺服驱动器，并且搭配AM26LS31芯片转换编码器的单端信号到差分信号以供控制器读取，采样机械臂的硬件系统结构如图7所示。

此控制系统采用上、下控制器两级分布式控制，上位机进行正、逆运动学的求解，管理系统等，下位机则负责控制轨迹的运动。本方案搭建的采样机械臂实验平台如图8所示。

4.3  轨迹规划实验

搭建采样机械臂的实验平台后，在仿真软件中的通信模块上，上位机电脑通过Ethernet以TCP协议与下位机实现连接。在轨迹规划模块输入相应的轨迹规划坐标，后台程序即可计算路径点在各个空间中的关节角度，通过控制器转化为关节脉冲以控制电机的运动。

在轨迹规划模块设置好规划轨迹的起始点及终点，进行仿真轨迹实验和硬件的通信连接，仿真界面及实验的结果对比如图9所示。

5  结  语

本文对一种行星采样机械臂进行轨迹规划的研究。首先建立采样机械臂运动学模型，接着利用Matlab工具进行笛卡尔空间直线插补和圆弧插补轨迹规划的仿真研究。然后根据OpenGL三维模型库和Visual Studio/MFC模块库开发了一套针对于机械臂的运动仿真程序。最后基于以上研究搭建了机械臂的硬件平台，用E?MAC200控制器作为机械臂的运动控制器，在笛卡尔空间进行直线插补和圆弧插补的轨迹规划实验。实验结果表明，各关节轨迹均能达到目标品质的要求，对此机械臂进行的轨迹规划是符合预期的，为今后进一步的实验和分析提供了重要参考。

参考文献

[1] 林杨挺.探索火星环境和生命[J].自然杂志，2016，38（1）：1?7.

[2] XU Lin， ZOU Yongliao， JIA Yingzhuo. China's planning for deep space exploration and lunar exploration before 2030 [J]. Chinese journal of space science， 2018， 38（5）： 591?592.

[3] 宁杭.六轮摇臂摆杆式行星探测车的结构设计与分析[D].镇江：江苏科技大学，2016.

[4] 蔡自兴，谢斌.机器人学[M].北京：清华大出版社，2015：35?49.

[5] 周霏，陈富林，沈金龙，等.基于Matlab的四自由度机械臂运动学仿真研究[J].机械制造与自动化，2016，45（1）：115?119.

[6] 冯瑶，公茂震.6R机器人笛卡尔空间轨迹规划中的逆运动学[J].自动化技术与应用，2018，37（6）：68?73.

[7] 付荣，居鹤华.基于粒子群优化的时间最优机械臂轨迹规划算法[J].信息与控制，2011，40（6）：802?808.

[8] FANG Yi， HU Jie， LIU Wenhai， et al. Smooth and time?optimal S?curve trajectory planning for automated robots and machines [J]. Mechanism and machine theory， 2019， 137： 127?153.

[9] XUAN Guantao， SHAO Yuanyuan. Reverse?driving trajectory planning and simulation of joint robot [J]. IFAC PapersOnLine， 2018， 51（17）： 384?388.

[10] WANG He， WANG Heng， HUANG Jiahai， et al. Smooth point?to?point trajectory planning for industrial robots with kinematical constraints based on high?order polynomial curve [J]. Mechanism and machine theory， 2019， 139： 284?293.

[11] 叶政.六自由度工业机器人轨迹规划算法研究与仿真[D].南京：南京航空航天大学，2017.

[12] 李泰国，李文新，王伟文，等.基于OpenGL空间机械臂三维重构可视化研究[J].计算机技术与发展，2018，28（1）：179?181.

[13] 王锐.OpenGL编程指南[M].北京：机械工业出版社，2017.

[14] 杜必强，陈佳楠.基于以太网通信和OpenGL的库卡机器人在线控制及仿真[J].现代制造技术与装备，2018（12）：48?50.