六自由度机械臂路径规划与优化

张小青，王长超，胡梦桃，陈凯元，徐胜清

（咸阳师范学院 物理与电子工程学院，陕西 咸阳）

引言

机械臂是模拟人手臂而构成的[1]。六自由度机械臂是关节型机械臂中的一种，目前广泛应用于工业自动化生产线上[2]，已成为人们研究的重点对象。

机械臂为机器人一种，其路径规划很重要[3]。合理规划运行轨迹不仅能使其以令人满意的性能执行任务，还能提高工作效率、减少能耗及延长运行寿命[4]。机器人轨迹规划常与轨迹优化同时进行，最小时间优化直接与机器人的生产效率相关，是最早被提出来的[5]，但在实际运行中若一味追求执行时间最短，易导致运行扭矩和加速度不连续，故不利于节能。对于六自由度拾放机械臂，平稳圆滑地拾放物品是其追求的首要目标。本文以门字形轨迹规划为基础，优化策略，使机械臂在抓取物体时动作圆滑，减少碰撞，为优化节能提供支持，具有重要意义。

1 六自由度机械臂及其控制

六自由度机械臂的结构主要包含转动基座、臂、肘、腕及抓取前端的钳子等部分，核心控制器件为STC89C51 单片机，从单片机输出六路PWM 脉冲分别控制机械臂的六个舵机，实现六自由度的操作，总体控制结构如图1 所示。

图1 总体控制结构

2 路径规划

2.1 坐标变换

依拾放特点，为研究方便，把OXYZ 坐标系下空间门字形路径变换到平面局部坐标UO'W 中。设拾放空间高度一致，从空间OXYZ 坐标系变换到平面坐标UO'W 系，仅需考虑坐标原点平移和沿X 轴旋转变换即可。设空间OXYZ 坐标中任意点为(x,y,z),将其变换到平面UO'W 坐标系后的坐标记为(u,w)，则有：

其中Q 为平面到空间坐标的变换矩阵,Q-1为空间到平面坐标的变换矩阵。

2.2 路径规划

2.2.1 路径规划目标

机械臂路径规划通过设计机械臂末端操作轨迹，完成给定拾放目标任务。为计算方便，此路径规划将在关节空间进行，利用运动学逆解公式求出各个关节变化角度，然后转换为对应的脉冲值进行分析计算。

为了实现六个舵机协调工作，在此依据经验把一个作业周期分成了多个动作步骤来完成，不同的动作路径对应不同的动作步骤，6 个数字舵机选型一致，其中舵机在-90°～+90°之间运动时对应的脉冲设定值范围为500～2 500，角度分辨率为0.09°。

以机械臂底座中心为坐标原点，拾取路径起始点为（-27,0,34），物件所在坐标为（26.5,0，0），机械臂从初始位置出发，移动到物件所在位置后对物件进行抓取，再将物体移回到起始点，完成拾放任务。

2.2.2 规划的舵机角度与脉冲值

首先把空间门字形路径变换成平面坐标，选取插值点，然后根据运动学及逆解模型，设计与门字形轨迹对应的各舵机转角。其中第1 舵机角度根据经验设计，主要实现爪子张开与闭合，第2 舵机角度在此次拾放路径规划中保持不变；第6 舵机主要用来控制机械臂底座旋转，其角度规划以被拾取物体与舵机底座在水平面相对位置夹角为参考设计；第3、第4 与第5舵机根据运动学逆解公式算出规划角度。据此，得到六个舵机的规划角度曲线（见图2），θ1～θ6分别表示各舵机（各关节）对应的运动角度。

图2 各舵机角度变化

根据舵机控制原理，需把以上规划的各舵机的角度转换成其控制所需的脉冲。舵机脉冲宽度与角度的关系可以表示为：

式中，θi表示第i 个舵机角度，i=1,2,…,6；hi表示第i 个舵机脉冲宽度值（μs）。当hi=1 500 μs 时，机械臂对应的舵机处于参考0°。

规划后各舵机脉冲宽度值见表1，编号表示完成目标任务的顺序，共有8 个动作过程；时间表示完成每个动作过程所需的时间。ID：1-ID：6 分别表示舵机1-舵机6 的脉冲设定值。以编号2 举例说明：舵机1、2设置为1 500 表示机械臂在编号2 过程中不动作，它们配合舵机3、4、5 完成编号2 过程中所要完成的动作。

表1 舵机脉冲设定值

2.3 路径规划实验

按照机械臂执行路径规划实验，得到的空间门字形拾放路径。执行规划后的门字形拾放步骤，实物操作记录如图3 所示，规划的门字形路径如图4(a)所示。

图3 机械臂拾放路径实验操作

图4 规划与优化的门字形拾放路径

3 路径优化

根据以上规划的路径，机械臂基本能实现抓取物品的任务，但在实验验证的过程中，发现仍存在两处可以继续优化的问题。

首先，机械臂爪子在初始位置执行抓取动作时，爪子易触及桌面，甚至还会有向下的趋势，具体可从图4(a)右下曲线看出，严重的会将机械臂自身底座撑起，这对准确抓取物品是不利的，若长期如此动作会影响舵机的使用寿命。针对此问题，进行了优化分析并提出了相应的优化措施。

优化分析：假设一个物体速度为v，质量为m，那么它具有的动量M=mv。从实验中明显能看出，机械臂爪子在触碰实验台面时仍有下冲趋势，说明其动量过大。若使机械臂不触碰实验台面，需减少运动过程对应动量值，在其它条件不变的情况下，需减小运行速度v。在规划路径一定的情况下，根据速度与路径成反比的关系，也就是需要增大对应运行步骤的执行时间。

另外，上述规划的拾放路径是直角拐弯，不圆滑，在实验中通过听取机械臂动作时的声音，也能察觉出其运行不连贯的问题，这将不利于提高机械臂的控制精度。

优化思路：用圆滑的弧线代替直角过渡，增加路径规划插值点，分析机械臂运动学模型，根据推导出的各个关节驱动电机角度的逆解公式，得到相应的关节舵机驱动角度，实现对拾放路径的优化。将空间轨迹先变换到平面坐标，依据机械臂的运动学模型逆解出对应关节的角度值，再根据舵机控制原理将各个关节轨迹对应的转角转换成了控制所需的脉冲，在上位机软件中输入各舵机每一步的脉冲值，运行并观察运动路径形状。具体的执行过程中，对圆滑过渡的门字形轨迹多增加规划点，也就是扩充执行步骤，以机械臂运行学模型为基础，用仿真软件对其轨迹进行优化，结合仿真结果和运动实际情况微调上位机舵机的参数值，得到优化后的实验结果。

经以上分析与实践验证，优化后的拾放路径轨迹如图4(b)所示。

图4(b)的轨迹表明门字形的直角过渡已变得相对圆滑，图5 为优化前后各坐标的操作值。从图5 中能看出，优化后Z 轴坐标明显变缓，表明在高处运行比较平缓，可减少物体脱落概率，有利于顺利完成任务。而且，在拾取物体作用点，爪子不再触碰实验台面，爪子抓取物体的冲量明显较少，改善了系统运行的性能。在实验过程中，机械臂在运行过程中摩擦声音明显减少，使得设备运行更加顺畅与稳定。需要说明的是，图5 中横坐标时间为归一化时间，在接近物体所在位置时，虽然Z 轴与Y 轴方向的坐标变化较快，但其运行速度已控制在合理范围内，优化后的系统未发生由于动量过大触及实验台面的情况。

图5 优化前后的规划坐标

4 结论

针对六自由度机械臂的路径规划与优化进行了研究，以机械臂的运动学模型为基础，结合单片机控制与舵机工作原理，规划了机械臂拾放物体的门字形轨迹，给出了相应的关节角度值及其舵机控制所需的脉冲值。再根据规划的结果，从减少抓取动量及圆滑过渡两个方面对规划的路径分别进行了优化，通过实践测试，验证了所设计方案的有效性。优化后的拾放路径更加圆滑，动作更加协调，更有利于提高机械臂的控制精度，为机械臂在实际生产中的应用提供一定参考。