2. 成都飞机设计研究所, 四川 成都 610041
机械臂控制技术的发展过程经历了多个阶段。机械臂诞生之初, 由于结构简单且所使用的环境较为单一, 任务类型对控制精度要求不是很高, 因此其控制系统设计较为简单。随着计算机等其他硬件设备性能的不断提升, 机械臂系统逐渐变得复杂起来, 以应对多样化的应用环境。当机械臂结构复杂度、冗余度增高之后, 各关节之间存在力的耦合作用, 因此其控制难度就会增大, 原本传统的控制方法不再适用。在当前阶段, 机械臂作业环境以及作业内容的变化, 要求轨迹跟踪控制器能够在高速运行状态下依然具有较强的抗干扰能力和较高的跟踪控制精度[1-2]。
机械臂运动控制系统属于多输入多输出系统, 系统结构复杂并且在运动学以及动力学上存在时变、强耦合以及非线性等性质。对于控制系统设计而言, 机械臂状态测量误差、机械臂建模误差以及外部环境扰动都会影响控制品质。在实际工业生产中, 基于独立关节的PID控制算法应用极为广泛[3-5]。将每个独立关节使用泰勒展开方式转换为二阶线性系统, 将关节之间的耦合效应视为外界干扰因素, 其控制器结构简单, 不需要生产研发人员具备相当专业的基础知识, 应用范围较为广泛。然而, 该类算法面对复杂被控对象时, 其参数整定与调试难度较大。针对机械臂动力学模型较为复杂的特性, 一些研究者提出了近似逼近的思想处理机械臂模型参数难以获取、建模难度高的问题。一些常用的逼近算法有构建模糊集、神经网络逼近法、小波网络和遗传算法等方法[6-18], 这些方法的特点都是使用近似模型代替参数辨识后的准确模型[19-20]。由于机械臂存在精确动力学模型难以获取、系统参数辨识难度较大的问题, 预测控制算法被提出用于机械臂的轨迹跟踪中, 预测控制算法对模型的精度要求不高, 对于系统参数摄动有很强的适应能力, 在工业过程控制中被广泛使用。文献[21]针对凿岩机械臂运动轨迹控制问题, 提出了一种广义预测控制与PID控制结合的控制方法, 该方法能够消除广义预测控制算法起始阶段计算不稳定的病态问题, 提升了整体的控制效果。
鉴于机械臂动力学模型时变、强耦合以及非线性等性质, 本文将反馈线性化与GPC算法结合起来, 一方面利用了GPC算法抗干扰能力强的优点, 另一方面又克服了GPC算法难以应用于非线性模型的问题。
1 问题描述本文针对机械臂的轨迹跟踪控制问题展开研究。为了方便说明所提算法的原理, 以图 1所示的平面二自由度机械臂结构为例进行讨论。该机械臂结构包含2个旋转关节以及2个质量均匀分布的刚性连杆, 第二个机械臂连杆的末端自由且无负载, 并且该机械臂结构仅能够在一个平面内运动, 此处只考虑重力对机械臂的影响。图 1所示的机械臂第一个旋转关节、关节角变量及关节连杆分别用o1, q1, l1表示, 第二个旋转关节、关节角变量用及关节连杆分别用o2, q2, l2表示, 以顺时针为旋转正方向。采用D-H(Denavit-Hartenberg)法对机械臂建模, D-H参数如表 1所示。
|
| 图 1 双连杆平面机械臂系统 |
机械臂的动力学建模方法较多, 其中牛顿欧拉法与欧拉-拉格朗日分析法应用最为广泛[22]。本文使用欧拉-拉格朗日分析法来构建机械臂运动学模型, 欧拉-拉格朗日力学表达式为
|
(1) |
式中：L为拉格朗日算子, 并且有L=K-P；K为机械臂总动能；P为机械臂总势能。首先对机械臂动能函数表达式进行推导, 机械臂的动能包含平动动能和转动动能, 假定机械臂的连杆li上有一点ri, 在基础坐标系中的位置为
|
(2) |
Ti为该点相对于基坐标系的变换矩阵, 那么该点的速度表达式为
|
(3) |
因此可以得到在连杆li的ri处, 质量为dm的质点动能为
|
(4) |
经过积分运算可以得到连杆li的动能为
|
(5) |
同时, 转子i的转动动能表达式为
|
(6) |
式中，Iai表示第i个转子的惯量。因此可以得到二自由度平面机械臂的总动能为
|
(7) |
根据重力势能的计算方式可以得到机械臂的总势能为
|
(8) |
将(7)~(8)式代入(1)式中, 可以得到二自由度机械臂的动力学模型为
|
(9) |
式中：
|
(10) |
一般机械臂的运动控制系统由三大部分构成, 分别是路径规划系统、轨迹规划系统与轨迹跟踪系统。其中轨迹规划系统为轨迹跟踪系统的上游环节, 其输出的关节角时变函数作为轨迹跟踪系统的参考轨迹。机械臂的轨迹跟踪问题就是在各关节驱动器动力学性能的约束下, 计算各关节的最优控制量, 使得机械臂能够快速精准地跟踪参考轨迹。
2 轨迹跟踪控制算法设计 2.1 算法框架设计传统预测控制算法在求解最优控制量时采用反复迭代方式, 把优化时域内的状态量进行叠加, 再将控制量的求解问题转化为一个优化问题, 然后结合一般优化问题求解思路求得最优控制量。非线性函数经过多次复合求导之后, 其形式会变得非常复杂, 数学处理难度大大增加, 这就造成了即使得到优化问题的解析表达式, 在具体使用时也会难以求解的问题。因此相比于传统广义预测控制算法, 基于反馈线性化的广义预测控制算法针对高度非线性、耦合效应较严重的被控对象有着较好的控制效果。
改进算法使用了分层优化的底层框架设计方式, 即底层为传统广义预测控制, 此时的非线性项使用预估值进行代替, 高层为迭代修正预估量, 使用迭代计算的方式对非线性项进行预估, 最终将非线性系统的预测控制问题转化为线性系统预测控制问题[23-24]。这种分层优化的非线性预测控制算法框架如图 2所示。
|
| 图 2 分层优化非线性预测控制框架 |
当被控对象为机械臂动力学模型时, 可以使用仿射非线性系统构建状态方程, 当考虑多输入多输出的仿射非线性系统时, 其状态方程可以表示为
|
(11) |
式中：f=(f1, f2, …, fn)T, f, gj: Rn→Rn是Rn上的光滑向量场；hi: Rn→ R是Rn上的光滑标量函数。为了将原系统中的非线性项消去, 需要设计合适的状态反馈律来代入非线性系统。为此首先定义hi对于f的李导数为
|
(12) |
对于多输入多输出系统的每一个输出yi可以定义其相对度ri, ri满足(13)式所示条件。
|
(13) |
式中，l=0, 1, …, ri-2, j=1, 2, …, m。
根据相对度的定义以及(11)式所述的yi表达式, 可以得到
|
(14) |
(14) 式可以改写为如(15)式所示形式
|
(15) |
式中，βil>0, l=1, 2, …, ri用来配置线性化后的系统状态方程, (15)式可简化表达为
|
(16) |
式中
|
|
当F(x)非奇异时, 由(16)式可以得到状态反馈律的表达式, 其形式如公式(17)所示
|
(17) |
把(17)式所示的反馈律代入到(16)式, 同时结合(15)式可得到如(18)式所示的线性系统表达式
|
(18) |
至此就得到了以vi为输入, yi为输出的m个解耦的单输入单输出线性系统。这种从仿射非线性系统到结构线性系统的去非线性算法流程可以用图 3表示。
|
| 图 3 基于反馈线性化的预测模型线性化算法流程 |
根据本文所述的基于反馈线性化的广义预测控制算法框架, 在对非线性系统进行解耦线性化处理之后, 可以根据(18)式得到各个解耦子系统的传递函数, 那么便可以使用GPC算法进行处理[25]。
根据GPC算法对预测模型形式的要求, 可以将反馈线性化处理后得到的系统传递函数写为
|
(19) |
式中：t表示采样控制的离散时间点; ql-1表示后移算子, 意为后退一个采样周期的量; Δ=1-ql-1为差分算子; ξ(t)是一个不相关随机序列, 表示随机噪声。
考虑(20)式所示丢番图方程
|
(20) |
式中, Ej和Fj是由A和预测长度j唯一确定的多项式, 并且有
|
在(19)式两端同时乘以Ej(ql-1)Δqlj, 并经过化简同时对误差量进行忽略, 可以得到GPC算法的预测模型
|
(21) |
Ej和Fj需要通过递推算法求解特定的丢番图方程得到。
最优控制律的计算与选取的指标量相关, 对于机械臂的关节轨迹跟踪控制问题而言, 一方面要保证各个关节角的运动状态能够快速跟踪上参考轨迹函数并且跟踪误差较小; 另一方面也要对关节控制力矩加以限制, 限制因素包括了关节角速度、角加速度以及起止状态的瞬间力矩, 从而达起到保护关节驱动器, 延长设备使用寿命的目的。按照以上原则可以定义滚动优化函数形式为
|
(22) |
式中：w为期望轨迹; N1, N2分别为优化时域的起始点与终止点; Nu为控制时域; λ(j)为控制加权系数, 一般为常数。
将公式(21)中的预测模型结果带入滚动优化函数中, 经过整理可以得到最优控制量增量ΔU的表达式为
|
(23) |
式中
|
gi为系统的单位阶跃响应系数, fi可以在求解丢番图方程的过程中而得到。
对于GPC的预测模型, 通过如下方法对其进行在线修正。首先将(19)式改写为(24)式所述形式
|
(24) |
式中, A1(ql-1)=A(ql-1)-1。把模型参数与数据参数分别用向量形式记做
|
进而带入(24)式得到(25)式所示结果
|
(25) |
采用减消记忆的递推最小二乘法估计参数向量
|
(26) |
式中：μ为遗忘因子是一个经验值常数; K(t)和P(t)分别为权重因子与正定的协方差阵。在控制算法开始阶段, 先设置参数向量θ和协方差矩阵P的初值, 然后在控制的每一步更新数据向量, 将更新后的值代入(26)式求得K(t), 
考虑如图 1所示平面两连杆机械臂的静态跟踪与动态轨迹跟踪问题。假定两连杆机械臂的每个连杆质量沿杆长平均分布, 且其长度与质量都为单位量。设置机械臂连杆长度L1=120 cm, L2=100 cm, 机械臂初始位置θ0=[0, 0]。设定2类跟踪目标, 即静态跟踪与动态跟踪。在静态跟踪任务中, 为2个活动关节分别设置1个固定目标数值, 使机械臂在轨迹跟踪器的控制下完成从初始状态向参考姿态收敛的运动; 在动态跟踪目标任务中, 为2个活动关节分别设定1个角度变化函数, 使机械臂从初始状态出发最终跟踪上各关节角的变化速度。为了说明本文所提出的控制算法的有效性, 选择经典的PID控制算法和基于RBF神经网络的自适应控制算法与本文所提出的FL-GPC算法作对比。
仿真实验初始条件设置参数如表 2所示。
传统PID控制算法参数设置为
|
基于RBF神经网络的自适应控制算法初始参数参考文献[27]进行设置, FL-GPC算法中采样周期T=0.1 s, 对于GPC算法P=4, M=1, Q=I, r=0.05。对于两关节机械臂的静态与动态跟踪仿真结果如下:
从图 4的仿真结果可以看出, 对于静态跟踪任务, 3种控制方法均能够使得关节角从0逐渐跟踪到目标角度(关节角1为2 rad, 关节角2为1 rad), 最终得到较为稳定的跟踪结果, 其中基于RBF的自适应控制算法有1.3 rad左右的超调量, 而PID算法与本文所提出的FL-GPC算法均无超调量。从收敛时间上来看, FL-GPC算法在2 s左右就已经收敛到了目标值, 而PID控制算法和基于RBF神经网络的自适应控制算法收敛时间则在8 s左右。并且从图 5的仿真结果还可以看出在0~8 s这段时间内, 基于RBF神经网络的自适应控制算法计算得到的最优控制量是震荡的, 其中关节1的最大输出力矩为190 N·m, 关节角2的最大输出力矩为80 N·m, 要远远大于PID控制算法和FL-GPC算法的最大输出力矩。综上可得在静态跟踪任务中FL-GPC算法性能较优。
|
| 图 4 静态跟踪仿真结果 |
|
| 图 5 静态跟踪控制力矩变化图 |
从图 6~7的仿真结果可以看出, 对于动态跟踪任务而言, PID控制算法和基于反馈线性化的分层优化非线性预测控制算法均能在仿真开始后3 s左右跟踪上目标轨迹, 而基于RBF神经网络的自适应控制算法在仿真开始后10 s还未完全跟踪上目标轨迹, 虽然从总体趋势来看是收敛的, 但是该方法的收敛速度相比于前2种方法慢很多。
|
| 图 6 动态角度跟踪结果 |
|
| 图 7 动态角速度跟踪结果 |
从图 8的仿真结果可以看出, 相比于PID控制算法, 基于反馈线性化的分层优化非线性预测控制算法在仿真初始阶段力矩变化更加平滑, 没有很大幅度的变化情况, 输出力矩的最大值出现在仿真开始时。以关节1为例, 输出力矩峰值约为50 N·m, 而PID控制算法峰值绝对值则在110 N·m左右, 对于工业机器人的运动控制系统来说非线性预测控制算法是比较好的, 平滑的力矩输出能够有效保护关节驱动器, 从而提升设备使用寿命。综上可得在动态跟踪任务中FL-GPC算法性能较优。
|
| 图 8 动态跟踪控制力矩变化图 |
本文通过对机械臂动力学系统特性进行分析, 根据其非线性、存在大量未建模动态特性等特点, 提出了基于反馈线性化的分层优化非线性预测控制算法, 并基于该算法设计了机械臂轨迹跟踪控制器。通过对比PID控制算法、基于RBF神经网络的自适应控制算法以及本文所采用的FL-GPC控制算法仿真结果, 验证了本算法在静态跟踪和动态跟踪任务场景中具有更好的控制效果。
| [1] |
马克W·斯庞, 赛斯·哈钦森, M·维德雅萨加. 机器人建模和控制[M]. 北京: 机械工业出版社, 2016.
MARK W·Spang, SETH Hutchinson, M·Vaderjasaga. Robot modeling and control[M]. Beijing: Machinery Industry Press, 2016. (in Chinese) |
| [2] |
PETERCorke. 机器人学、机器视觉与控制[M]. 北京: 电子工业出版社, 2016.
PETER Corke. Robotics, Machine Vision and Control[M]. Beijing: Electronics Industry Press, 2016. (in Chinese) |
| [3] |
丁学恭. 机器人控制研究[M]. 杭州: 浙江大学出版社, 2006.
DING Xuegong. Research on robot control[M]. Hangzhou: Zhejiang University Press, 2006. (in Chinese) |
| [4] | CERVANTES I, ALVARE-RAMIREZ J. On the PID tracking control of robot manipulators[J]. Systems & Control Letters, 2001, 42(1): 37-46. |
| [5] |
张铁, 洪景东, 李秋奋, 等. 基于BP神经网络的机器人波动摩擦力矩修正方法[J]. 工程科学学报, 2019, 41(8): 1085-1091.
ZHANG Tie, HONG Jingdong, LI Qiufen, et al. Wave friction correction method for a robot based on BP neural net-work[J]. Journal of Engineering Science, 2019, 41(8): 1085-1091. (in Chinese) |
| [6] |
马宇豪, 梁雁冰. 一种基于六次多项式轨迹规划的机械臂避障算法[J]. 西北工业大学学报, 2020, 38(2): 392-400.
MA Yuhao, LIANG Yanbing. An obstacle avoidance algorithm for manipulator based on sixth-order polynomial trajectory planning[J]. Journal of Northwest Polytechnical University, 2020, 38(2): 392-400. (in Chinese) |
| [7] | YANRU L, YAN Z. Two-DOF manipulator trajectory tracking control based on unfalsified control[C]//The 27th Chinese Cont-rol and Decision Conference, Qingdao, 2015: 4563-4566 |
| [8] | ABDALLA A Y, ABDALLA T Y, CHYAID A M. Grasshopper algorithm based fuzzy system for trajectory tracking of robot manipulator[C]//2022 International Conference on Electrical, Computer and Energy Technologies, Prague, 2022: 1-5 |
| [9] | ZHANG X, GU J, ASAD M U, et al. Beetle bee algorithm applied to trajectory tracking control of omni manipulator[C]//2022 International Conference on Emerging Trends in Electrical, Control, and Telecommunication Engineering, Lahore, 2022: 1-5 |
| [10] | SINGH R, PRASAD L B. Optimal trajectory tracking of robotic manipulator using ant colony optimization[C]//2018 5th IEEE Uttar Pradesh Section International Conference on Electrical, Electronics and Computer Engineering, Gorakhpur, 2018: 1-6 |
| [11] | SHAOMING L, RUIPENG L. Research on trajectory tracking control of multiple degree of freedom manipulator[C]//2017 32nd Youth Academic Annual Conference of Chinese Association of Automation, Hefei, 2017: 218-222 |
| [12] | JUAN W, YANG H, XIE H. Control of manipulator trajectory tracking based on improved RBFNN[C]//2009 International Conference on Intelligent Human-Machine Systems and Cybernetics, Hangzhou, 2009: 142-145 |
| [13] | MIRÓ J V, WHITE A S, GILL R. On-line time-optimal algorithm for manipulator trajectory planning[C]//1997 European Control Conference, Brussels, 1997: 2611-2616 |
| [14] | ATALAR-AYYLLDLZ B, KARAHAN O. Tuning of fractional order pid controller using CS algorithm for trajectory tracking control[C]//2018 6th International Conference on Control Engineering & Information Technology, Istanbul, 2018: 1-6 |
| [15] | ZHANG L, CHENG L. An adaptive neural network control method for robotic manipulators trajectory tracking[C]//2019 Chinese Control and Decision Conference, Nanchang, 2019: 4839-4844 |
| [16] | WIDYIANTO A, YAZID E, MIRDANIES M, et al. Optimization of PD controller using ACO for the trajectory tracking of a ship-mounted two-DOF manipulator system[C]//2022 6th International Conference on Information Technology, Information Systems and Electrical Engineering, Yogyakarta, 2022: 634-638 |
| [17] | LAMPINEN S, NIEMI J, MATTILA J. Flow-bounded trajectory-scaling algorithm for hydraulic robotic manipulators[C]//2020 IEEE/ASME International Conference on Advanced Intelligent Mechatronics, Boston, 2020: 619-624 |
| [18] | ZHU Q, WANG J, ZHANG W A, et al. A Geometry based IK solver and b-spline method for trajectory tracking of 5-DOF manipulators[C]//2018 37th Chinese Control Conference, Wuhan, 2018: 3865-3870 |
| [19] |
杨易旻. 基于极限学习的系统辨识方法及应用研究[D]. 长沙: 湖南大学, 2013 YANG Yimin. Researches on extreme learning theory for system identification and applications[D]. Changsha: Hunan University, 2013 (in Chinese) |
| [20] |
于欣波, 贺威, 薛程谦, 等. 基于扰动观测器的机器人自适应神经网络跟踪控制研究[J]. 自动化学报, 2019, 45(7): 1307-1324.
YU Xinbo, HE Wei, XUE Chengqian, et al. Disturbance observer-based adaptive neural network tracking control for robots[J]. Acta Automatica Sinica, 2019, 45(7): 1307-1324. (in Chinese) |
| [21] |
王雷坤. 凿岩机器人钻臂运动轨迹控制研究[D]. 赣州: 江西理工大学, 2019 WANG Leikun. Research on trajectory control of drilling arm of rock drilling robot[D]. Ganzhou: Jiangxi University of Science and Technology, 2019 (in Chinese) |
| [22] |
崔敏其. SCARA机器人的拉格朗日动力学建模[J]. 机械设计与制造, 2013(12): 76-78.
CUI Minqi. Dynamical modeling of SCARA robot based on lagrange formulation[J]. Mechanical Design and Manufacturing, 2013(12): 76-78. (in Chinese) |
| [23] |
周岗, 姚琼荟, 陈永冰, 等. 基于输入输出线性化的船舶全局直线航迹控制[J]. 控制理论与应用, 2007(1): 117-121.
ZHOU Gang, YAO Qionghui, CHEN Yongbing, et al. Global straight-line tracking control of ships based on input-output linearization[J]. Control Theory and Application, 2007(1): 117-121. (in Chinese) |
| [24] |
李铁山, 杨盐生, 郑云峰. 不完全驱动船舶航迹控制输入输出线性化设计[J]. 系统工程与电子技术, 2004(7): 945-948.
LI Tieshan, YANG Yansheng, ZHENG Yunfeng. Input-output linearization designs for straight-line tracking control of undera-ctuated ships[J]. System Engineering and Electronics, 2004(7): 945-948. (in Chinese) |
| [25] |
帅鑫, 李艳君, 吴铁军. 一种柔性机械臂末端轨迹跟踪的预测控制算法[J]. 浙江大学学报, 2010, 44(2): 259-264.
SHUAI Xin, LI Yanjun, WU Tiejun. Real time predictive control algorithm for endpoint trajectory tracking of flexible mani-pulator[J]. Journal of Zhejiang University, 2010, 44(2): 259-264. (in Chinese) |
| [26] | SHOHEI Hagane, LIZ Katherine Rincon Ardila, TAKUMA Katsumata, et al. Adaptive generalized predictive controller and cartesian force control for robot arm using dynamics and geometric identification[J]. Journal of Robotics and Mechatronics, 2018, 30(6): 927-942. DOI:10.20965/jrm.2018.p0927 |
| [27] |
程林云, 张雷, 宋晓娜. 基于RBF神经网络的机械臂自适应控制方法[J]. 计算机测量与控制, 2019, 27(7): 79-84.
CHENG Linyun, ZHANG Lei, SONG Xiaona. Adaptive control method of manipulator based on RBF neural network[J]. Computer Measurement and Control, 2019, 27(7): 79-84. (in Chinese) |
2. Chengdu Aircraft Design & Research Institute, Chengdu 610041, China
