李克讷 王温鑫 胡旭初 马玉如 贺之祥 袁伟明 叶洪涛
摘 要：针对目前机械臂运动规划与控制系统成本较高，机器人技术难以普及等问题，选择8位单片机实现机械臂运动规划算法;为提高机械臂执行任务的实时性，提出了一种基于Simulink的机械臂运动规划硬件实现模型。首先，对机械臂进行运动学分析，建立数学模型;其次，将机械臂的运动规划问题转化成二次规划问题，同时引入误差补偿函数以减小机械臂运动过程中产生的位置误差;在Arduino Mega2560平台下，利用E47数值算法对机械臂运动规划问题进行求解;利用Matlab对机械臂执行圆形和正方形轨迹跟踪任务进行仿真实验;最后，通过硬件实现模型对机械臂执行正方形轨迹进行实物验证。结果表明：8位单片机对机械臂运动规划算法的实现具有可行性和可适用性，在硬件实现模型中具有有效性，为后续更好地研究机械臂运动规划提供了参考。
关键词：8位单片机;机械臂运动规划;E47数值算法;误差补偿;硬件实现模型
中图分类号：TP241;TP183 DOI：10.16375/j.cnki.cn45-1395/t.2022.02.011
0 引言
一般机器人运动规划与控制系统成本较高，不利于开展机器人实操教学。与其他控制器相比，8位单片机价格低，通用性强，运算精度能满足研究者的基本需求，因此，将其应用于机器人运动规划与控制系统设计，可以降低学习成本，有利于机器人技术的普及和发展。冗余度机械臂运动较为灵活，具有避障、躲避关节极限[1-2]等能力，是很多教师在教学方面的研究对象。在冗余度机械臂的运动规划过程中，逆运动学求解是一个不容忽视的问题。目前，求解逆运动学问题的方法很多，其中的伪逆法[3-4]应用广泛，但该方法计算量大，求逆复杂且存在矩阵奇异的问题。为提高计算效率，学者们提出将逆运动学问题转化为二次规划问题（QP）进行求解[5-7]。二次规划问题可与线性变分不等式（LVI）进行等价转换，然后通过递归神经网络或者数值算法对LVI进行求解[8-10]，最终得到QP的最优解。在实际应用中机械臂通过8位单片机对其电机或舵机进行控制，但神经网络求解器需要求解常微分方程，不便于C/C++等进行程序的编程。因此，对于实际机械臂的逆运动学求解，本文采用了文献[11]所提出的核心方程，即E47数值算法[12]。该方法在求解机械臂的逆运动学问题时，能够降低计算复杂性，适用于低性能的8位单片机系统。由于8位单片机对算法运算的实时性不理想，使得机械臂在执行任务时由于操作时间间隔长，不能连续完成指定任务，有可能会因为外界原因导致机械臂末端位置产生误差，进而影响实验结果，为此提出了一种基于Simulink的机械臂运动规划硬件实现模型，以达到实时控制机械臂的目的。
综上所述，本文利用算法移植在8位单片机系统上对机械臂运动规划算法的实现进行研究。首先，利用D-H参数法对机械臂进行数学模型的建立与工作空间的求解。其次，以机械臂运动过程中实时产生的位置误差构造误差补偿函数，并基于E47数值算法设计机械臂的运动规划方案，减少机械臂末端执行器的位置误差。然后，利用Arduino单片机系统对该算法进行运算，所得结果通过MATLAB软件进行仿真分析。最后，利用硬件实现模型对六自由度机械臂执行期望任务进行实物验证，结果表明，8位单片机对机械臂运动规划算法的实现具有可行性和有效性。
1 机械臂运动学建模与工作空间的求解
1.1 机械臂运动学建模
目前建立机器人模型的方法主要有D-H法[13]和旋量法，其中D-H法是一种对机器人关节和连杆进行运动学建模的方法，该方法建模简单，具有通用性，被广泛用于机器人的运动学建模。为验证机械臂运动规划算法在8位单片机上的实现结果，选用图1所示的六自由度机械臂为实验平台，并将铅笔作为末端执行器以跟踪绘制期望轨迹。为便于将末端执行器进行统一建模，采用标准D-H法建立如图2所示的六自由度机械臂坐标系。机械臂D-H参数如表1所示。
根据表1中的参数，采用机器人正向运动学方程推导出机械臂末端位姿矩阵。机械臂末端相对于连杆[i-1]坐标系的位置可通过式（1）得到：
[i-1Tn=i-1TiiTi+1…n-1Tn，i=1，2，…，n] ， （1）
式中：[n]為机械臂的关节空间维数，[i-1Ti]为相邻连杆的坐标变换矩阵。坐标系变换通式为：
[i-1Ti=cθi-sθicαisθisαiaicθisθicθicαi-cθisαiaisθi0sαicαidi0001]， （2）
式中：[cθi=cosθi，sθi=sinθi，cαi=cosαi，sαi=sinαi]。实验对象为六自由度机械臂，则[n=6]。
将表1中D-H参数代入坐标变换矩阵[i-1Ti]中，可得到6个相邻连杆坐标系之间的连杆变换矩阵[i-1Ti]，令i=1，由式（1）可得从机械臂末端到基座的变换位姿矩阵[0T6][14-15]：
[0T6=0T11T2…5T6=nxoxaxpxnyoyaypynzozazpz0001]. （3）
最后，利用机器人robotics工具箱在MATLAB上建模，应用Link函数和SerialLink函数搭建如图3所示的机械臂三维模型。
图3中，机械臂的初始角度和初始位置与D-H参数表中的数据保持一致。图3（a）上部分为机械臂末端的位姿，图3（a）下部分可用来调节左侧的各个关节角的大小，并在三维坐标中（图3（b））实时反映其位姿状态，从而观察机械臂的运动轨迹和工作空间。
1.2 机械臂工作空间的求解
机械臂受到本身物理结构与参数的约束，其工作空间有限。采用蒙特卡罗的方法[16-17]对机械臂工作空间进行MATLAB仿真，结果如图4所示。从图4（a）可知，利用蒙特卡罗的方法可以较好地获得一系列反映机械臂工作空间的随机点云图。为了方便测量和计算，设置机械臂末端在空间三维图中的yoz平面执行跟踪任务，只需对yoz平面进行工作空间分析。从图4（b）中可得，机械臂在y轴方向上的近似工作范围为[y∈（-0.36，0.36） m]，在z轴方向上的近似工作范围为[z∈（-0.24，0.40） m]。虽然该结果不能精确地表示出工作空间的边界，但是也能基本满足一般轨迹任务的设置需要。
2 运动规划方案
2.1 二次规划
本文采用的二次型机械臂运动规划方案[5-7] 如下：
最小化：
[θTWθ2+qTθ] ， （4）
约束条件：
[J（θ）θ=r] ， （5）
[θ-≤θ≤θ+] ， （6）
[θ-≤θ≤θ+] ， ; （7）
其中：[W]为系数矩阵，[q]为系数向量，[r]为机械臂在笛卡尔空间中的速度，[θ]和[θ]分别表示机械臂关节空间的位置和速度，[J]为机械臂的雅可比矩阵，[θ±]和[θ±]分别表示机械臂关节向量和关节速度向量的上下极限。
将式（6）转化为[θ]的双端约束：
[k（θ--θ）≤θ≤k（θ+-θ）] ， （8）
式中正系数[k]用于调节关节速度的可行域。因此，关节极限（6）和关节速度极限（7）可以统一为一个 约束：
[ζ-≤θ≤ζ+] ， （9）
其中，[ζ-]和[ζ+]的第[i]个元素的关节速度分别定义为：
[ζ-i=max（k（θ-i-θi），θ-i），ζ+i=min（k（θ+i-θi），θ+i）.] （10）
由于本文不研究特定的机械臂运动规划问题，可将系数向量设置为[q=0∈Rn]，系数矩阵[W=I∈Rn×n]，因此，可得简化的二次规划方案：
最小化：
[xTx2]， （11）
约束条件：
[Cx=d]， （12）
[ζ-≤x≤ζ+]， （13）
其中：待求解的决策向量[x=θ∈Rn]，系数向量[d=r∈Rm]，[C=J（θ）∈Rm×n]。
2.2 E47算法求解器
针对2.1提出的二次规划问题（即式（11）—式（13）），拟将二次型问题转化为线性变分不等式（LVI），再转化为分段线性方程，最后应用E47算法求解器进行求解。
首先，将二次规划问题转换为线性变分不等式，即找到一个原对偶决策向量[y∗∈Ω=y|y-≤y≤y+⊂Rn+m]，使得[∀y∈Ω]都满足：
[（y-y∗）T（Hy∗+z）≥0]， （14）
式中：[H=W-CTC0]，[z=0-d]，原對偶决策向量[y]及其上下极限分别定义为：
[y=xu]，[y+=x++∞]，[y-=x--∞]，
式中：[∞]表示一个无穷大的向量，[u]表示等式约束（10）所对应的对偶决策向量。
上述结论可以表示为如下定理：
定理[8]：当至少存在一个最优解[x∗]时，二次规划问题（式（11）—式（13））可以转化为LVI问题（式（14））。
其次，LVI问题和分段线性方程问题之间的等价，是将不等式（14）转换成等式（15）：
[PΩ（y-（Hy+z））-y=0]， （15）
式中：[PΩ（⋅）]为分段线性投影算子。[PΩ（y）]从空间[Rn+m]到集合[Ω]的投影结果定义为：
[PΩ（yi）=y-i，yi，y+i， yi<y-i; y-i<yi<y+i; y+i>yi.]
为实现具体的求解算法，在上述等价转化定理的基础上，定义向量误差函数[11]如下：
[e（y）=y-PΩ（y-（Hy+z））] . （16）
因此，求解QP问题（式（11）—式（13））等价于寻求误差函数（式（16））的[0]点。将寻找线性变分不等式的[0]点定义如下：
[g（y）=HTe（y）+（Hy+z）] ， （17）
可得第[k]次迭代的结果为：
[yk+1=PΩ（yk-ρ（yk）g（yk））] ， （18）
[ρ（y）=e（y）22（HT+I）e（y）22] . （19）
在算法实现中，为了保证算法的求解效率，在算法程序中设置了2个结束算法流程的控制条件，即求解精度与迭代次数（均可根据实际情况自行设置）。在求解过程中，如果求解得到的数值精度小于预先设置的求解精度，算法流程结束。可根据求解时间的需要来设置迭代次数，如果迭代次数达到预先设置的次数，那么算法流程强制结束。最后一次迭代过程中计算生成的[y]的前[n]个元素便构成了二次规划问题（式（11）—式（13））的解，其中式（16）—式（19）构成了E47数值算法。
由于机械臂在执行任务时，其末端执行器存在位置误差，因此，可将机械臂末端执行器的实时偏差定义为：
[w（t）=rd（t）-r（t）] ， （20）
式中：[w（t）∈Rm]为机械臂末端的实际轨迹与期望轨迹之间的实时误差，[rd（t）∈Rm]为机械臂期望的末端运动轨迹，[r（t）∈Rm]为机械臂实际的末端运动轨迹。为了更好地使位置误差最终收敛到0，可设计误差补偿函数：
[w（t）=-λw（t）]， （21）
式中：[λ]为误差补偿系数，为一个标量系数。根据误差补偿的需要选取不同的[λ]值[18]。
引入误差补偿函数以后，机械臂在执行任务过程中的实际末端执行器速度可表示为：
[r（t）=rd（t）+w（t）] . （22）
综上，加入误差补偿函数的E47算法求解器的求解控制框图如图5所示。
3 仿真研究
由于Arduino单片机系统的程序开发条件较为便利[19-20]，因此，可在Arduino单片机系统上对机械臂执行圆形和正方形轨迹跟踪方案分别求解，并通过MATLAB软件对所得结果进行仿真分析。由于产生的轨迹数据点较多且实物为舵机驱动的简易六自由度机械臂，因此，可选用以8位单片机为核心的Arduino Mega 2560开发板作为主控制器，其配置容量为256 kB的Flash[19]，数据存储量相对充足，满足本实验的需要。而且开发板具有专门的PWM通道[20]，便于机械臂系统中6个舵机的连接控制。
3.1 圆形轨迹跟踪
设机械臂末端执行器从指定初始位置执行半径为0.03 m的圆形轨迹跟踪任务，其期望圆形轨迹参数方程如下：
[rx（t）=ix，ry（t）=cos（desird_p）+iy-r，rz（t）=-rsin（desird_p）+iz.] （23）
式中：[ix]、[iy]、[iz]分别代表机械臂末端的初始位置[（0.253，0，0.215）m]，[desird_p=aa⋅T⋅（t-T⋅sin（2π·]
[t/T）/（2π））/（2π）]，其中長度参数[aa=0.398]，任务周期[T=10 s]。初始的关节角[θ（0）=0，π/2，0，0，0，0Trad]，各个关节角度的上下极限分别为：
[θ+=π/2，147π/180，52π/180，π，-π/2，πTrad]，
[θ-=-π/2，36π/180，-53π/180，0，-π/2，0Trad].
设置求解精度为[10-6 m]，且迭代次数达1 000次时强制退出。仿真结果如图6所示。
通过Arduino Mega2560对机械臂运动规划算法进行运算，得到的各个关节角速度数据所构成的曲线如图6（a）所示，曲线连续、较光滑且平稳，没有出现明显毛糙的现象，说明Arduino Mega2560平台运算结果数据精度较高，利于机械臂的平稳控制。图6（b）展示了机械臂的末端运动轨迹，从图中可以看出，机械臂末端执行器运动轨迹能很好地跟踪期望轨迹。将沿x轴、y轴和z轴方向上的位置误差分别定义为[ex]、[ey]和[ez]，具体的末端位置误差随时间变化情况如图6（c）所示。从图中可以看到，最大绝对值位置误差沿x方向为[ex=1.208×10-4 m]，沿y方向为[ey=1.590×10-4 m]，沿z方向为[ez=1.906×10-4 m]，该误差精度达到了毫 米级。
为进一步提高误差精度以获得更好的跟踪效果，可在机械臂运动规划方案中引入误差补偿函数（即式（21）—式（22）），取[λ=30]，数据仿真结果如图7所示。从图7（a）中可以明显地看到，机械臂末端跟踪轨迹与期望轨迹较好地重合在一起。由图7（b）可以发现，最大绝对值位置误差沿x方向为[6.025×10-6 m]，沿y方向为[3.871×10-5 m]，沿z方向为[3.325×10-5 m]，误差精度相比没有加入误差补偿函数时得到了提高。以上分析结果说明了8位单片机对机械臂运动规划算法的实现是可行的。
3.2 正方形轨迹跟踪
为进一步验证在8位单片机上实现机械臂运动规划算法，对机械臂执行的边长为0.05 m的正方形轨迹跟踪任务进行仿真分析。该任务执行周期[T=8] s，求解迭代次数最高为200次，求解精度、机械臂初始关节角、机械臂末端初始位置以及关节角的上下极限均与上例跟踪圆形轨迹相同。
图8是未加入误差补偿时，8位单片机对机械臂执行正方形轨迹跟踪任务进行求解的数据仿真结果。从图8（a）机械臂末端运动轨迹中可知，机械臂在执行整个任务过程中产生了一定的误差，没能完全跟踪期望轨迹。从图8（b）中可知，机械臂最大绝对值误差在x方向为[6.580×10-4 m]，在y方向为[9.899×10-4 m]，在z方向为[9.911×10-4 m]。并且从图中可明显地看到，机械臂末端在[t=8] s时，虽然x轴方向上的位置误差没有收敛到0，但误差精度仍为毫米级。
为使机械臂更好地跟踪期望轨迹，可考虑加入误差补偿函数，使其整体的跟踪误差减小。仿真结果如图9所示。
从图9（a）中可以看到，加入误差补偿函数后，机械臂末端跟踪轨迹与期望轨迹较吻合。从图9（b）可以看到，在x、y、z轴方向上的误差均减小了，误差精度得到了提高。以上分析结果说明8位单片机对机械臂运动规划算法进行运算时能达到较高的精度需求。从图9（c）可以看出，机械臂能较好地跟踪正方形轨迹。通过以上仿真结果分析可知，8位单片机对机械臂运动规划算法进行运算所得的数据曲线较平滑，误差精度较高，进一步说明了8位单片机对机械臂运动规划算法的实现具有较好的可行性和实用性。
3.3 构造硬件实现模型及实物验证
由于8位单片机对算法运算的实时性不理想，在實际执行任务时会由于执行任务不连续而导致机械臂末端位置存在误差，影响实验结果，因此，构造了基于Simulink的机械臂运动规划硬件实现模型，如图10所示。该模型分为4个部分，data模块用于放置由Arduino Mega2560单片机对机械臂运动规划算法运算得到的6个关节角度运动数据，数据读取模块用于读取单位时间内的角度数据，将角度数据传递给舵机，驱动机械臂进行轨迹跟踪任务。图11为对应3.2小节机械臂跟踪边长为0.05 m的正方形仿真的实物实验情况。图11中，“起点”表示机械臂初始的末端位置，箭头表示机械臂对正方形轨迹的跟踪方向。图12展示了实际机械臂末端在整个周期内执行的完整路径。从图中能够清晰地看到，画出的正方形边长为0.05 m，与期望轨迹较为吻合。因此，该实验表明了E47数值算法在实际应用中的可行性以及所构造的硬件实现模型的有效性。
4 总结
目前机器人运动规划与控制系统成本较高，机器人实验装置难以普及，学生缺少实践动手机会。为解决这一问题，通过8位单片机对E47数值算法进行了实现，并对机械臂执行圆形和正方形轨迹跟踪的运动规划进行了仿真。仿真结果表明，8位单片机对机械臂运动规划算法的实现具有可实现性和可适用性。为提高机械臂执行任务的实时性，构造了基于Simulink的硬件实现模型，从实验结果中可以看出所构造的机械臂硬件实现模型有效，说明本文所提出的方案能够降低机械臂控制系统的成本，满足研究者对机械臂运动规划学习的需要，为后续更好地研究机械臂运动奠定了一定的基础。
参考文献
[1] HU T J，WANG T S，LI J F，et al.Obstacle avoidance for redundant manipulators utilizing a backward quadratic search algorithm[J].International Journal of Advanced Robotic Systems，2016，13（3）：109-116.
[2] ZHANG Y N，GUO D S，CAI B H，et al.Remedy scheme and theoretical analysis of joint-angle drift phenomenon for redundant robot manipulators[J].Robotics and Computer-Integrated Manufacturing，2011，27（4）：860-869.
[3] HUANG S H，PENG Y G，WEI W，et al.Clamping weighted least-norm method for the manipulator kinematic control：avoiding joint limits[C]//Proceedings of the 33rd Chinese Control Conference. IEEE，2014：8309-8314.
[4] LIAO B L，LIANG P Y，YANG X.Pseudoinverse-based optimization scheme for motion control of redundant robot manipulators[J].Information and Control，2013，42（5）：645-651，656.
[5] ZHANG Y N，LI K N.Bi-criteria velocity minimization of robot manipulators using LVI-based primal-dual neural network and illustrated via PUMA560 robot arm[J].Robotica，2009，28（4）：525-537.
[6] LI K N，ZHANG Y N.Fault-tolerant motion planning and control of redundant manipulator[J].Control Engineering Practice， 2012，20（3）：282-292.
[7] 张雨浓，符刚，尹江平.基于双判据优化方法的机器人逆运动学求解[J].大连海事大学学报，2007，33（3）：1-5.
[8] 谢清，张雨浓，余晓填，等.面向冗余度机械臂QP问题求解的E47和94LVI数值算法[J].计算机工程与科学，2015，37（7）：1405-1411.
[9] ZHANG Y N.On the LVI-based primal-dual neural network for solving online linear and quadratic programming problems[C]//Proceeding of American Control Conference，2005：1351-1356.
[10] ZHANG Y N，WANG J.A dual neural network for constrained joint torque optimization of kinematically redundant manipulators[J].IEEE Transactions on Systems，Man，and Cybernetics-Part B：Cybernetics，2002，32（5）：654-662.
[11] HE B S.A new method for a class of linear variational linequalities[J].Mathematical Programming，1994，66（2）：137-144.
[12] ZHANG Y N，FU S B，ZHANG Z J，et al.On the LVI-based numerical method （E47 algorithm） for solving quadratic programming problems[C]//Proceeding of the IEEE International Conference on Automation and Logistics，2011：125-130.
[13] 劉海燕，苏宇，游庆如，等.HSR-JR605工业机械臂的运动仿真研究[J].广西科技大学学报，2020，31（3）：22-27.
[14] 毛志贤，韦建军，王春宝，等.新型四臂扶持式康复机器人设计[J].广西科技大学学报，2020，31（3）：1-7.
[15] 臧庆凯，李春贵，钟宛余.基于RBF和BP网络的机器人逆运动学求解[J].广西工学院学报，2012，23（1）：28-33.
[16] 孙野，殷凤龙，王香丽，等.六自由度机械臂运动学及工作空间分析[J].机床与液压，2015，43（3）：76-81.
[17] 王春，韩秋实.六自由度串联机械臂运动学及其工作空间研究[J].组合机床与自动化加工技术，2020，62（6）：32-36.
[18] 李克讷，杨津，徐剑琴，等.机械臂初始位置误差的容错运动规划[J].哈尔滨理工大学学报，2020，25（1）：93-99.
[19] 宋振鹏.一种家庭服务型机器人移动平台开发[D].成都：西南交通大学，2016.
[20] 聂超，宋志丹，孙晓宇，等.基于Mega2560的物联型风机主轴部件加热装置[J].船舶工程，2020，42（S2）：286-291.
Implementation of E47 algorithm of redundant manipulator in
Arduino environment
LI Kene1， WANG Wenxin1， HU Xuchu2， MA Yuru1， HE Zhixiang1， YUAN Weiming1， YE Hongtao3，4
（1.School of Electrical， Electronic and Computer Science， Guangxi University of Science and Technology，
Liuzhou 545616， China; 2.Guangxi Liuzhou Special Transformer Co.，Ltd.，Liuzhou 545006， China;
3.Guangxi Key Laboratory for Automatic Detecting Technology and Instruments， Guilin University of Electronic Technology， Guilin 541004， China; 4.Guangxi Key Laboratory of Automobile Components and Vehicle
Technology （Guangxi University of Science and Technology）， Liuzhou 545006， China）
Abstract：
The high cost of the motion planning and control system of the manipulator decreases practical opportunities for the related educatees， which makes it difficult to popularize the robot technology. In this paper， an 8-bit microcontroller was used to implement the manipulator motion planning algorithm and a hardware implementation model of manipulator motion planning based on Simulink was proposed to improve the real-time performance of manipulator. Firstly， the kinematic analysis of the manipulator was given out， and the mathematical model was established. Secondly， the motion planning scheme of the manipulator was transformed into a quadratic programming problem， and the error compensation function was introduced to reduce the position-error generated during the manipulator motion. Then， in the Arduino Mega2560 platform， the E47 algorithm was used to resolve the manipulator motion planning scheme; the Matlab was used to perform the circular and square path tracking tasks of the manipulator. Finally， the hardware implementation model was used to verify the square trajectory of the manipulator. The results indicate the feasibility and applicability to realize the manipulator motion planning algorithm using the 8-bit microcontroller， and the validity of the hardware implementation model， which provides a basis for the study of the motion planning of the manipulator.
Key words：
8-bit microcontrolle; manipulator motion planning; E47 algorithm; error compensation; hardware implementation model
（責任编辑：黎 娅）
猜你喜欢 运动学轨迹误差 基于MATLAB的工业机器人运动学分析与仿真内燃机与配件(2022年2期)2022-01-17浅谈求轨迹方程中的增解与漏解福建中学数学(2021年1期)2021-02-28无从知晓小资CHIC！ELEGANCE(2021年44期)2021-01-11隧道横向贯通误差估算与应用智富时代(2018年5期)2018-07-18隧道横向贯通误差估算与应用智富时代(2018年5期)2018-07-18例析运动学中STS类信息题新高考·高一物理(2016年11期)2017-07-07捕捉物体运动轨迹课堂内外(小学版)(2017年3期)2017-04-15精确与误差中学生数理化·七年级数学人教版(2016年6期)2016-05-14“必修1”专题复习与训练新高考·高一物理(2016年1期)2016-03-05压力表非线性误差分析与调整海峡科学(2013年3期)2013-10-21 相关热词搜索： 冗余 算法 环境上一篇：谭淇尹：把歌献给孩子们
下一篇：彝族月琴音乐文化研究综述