串联型机械臂直线轨迹规划Bresenham算法应用与改进*

张 岩 过仕安 李 争 安国庆 薛智宏 盖祥虎

(①河北科技大学电气工程学院，河北 石家庄 050018；②陆军工程大学石家庄校区电磁环境效应国家级重点实验室，河北 石家庄 050003)

串联型机械臂是广泛使用的一种机械臂类型。而在机械臂的轨迹规划上，令机械臂末端在空间上沿给定直线运动，这样的直线规划是最基本的一种轨迹规划。仅采用直线解析式规划时，由于是任意直线，斜率往往不是常数，在微控制器中做除法运算会出现误差，这样在微控制器中累计叠加容易使轨迹出现偏差，并且运算量较大。

为克服上述直线规划中出现的一些缺陷，本文采用在计算机绘图中常用的Bresenham直线绘制算法规划机械臂末端运动。该算法能够避免除法运算，具有较好的精度，同时能够减小微控制器的运算量[1]。

本文依托设计的用于搬运用途的四轴机械臂，对该机械臂建立D-H参数的数学模型，在该模型的基础上对机械臂做运动学分析求解；然后分析Bresenham直线插补算法在该机械臂的应用过程和对机械臂的影响，并针对其中的不良影响做出步数补偿的优化方案。

1 机械臂的数学模型和运动学分析

1.1 机械臂的数学模型

常用于描述串联型机械臂的数学模型有D-H模型、改进的D-H模型和POE模型[2-3]。其中改进的D-H模型物理意义表示更为明确，能够清晰表述类似本系统所设计的串联型机械臂的位姿，因此本系统采用该模型作为机械臂的位姿描述方式和控制基础。

改进的D-H模型是通过对每一个构件描述其参数而建立。对于一个构件k的D-H模型使用4个参数来描述：连杆长度lk、扭角αk、关节偏距ck和关节转角θk[4-7]。本系统设计的机械臂为关节可转动的类型，以关节转角作为运动参数。机械臂的模型示意图如图1所示，其中标注了所需的D-H参数位置和各个关节的坐标系。在本系统的设计中各构件的D-H参数如表1所示，其中关节转角为初始值。

表1 机械臂构件的D-H参数表

1.2 基于位姿矩阵的机械臂的正运动学求解

机械臂的运动学求解是使用已建立的数学模型，对控制量：如关节的旋转角度，和运动学中的变量：如位置、速度、加速度等相互转换。其中，由控制量转换为运动学变量的过程为正运动学求解；由运动学变量转换为控制量的过程为逆运动学求解[8]。逆运动学求解是本文设计实现中的一个难点。

机械臂运动参数的改变会导致机械臂各个关节坐标系发生变化，因此可以通过描述关节坐标系变化求解运动学变量，即求解正运动学。

两个不同的坐标系Oxyz和O′x′y′z′，描述点O′相对于点O的变化可用向量OO′来描述，称为位置向量r；而坐标系的变化除了坐标系原点发生位移，坐标系的各个坐标轴也产生了旋转，通过姿态矩阵R描述坐标系旋转变化。为方便矩阵的推导运算，将姿态矩阵和位置向量合并，并增加一行单位行向量，构成4×4的一个矩阵，称为位姿矩阵M，如式(1)所示。

(1)

在机械臂中当构件k的运动参数关节转角θk发生变化的时候，通过描述两个关节坐标系位姿矩阵相对变化，并通过最终求解的位姿矩阵获得机械臂转动后的位姿信息。对于构件k的位姿矩阵如式(2)所示。[9]

M=

(2)

对于本文设计的四轴机械臂，由于每个连杆机构都是首尾相连的串联型构件，因此在知道各个关节的D-H参数后，即能推导出机械臂末端执行器的位姿[10]，对于本系统的末端位姿推导公式如式(3)所示。

(3)

Mk代表构件k的相邻两个关节坐标系变化的位姿矩阵，从定义式(2)中可以看出既与机械臂的固有参数有关，又与机械臂的运动参数有关。正运动学求解结果即为式(3)。可从矩阵结果的最后一列列向量中位置向量得到我们所需要的机械臂末端执行器的位置信息。

1.3 基于简化自由度的平面解析算法的机械臂的逆运动学求解

本系统中的机械臂逆运动学求解是在给定的目标末端执行器位置下求解各个关节舵机的旋转角度。在文献[2,11]中，提及了多种解算的方法，包括几何法[12]、解析法、数值法和人工智能法[13]。对于除几何法以外的三种方法，大多都是运用成熟的开源运算库实现完成的，这些运算库一般需要系统在上位机上实现，增加了控制成本。而普通的平面几何法求解结果对于具有空间运动 特性的机械臂解算具有一定的局限性。针对本文的机械臂结构，设计了一种简化自由度的平面解析法，能够实现以相对较少的运算量使执行器末端到达预定的位置，同时也便于将程序移植入微控制器中。

首先，采用合适的坐标系。由于本系统是以底座为中心旋转的结构，采用柱坐标系为基础坐标系。因此，当目标位置为空间坐标系位置时，须以底座原点为中心，将目标的空间直角坐标转换为柱坐标系。对于柱坐标系中另外两个坐标轴构成的平面直角坐标系，可采用平面几何法，因为除底座外的其他舵机和构件都是在同一平面内运动。但在该平面内仍存在3个连杆构件，采用几何法求解时对二连杆求解十分直观，对三连杆求解的过程较为冗杂[14]。根据本机械臂系统的实际用途，为平稳夹持物体。因此最末端的连杆构件应时刻保持水平，这样就可以简化自由度，将最后一个连杆构件添加固定约束，先对目标位置作修正后再对二连杆做平面几何法求解，即可得到目标解。简化自由度的平面解析法运算流程如图2所示。

2 使用Bresenham算法的直线轨迹规划

2.1 Bresenham算法介绍

空间中的最短路径便是空间中起点与终点直接相连的直线，容易得出直接通过求解空间直线的解析式，使机械臂末端沿解析式轨迹运动。但解析式中直线的斜率往往不是整数，而微控制器对于除法运算的求解存在除不尽时结果不精确的缺陷。在微控制器中利用解析式求解空间直线上各个点的位置时，会不断累加这些不精确的斜率，使得实际运行轨迹与既定运行轨迹产生偏差。Bresenham直线插补算法很好地规避了这一缺陷。Bresenham直线插补算法原本是用在图像绘制上的一种算法，使用像素拟合出解析式规定的直线形状。这种算法是通过比较距离大小的方式规避除法运算，从而求解得到精确的结果[15]。图3中圆离散点和像素块表示了该算法对图中直线的拟合效果。

由图4说明Bresenham算法思想。直线的起始点为(x1,y1)，终止点为(x2,y2)，拟合这两点之间连成的直线。直线解析式为

y=kx+b

(4)

假设Δx>Δy，即斜率k∈(0,1)，求解图中D点(xi+1,yi+1)的拟合点，应选取D1点(xi+1,yi)还是D2点(xi+1,yi+1)。通过比较D1、D2，选取与D最近的点即为最佳拟合点，即比较图4中d1与d2的大小判断：若d1d2则选择D2点；若d1=d2则可选任意一点，这里固定选择D2点。距离d1和d2的表达式为

(5)

代入直线解析式(4)为

(6)

设像素长度和宽度为单位长度1，对距离d1和d2比较作差得

d1-d2=(kxi+1+b-yi)-(yi+1-kxi=1-b)

=2kxi+1+2b-yi-yi+1

=2kxi+1+2b-yi-1

(7)

差值的表达式中仍然存在斜率k，斜率k的表达式为

(8)

将k代入式(7)中，并且等式两边同时乘以Δx

Δx(d1-d2)=2Δyxi+1-2Δxyi+Δx(2b-1)

(9)

新定义一个变量p，令pi定义为

pi=2Δx(d1-d2)=2Δyxi+1-2Δxyi+Δx(2b-1)

(10)

则pi+1为

pi+1=2Δx(d1-d2)=2Δyxi+2-2Δxyi+1+Δx(2b-1)

(11)

将式(11)与式(10)相减，可得

pi+1-pi=2Δy(xi+2-xi+1)-2Δx(yi+1-yi)

=2Δy-2Δx(yi+1-yi)

(12)

当xi+1处d1>d2时，则应取yi+1=yi+1，此时pi>0，由此可得

pi+1=2Δy-2Δx(yi+1-yi)+pi+2(Δy-Δx)

(13)

而当xi+1处d1

pi+1=2Δy-2Δx(yi+1-yi)+pi+2Δy

(14)

当xi+1处d1=d2时，可任取yi+1值是否在原值基础上增长，按照上文选取点的约定，取yi+1=yi+1，因此式(13)适用于pi≥0的情况。

由此，根据式(13)和式(14)，可以在已知pi的结果时，可以顺序推出后续所有点p的结果；而已知p的结果，就能判断d1与d2的大小，进而判断出点的选取结果[16]。而此时还要解决的问题是p1的值如何求出，按照p的定义

p1=2Δx(d1-d2)

=2Δyx2-2Δxy1+Δx(2b-1)

=2Δy(x1+1)-2Δxy1+Δx(2b-1)

(15)

由x1点处直线解析式

(16)

等式两侧同时乘以Δx，整理可得

Δxy1=Δyx1+Δxb

(17)

将式(17)代入式(15)

p1=2Δyx1+2Δy-2Δxy1+Δx(2b-1)

=2Δyx1+2Δy-2Δxy1+2bΔx+2bΔx-Δx

=2Δy-Δx

(18)

由此可以确定初始值p1，进而确定所有取值的位置。

上述算法过程的过程描述中可以看出，算法的思路是x方向每移动一个单位长度，判断y方向是否前进插值，判断的依据是解析式上的点与两个待插值点之间的距离，取最近点。上述算法只描述了Δx>Δy的情况；若Δx<Δy，则应在y方向每移动一个单位长度，判断x方向是否前进插值，判断思路与上述对称；若Δx=Δy，则按照上述任意一种情况计算即可。因此在程序设计时应有两个分支，先判断Δx与Δy的大小[17]。该算法通过距离比较判断插值点位置，避免了在微控制器中不精确的斜率累加带来的误差，提高了输出结果的精确度。

将上述算法再扩展到三维空间中，将空间坐标分解为两个平面上的坐标去判断，则应先判断Δx、Δy与Δz的大小，取最大值的坐标为单位移动方向，每移动一步判断另外两个坐标是否前进插值[18]。因此空间直线插补在程序设计时应有3个分支。完整的程序流程如图5所示。

2.2 Bresenham算法直线规划仿真校验

使用MATLAB Robotics Toolbox工具，通过运用Bresenham直线插补算法，规划机械臂末端在空间中沿直线轨迹运行。通过仿真校验，获得使用该算法下的机械臂的相关运动学参数，校验算法的可靠性。

仿真程序的运行流程如图6所示[19]。

使用MATLAB Robotics Toolbox工具仿真的机械臂模型如图7所示，仿真运行结果如图8所示。从图中可以看出机械臂末端能够在空间中沿直线轨迹运行。在图像中，可以看出直线轨迹有锯齿状，这是直线插值较为稀疏的缘故导致的。在程序代码中，设立了一个变量控制插值密度，称为轨迹精细度的放大倍数。放大倍数越高，步数越多，轨迹越平滑。

从图8a中可以看出，使用该Bresenham算法可以规划机械臂末端轨迹为直线，验证了算法能够规划机械臂末端轨迹为直线；图8b是形成直线轨迹时机械臂各关节舵机的旋转角度；图8c是根据各关节舵机的旋转角度计算出的旋转角速度，从图像可以看出各关节舵机起动时速度要求很高，随后趋缓。

2.3 Bresenham算法的步数补偿优化

从仿真结果中，注意到了该算法存在起动转速突变的缺点。为了缓解这一不足，本文提出一种变步长的步数补偿优化方案，对该算法在机械臂轨迹规划上的运用进行优化。

在阐述Bresenham直线插补算法时提到，该算法的大致过程是差值最大的方向每移动一步，判断另一个方向是否需要移动，形成插值点。从实验结果的图像中可以看出，起动时电机的转速较高，易形成冲击。因此对起动时的相应步数内进行补偿，插补更多的步数，来减缓起动转速和起动冲击。

接下来设计一个较为简易的步数补偿优化。从实验结果的图像中可以看出，前1/4的步数内电机的转速较高，因此对前1/4的步数内进行补偿，插补更多的步数。改进后的程序流程如图9所示。虚线框所标的流程是相较于之前仿真程序增加的流程。

使用MATLAB Robotics Toolbox工具仿真校验的结果如图10所示，图像中选取了肩关节舵机的优化前后情况进行对比。从图10a中可以看出，算法优化前后运行轨迹一致，没有影响算法使用的目的；图10b中可以看出由于步数插补使得优化后的旋转角度变化滞后于优化前的旋转角度曲线；图10c中可以看出起动时转速要求降低了近1/2，但当步数补偿结束后转速发生了突变，随后的变化趋势与补偿前一致。

使用直线解析式规划轨迹和Bresenham算法及其补偿优化算法在运行测试在运行时间、末端执行器误差和起动角速度比较如表2所示。该测试是在生成同样插值点数量条件下多次测试的结果，起动角速度选取了其中一个关节。

表2 算法运行测试结果

根据算法的仿真实验结果，与直线解析式规划轨迹方法比较，得出Bresenham算法的一些优点和缺点。

使用Bresenham算法规划结果的优点有：

(1)空间上路径为直线，距离最短，用时少；

(2)算法较简洁，运算量较小；

(3)算法规避了除法运算，精确度高。

使用Bresenham算法规划结果的缺点有：

(1)起动或制动时要求电机转速突变，对电机冲击大；

(2)插补步数稀疏时震动较大。

因此，该算法适合应用于对机械臂的空间路径要求非常严格的场合，并且希望时间尽可能缩短，选择Bresenham直线插补算法较为合适。

从仿真结果中可以看出简单的补偿优化前后算法运行轨迹完全一致，起动时的转速要求降低了，得到了预期的优化效果。但是简单的补偿算法带来的问题也不可忽视：一是补偿带来电机起动减缓是以延长起动时间为代价，因此该算法运行用时少的优点被弱化了；二是简单补偿前后存在转速差，会对电机造成二次冲击；三是起动步数增多可能会造成机械臂的抖动增加。因此实际使用时需考虑这些会对机械臂系统造成的影响。还可以考虑更复杂的变步数补偿优化方法，动态地补偿起动步数，优化算法的起动过程。

3 结语

本文针对机械臂末端轨迹规划问题中的直线轨迹规划，在采用直线轨迹规划时使用微处理器处理斜率中除法运算不精确、运算量大的问题，引入Bresenham算法解决。建立机械臂的D-H参数数学模型，在该模型的基础上根据模型参数设计的基于位姿矩阵的正运动学求解和基于简化自由度的平面解析法的逆运动学求解，作为算法输出机械臂指定位置时解算为主控制器控制舵机指令的基础。使用Bresenham算法规划机械臂的直线轨迹运动。Bresenham算法以比较待插值点距离的方式规避了除法运算，且运算量较小。而Bresenham算法规划中存在对起动时起动和制动时转速可能突变的缺陷，本文采用步数补偿优化加以解决。使用步数补偿优化有效降低了起动时的转速要求。但也要注意使用简单的步数补偿优化时带来的时间延缓、二次冲击等附加影响，实际需权衡使用。