一种双机械臂的避障运动轨迹规划方法

付 青，王 直

（江苏科技大学 电子与信息学院，江苏 镇江 212003）

一种双机械臂的避障运动轨迹规划方法

付 青，王 直

（江苏科技大学 电子与信息学院，江苏 镇江 212003）

本文讨论一种应用于血凝检测系统的两自由度的双机械臂的运动轨迹规划的算法优化问题。机械臂是该系统的重要组成部分，其工作状态直接关系到整个系统运行的稳定和可靠。文中采用DH矩阵对双机械臂的位置和姿态进行数学建模，进而通过基于笛卡尔空间的圆弧插补法进行路径规划，得到更加平滑机械臂运动轨迹。最后，使用优化的人工势场法，使双机械臂在无碰撞的情况下顺利达到目标点。MATLAB仿真实验证明改进的算法能够较好的弥补传统人工势场法的缺陷，提升机械臂运动轨迹规划效果。

D-H矩阵；笛卡尔空间；圆弧插补法；人工势场法

机械臂的运动轨迹是指通过给定的路径的起点与终点[1]，以及机构本身或者机构运行所存在的约束条件，求出每个关节的位移S，速度V，加速a的完整过程。

文中采用笛卡尔空间的圆弧插值法使得机械臂运动平滑。笛卡尔空间的规划与机械臂目标点的坐标位姿函数相关，必须通过机械臂的逆向运动学求解，如此可获取各个关节变量的函数。笛卡尔空间的轨迹规划方法中的圆弧插值法适用于所研究主从机械臂运动状态，主从机械臂最基本的要求是在起点与目标点的速度为零，两机械臂运行平滑，时间和速度保持最优的情况下，主从机械臂同步运行且不发生碰撞的现象[2]。

1 双机械臂的位姿描述

讨论双机械臂的运动状态与操作，应先建立对应的数学模型，其应包括空间中某个点的位置和姿态等等。需要多个坐标系，以便于建立机械臂连杆之间的联系。首先确定机械臂连杆与末端执行器位姿，其次，将两机械臂的基座固定，将其设为基座标，根据机械臂的连杆和关节两者的联系，提出许多适用的运动学建模方法[3-4]。文中采用D-H矩阵参数法建模。

1.1 运动模型简化

选择D-H矩阵来设置双机械臂的坐标系，Z轴的方向与连杆方向一致，X轴垂直两连杆的公垂线，Y轴的方向由右手法则确定。

图1 双机械臂机构及连杆图

图2 双臂轨迹示意图

为了便于区分与分析，将机械臂分为主机械臂和从机械臂，因两机械臂对称，只需要计算出其中一个机械臂的位姿，另一机械臂就可相应得出。从上图中可以列出相应的表格。

1.2 机械臂的正运动学分析与逆运动学分

由图1和表格1，主机械臂的各连杆的位姿矩阵可表示为：

表1 机构关节角

在机械臂的运行设计中，已知机械臂的初始位置，为了使得机械臂达到期望位姿，那么需要知道达到指定位姿各个关节需要运动的值，根据下列逆运动学方程[5-7]，计算关节角数值。

2 双机械臂的协调工作

机械臂的轨迹规划指机械臂的运行过程中的路径规划，也就是在机械臂从起始点运动到目标点，机械臂运行过程中受到某些限制（位移，速度，加速度），简而言之，双机械臂任务就是，两个机械臂在共同运行的过程中达到目标点完成某项任务，关节空间法与笛卡尔空间为轨迹规划的两种方法，关节空间法计算简洁明了，仅仅能显示机械臂的开端和结尾，不能直观描述机械臂到达终端过程中运动轨迹。而笛卡尔坐标系中的路径规划，就是在笛卡尔坐标系中找出一条与已知路径相似运动轨迹。

2.1 笛卡尔空间的路径点规划

双机械臂共同工作的过程中，机械臂的工作任务是用机械臂的末端执行器的笛卡尔空间坐标节点定义，节点为终端执行器位姿的齐次变换矩阵。用笛卡尔空间[8]的方法规划两机械臂运动，在笛卡尔坐标系中运算进行大部分的计算及优化。

笛卡尔空间的轨迹规划中，为使机械臂按照理想方向运行，每个路经点的位姿必须可知。这里就需要算出机械臂在路径各点的变换矩阵。笛卡尔空间的轨迹规划以其拥有直观明确的运行轨迹的优势而得到广泛应用，笛卡尔空间是轨迹规划的重要的方法，它的求法如下：

1）通过机械臂的各个关节角度和机械臂正向运动学计算公式求得[8-9]相应的位姿，在上文中已经解决。

2）对机械臂的运动轨迹进行笛卡尔空间插补：直线插补法与圆弧插补法等等，形成笛卡尔空间路径点。

2.2 血凝仪的轨迹要求

笛卡尔空间轨迹规划方法有：直线插补法和圆弧插补法，直线插补法即已知点与点之间运行轨迹为直线，圆弧插补法的运行轨迹为圆弧。从图2可以看出双机械臂的路线为：

1）血凝仪两机械臂的任务是提供四种试剂和N种血液样本进行混合。

2）主臂的任务是先从（A→O）吸取试剂，吸取试剂后，主臂在上升的过程中同时进行旋转（O→E），其运行放如上图。

3）主臂完成第一种试剂的进样任务之后，主臂按照反方向运行，进行进样针清洗，在按照第二个步骤的顺序，进行第二种试剂的取样。

4）从臂的运行过程与主臂的运行过程相似，可根据主臂的方向得出。

2.3 圆弧插补法

圆弧插补法一般为两种，在平面范围内的平面圆弧插补，另一种为三维空间范围内的空间圆弧插补，文中所用的为三维空间的圆弧插补。平面圆弧[10]指代圆弧平面与基座标系的三大平面之一重叠，空间圆弧为三维空间任一平面内的圆弧。

空间圆弧插补法可分为三步执行：

1）将三维问题转换成两维，求出圆弧所在平面

2）利用二维平面插补计算插补点对应坐标（Xi+1，Yi+1）。

3）把该点的坐标值转变为基础坐标系下的值，

通过非一条直线的三点P1，P2，P3可确定一个圆及三点间的圆弧，它的圆心OR半径R，基础坐标平面和圆弧所在平面的交线分别为AB、BC、CA。

建立圆弧平面插补坐标系，使ORXRYRZR与圆心O重叠，令ORXRYRZR平面为圆弧所在平面，且保持ZR为外法线方向，因此，三维问题就转换成平面问题[11-12]，用上述所讲的平面圆弧插补的方法即可。在MATLAB中，编程实现机械臂笛卡尔圆弧插补轨迹规划为，并实现动画仿真，结果如图3、4所示。

图3 使用笛卡尔轨迹规划生成轨迹曲线图

图4 轨迹规划直角坐标系下的曲线图

3 双机械臂的协调工作

在两机械臂进行作业的路径中，主机械臂的运动路径可通过笛卡尔空间圆弧插补法得出，那么在两机械臂同时运行期间，相当于实验的过程中主机械臂的位置状态已经人为确定，因此，从臂的运行状态是在主机械臂已知的情况下运行的，主机械臂相当于从机械臂的障碍物，这个障碍物的虽然是可移动的，但是其运动路径是已知的，因此可以使用人工势场法规划两机械臂的路径，人工势场的简洁性和高效性试用于机械臂路径规划领域。

3.1 人工势场法的优化

在研究机械臂在人工势场中运行的过程中，如果当机械臂逼近终点的同时也接近障碍物，而斥力比引力大的时候，那么机械臂就很难运行到目标位置[15-16]，因此为了机械臂能够到达指定位置，必须重定义斥力函数，使其无论在何种状况下都能达到终点。

3.2 新的斥力函数

因为机械臂不能达到指定位置的情况，假设当机械臂向终点靠近时，斥力大小趋近于0，这样可以解决达不到终点的问题。因此我们将机械臂与终点的相对位移也考虑在内，新斥力场公式为

上式中的（X-Xgoal）为机械臂与目标点之间的距离，q 代指物体当前的位置，qg指终端位置，ρ（q）=||qqc||，qc指到障碍物最近的位姿点。Δρ（q）指 qc→q 的单位向量。机械臂运动方向受引力和斥力的合力Fsum控制，其合力使得机械臂逼近障碍物，其中，可以将斥力可看成是两个分力Ferp1，Ferp2的合力：

用传统人工势场法与优化的人工势场法公式，分别对机械臂的运动轨迹的进行仿真如图6、7所示。

图5 机械臂在优化人工势场中的合力分析

图6 传统人工势场下机械臂的轨迹图

图7 新人工势场下机械臂的轨迹图

4 结束语

双机械臂的避障路径规划，其中两机械臂都为两自由度，具有平移和旋转的两种运动方式，两机械臂在运动过程中，部分路径可能发生碰撞，因此需要对路径进行详细的规划，防止发生碰撞。

1）首先应用D-H矩阵建立双机械臂的位姿的运动模型，应用笛卡尔空间的圆弧插补法，使得机械臂在笛卡尔空间中的运动轨迹平滑。

2）通过对主机械臂的路径规划，可以预知主机械臂的路径，将其作为从机械臂运动的已知障碍。

3）人工势场法是避障路径规划的方法之一，但在有些情况中存在目标达不到的情况，这里对传统的人工势场法进行深入分析，优化，使其满足双机械臂避障的路径规划。

[1]闫华文.一种医用光电检测前置放大电路设计[J].电子产品世界，2014（2）:34-36.

[2]姜铭，易红.李鹭杨.RGRR-I构造混联6R机器人[J].2010，46（2）:1-36.

[3]李宪，郭永存，张军华.模块化六自由度机械臂逆运动学解算与验证[J].计算机仿真，2012，29（3）:239-243.

[4]刘极峰.丁继斌.机器人技术基础[M].2版.北京：高等教育出版社，2012.

[5]黄祥.基于最小区域发的圆柱度几何误差评定[J].组合机床与自动化加工技，2013（6）:27-29.

[6]田桂，谢建，陈永宁.大型导弹起竖过程最优时间轨迹规划研究[J].计算机仿真，2012，29（1）:41-44.

[7]王会方，朱世强，吴文祥.基于INSGA-算法的机械手多目标轨迹规划[J].浙江大学学报（工学版），2012，46（4）:622-628.

[8]李小霞，汪木兰，刘坤.基于B样条的机械手关节空间平滑轨迹规划[J].组合加床与自动化加工技术，2013（12）:18-20.

[9]王东霞，温秀兰，汪凤林.基于微分进化算法的平面误差评定[J].计算机真，2012，29（1）:41-44.

[10]Zhang Xiaochao，Jiang Xiangqian，Scott P J.A reliable method of minimum zone evaluation of cylindricity and conicity from coordinate measurement data[J].Precision Engineering，2011，35（3）:484-489.

[11]Wisnu Aribowo，Kazuhihko Terashima.Cubic Spine Trajectory Planning and Vibration Suppression of Semiconductor Wafer Transfer Robot Arm [J].Automation Technology，2014，8（2）:265-274.

[12]王立刚，建天成.基于光电二极管检测电路的噪声分析与电路设计[J].大庆石油学院学报，2009，33（2）：88-92.

[13]付文羽.硅光电二极管的线性度及信噪比分析[J].半导体光电，2003，24（4）：267-269.

[14]陈强，黄声享.小波去噪效果评价的另一指标[J].测绘信息与工程，2008，33（5）:13.

[15]MATLAB技术联盟，孔玲军.MATLAB小波分析超级学习手册[M].北京：人民邮电出版社，2014:297-308.

An obstacle avoidance trajectory planning for double manipulators

FU Qing，WANG Zhi
（School of Electrical and Information，Jiangsu University of Science and Technology，Zhenjiang 212003，China）

This article discusses an improved algorithm of trajectory planning for double 2-Dof-Freedom manipulators which apply to the cruor measurement system.With people pay more attention to the cardiovascular disease and healthy，the cruor measurement system becomes one of the hot spots in the research of automatic medical detection.As well know，manipulator is a vital component of the automatic system.The working state of the manipulator is related to the stability and reliability of the system directly.This paper provides a mathematical model for position and orientation of the double manipulators system by using D-H Transforming Matrix，and a path planning base on the method of Circular Interpolation in Cartesian Space，which makes the trajectory of the Manipulator more smooth.The traditional algorithm of Artificial Potential Field has a defect that can't reach the target point near the obstacle.In order to solve this problem，a new repulsive function was created which taking the relative distance between the double manipulators and the target into consideration to improve the traditional Artificial Potential Field，and making the manipulators reach the target points without collision.Through simulation of MATLAB and experiment，it was proved that the improved algorithm can make up for the deficiency of traditional Artificial Potential Field，and can improve the efficiency of trajectory planning and movement of the double 2-Dof-Freedom manipulators.

D-H transforming matrix； cartesianspace； method of circular interpolation； artificial potential field

TN02

A

1674－6236（2017）16-0068-05

2016-06-22稿件编号：201606162

付 青（1990—），女，江苏连云港人，硕士研究生。研究方向：工业控制。