海流环境下二关节机械臂动力学建模及其跟踪控制

葛大会, 尤云祥,3, 冯爱春,3

(1.上海交通大学 海洋工程国家重点实验室,上海 200240;2.上海交通大学 船舶海洋与建筑工程学院,上海 200240;3.上海交通大学三亚崖州湾深海科技研究院,海南 三亚 572000)

0 引 言

在陆地上不可再生资源日渐减少的今天,对蕴藏丰富资源和能源的海洋空间的探索和开发十分必要且紧迫,在执行海底勘测、水面打捞等各种各样的作业任务时,水下机械臂是非常重要和必不可少的装备[1]．不同于陆用机械臂的工作环境,水下机械臂在水下会受到水动力因素的影响以及多种难以预测的外部干扰[2]．目前大多对机械臂动力学建模采用Lagrange法[3-5]或Newton-Euler法[6-8],针对陆地机械臂和水下机械臂的控制方案越来越成熟,但对水下机械臂的控制或限于静水情况,或将海流载荷作为一种随机性质的扰动来考虑．不同传统的AUV,对于用于船舶应急维修的水下机械臂来说,机械臂一般固结于船舶底部基座上,受海流载荷的影响较大．董方方等[9]将Lagrange法与Udwadia-Kalaba (U-K)方法结合,建立了机械臂的动力学模型,减少了建立动力学模型所需要的计算量,然后通过数值仿真验证了该方法所得到的动力学模型的准确性．韩江等[10]提出了一种基于Udwadia-Kalaba方程的鲁棒伺服控制方法,使2自由度冗余驱动并联机器人系统能够精确跟踪理想约束轨迹．宁会峰等[11]对大型光伏电站中光伏组件清扫机器人的结构及其运动过程进行具体分析,并采用Lagrange法建立了机器人中清扫机械臂的动力学模型．Sciavicco等[12]利用Newton-Euler法对柔性机械臂进行了建模．

水下机械臂是一种高度非线性、强耦合、多输入多输出的系统,具有时变、外部干扰和水动力干扰的特点,所以水下机械臂的精确动力学建模和控制问题成为一个热点和难点,因此对水下机械臂的轨迹跟踪进行研究具有重要的理论意义和实用价值．Zhong等[13]提出了一种改进的自适应模糊滑模控制,解决了多自由度水下机械臂系统中滑模控制的抖振问题和归一化因子不可用的问题．在轨迹跟踪方面,虽然实现了其机械臂系统能精确地跟踪目标理想轨迹,但是其在建模过程中对水下环境做了静水的假设,这样会使机械臂模型与实际存在误差,进而影响控制精度．赵伟等[14]提出了一种RBF(radial basis function)滑模控制策略,通过仿真对比证明了该算法可以较好地满足水下机械臂系统的控制要求,相比传统滑模控制缩短了关节响应时间并降低了稳态误差．他们所设计的控制算法可以满足其机械臂系统的控制要求,但其同样对水下环境做了静水的假设,会使实际中机械臂控制精度降低．孙国法等[15]设计了一种基于扰动观测器的改进型非奇异快速终端滑模控制策略,很好地补偿了机械臂系统在轨迹跟踪中存在的建模误差,并实现了精确轨迹跟踪控制,但其将各种未知干扰作为一种随机性质的扰动来考虑会使控制精度降低．滑模控制是由Emeleyanov等在20世纪60年代提出的一类控制方法,经过近60年的发展,已经成为非线性控制理论体系的一个重要分支,也不断涌现出各种各样的新方法,如二阶滑模、高阶滑模、终端滑模等．通过趋近律来确定滑模控制的控制律,不仅可以减少趋近阶段的时间,提高系统的鲁棒性,而且能够有效地减弱滑模控制中的抖振问题[16]．Han等[17]提出了一种新的动态系数趋近律,加快了滑模控制系统的响应速度．Pan等[18]设计了一种基于改进的双幂次趋近律积分的滑模控制算法,并将该算法应用于开环不稳定磁悬浮系统．Wang等[19]提出了一种基于改进趋近律的PID(proportional integral derivative)滑模控制方法,可以很好地应用到光伏系统的超级降压转换器中．Sun等[20]提出了由一种基于混合趋近律的滑模控制和扩展滑模干扰观测器组成的控制方法．综上所述,目前水下机械臂控制的研究中大多将水下环境做了静水的假设或将海流扰动简化处理,这样会降低模型精度,需进一步确定海流载荷的大小进而得到精确的机械臂动力学模型来提高控制精度．同时可以看出通过趋近律来确定滑模控制的控制律非常方便且高效,而且还可以提高鲁棒性和减弱抖振．

固定在船底部的机械臂有着各种各样的用途,例如船舶维修[21]、水下抓取物件[22]、水下切割[23]、水下标本采集[24]等,这类机械臂作业时需要固结于船底的基座上,机械臂依赖稳定的基座,需按照规划的路线精准作业．实际水下环境中的机械臂在作业时常常遭受稳定的海流影响,但是现今大多数研究中对水下环境做了静水的假设并没有考虑海流的影响,或只是把海流的干扰当作一种简单的随机扰动来处理,这样会使动力学模型精度不够,进而会影响控制精度．故本文推导出与实际环境更接近的机械臂动力学模型,为提高轨迹跟踪的精度打下基础．现今对二关节机械臂的研究成果较多,但针对二关节水下机械臂的轨迹跟踪控制的研究大多都着眼于控制算法的创新,而对机械臂本身的水下模型精度方面的考虑较少,故考虑海流载荷影响的二关节机械臂模型具有一定的创新意义．本文针对基座静止的二关节机械臂,分别应用Lagrange法和Newton-Euler法推导出其在均匀海流环境下的动力学模型,对比验证了该模型的正确性．针对此模型分别进行了PD控制和滑模控制研究,发现滑模控制能得到满意的轨迹跟踪控制效果．

1 海流环境下二关节机械臂动力学建模

图1为海流环境下的二关节串联机械臂结构,图中m1和m2是机械臂连杆的质量;l1和l2是连杆的长度;θ1和θ2是机械臂关节的转动角度;τ1和τ2是关节的驱动力矩;x1和x2是沿连杆轴线方向的位移;(X1,Z1)和(X2,Z2)是连杆的质心;g是重力加速度,取g=9.8 m/s2．假设机械臂连杆的质量为均匀分布．本文水下机械臂的基座是固定的,同时简化为在坐标系XOZ下考虑机械臂的运动．

图1 二关节机械臂结构Fig.1 Schematic of the 2-joint manipulator

水中作业的机械臂除受到重力等保守力作用外,还要受到海流载荷等非保守力的影响．在应用Lagrange法或Newton-Euler法对二关节机械臂进行建模之前,需计算均匀海流对机械臂的作用力．机械臂在水下运动时所受到的作用力可以表示为

f=fd+fm+ff+fl,

(1)

式中,fd是水阻力;fm是附加质量力;ff是浮力;fl是升力．升力是由于水下运动物体有异形冲角时产生的,本文研究的机械臂各连杆均为规则圆柱体,故升力可忽略不计．

Morison方程[25]给出了水阻力和附加质量力的计算公式:

(2)

1.1 海流载荷

1.1.1 水阻力矩

当机械臂在水中运动时,水由于其黏滞特性会引起水阻力．如图1所示,根据式(2)右边第一项可得二关节机械臂的水阻力矩分别为

(3)

(4)

式中,τd1和τd2是两个关节的水阻力矩;D1和D2是机械臂连杆的直径．

1.1.2 附加质量力矩

当机械臂在水下做加速运动时,由于机械臂和水之间的相对加速度产生的作用于机械臂的反作用力,称为附加质量力．如图1所示,根据式(2)右边第二项和第三项可得二关节机械臂的附加质量力矩分别为

(5)

(6)

式中,τm1和τm2是两个关节的附加质量力矩．

1.1.3 浮力力矩

浮力是物体受到液体对其产生的竖直向上的力,假设机械臂各连杆的重心和浮心位置重合,又由于重力和浮力的方向相反,所以可以在原有的重力项上加一个修正因子来等效替代重力和浮力[27],可根据式(7)求得机械臂各连杆的等效重力为

(7)

式中,G是等效重力;ρi是机械臂连杆的密度;mi是机械臂连杆的质量;Vi是机械臂连杆的体积．

综上所述,海流对两个机械臂的作用力矩分别为τ′1=τd1+τm1和τ′2=τd2+τm2．

1.2 机械臂动力学建模

在应用Lagrange法或Newton-Euler法对二关节机械臂进行建模之前,需求出机械臂的位置、速度坐标、动能和势能．

机械臂两个连杆的位置坐标为

(8)

对位置坐标求导得

(9)

机械臂两个连杆的转动惯量分别为

(10)

机械臂两个连杆的线速度的平方分别为

(11)

机械臂两个连杆的动能分别为

(12)

机械臂两个连杆的势能分别为

(13)

这样,二连杆机械臂系统的总动能和总势能分别为

(14)

1.2.1 Lagrange法

Lagrange函数L被定义为系统的动能K和势能P之差,根据式(14)可以求得此二连杆机械臂系统的Lagrange函数L,即

(15)

系统的动力学方程式,即Lagrange方程定义如下:

(16)

-(τd2+τm2),“-”表示海流作用力对机械臂的运动起阻力作用．

(17)

式(17)中的后两式对时间求导数:

(18)

将式(17)、(18)中相应各导数和偏导数代入式(16),即可分别求得机械臂两个连杆关节处的力矩为

(19)

(20)

1.2.2 Newton-Euler法

用Newton-Euler法求同一个二关节机械臂系统的动力学方程,其一般形式为

(21)

式中,W为保守力所做的功,因为还受到海流作用力Q1(τ′1)和Q2(τ′2)等非保守力的作用,故W=(T1+Q1)θ1+(T2+Q2)θ2,其中T1和T2为机械臂两个连杆关节处的力矩;D为系统的能耗,因为在这里不考虑摩擦力,所以有D=0;其余K和P等与Lagrange法中含义一样．

把式(21)中各项导数与偏导数计算后代入可发现τ1=T1和τ2=T2,故用Lagrange法和Newton-Euler法推导出的机械臂两个连杆关节处的力矩相等,故用这两种方法对水下机械臂建模,对比验证了模型推导的正确性,为后续控制器的设计奠定了坚实可靠的基础．

1.2.3 二连杆机械臂系统动力学模型

把海流载荷τd1,τm1,τd2和τm2计算后代入式(19)、(20)中并整理成矩阵方程的形式,可以总结得到均匀海流环境下二连杆机械臂系统的动力学模型为

(22)

(23)

(24)

(25)

(26)

(27)

(28)

式中

2 跟踪控制方法

2.1 问题表述

本文旨在为固定在船舶底部的水下机械臂建立更精确的动力学模型,因其在水下作业时要受到海流载荷的影响,所以不能单纯地认为机械臂在静水中工作,还要考虑海流的速度和扰动才能使机械臂的动力学模型有更高的控制精度和更接近真实的海洋状况．建立好机械臂的动力学模型,需要选用合适的控制方法来使它能达到理想的轨迹跟踪,即在很短的时间内能使机械臂运动的实际轨迹能跟踪上设定好的理想轨迹,这样才能使机械臂能快速地到达预期位置．所以控制目标就是在很短的时间内能使机械臂的实际轨迹与理想轨迹重合,即θ-θd→0,在这里分别尝试了PD控制与滑模控制．

2.2 PD控制

PD控制是按偏差的比例和微分进行控制的方法,其调节的实质是根据输入的偏差值,按比例和微分的函数关系进行运算,其运算结果用于输出控制．设机械臂的理想轨迹为θd,实际轨迹为θ,PD控制律为

(29)

2.3 滑模控制

滑模控制系统的运动可分为两个阶段,分别为趋近运动阶段和滑模运动阶段．根据滑模变结构原理,滑模可达性条件仅能保证状态空间任意位置运动点在有限时间内到达切换面的要求,而对趋近运动的具体轨迹没有任何限制．因此为了改善趋近运动的动态品质,提出了各种各样的趋近律公式,式(30)—(33)是3种经典趋近律(等速、幂次和指数趋近律)和一种改进指数趋近律:

(30)

(31)

(32)

(33)

式中,ξ,α和k均为控制参数,定义滑模函数为

(34)

式中,取λ1=λ2=5．对式(34)求导得

(35)

将式(22)、(35)与趋近律方程结合就可以得到控制律,如与指数趋近律式(32)结合可以得到基于指数趋近律的滑模控制律:

(36)

选取Lyapunov函数为

(37)

① 可达性

(38)

因此控制系统满足滑模可达性条件．

② 存在性

对式(32)应用Bernoulli方程求解可得

(39)

因此控制系统能够在有限时间内到达切换面．

3 机械臂跟踪仿真

3.1 仿真概况

本文仿真时选用的二关节机械臂系统的相关参数为m1=5 kg,m2=3 kg,l1=0.6 m,l2=0.4 m,D1=0.1 m,D2=0.09 m,ρ1=1 061 kg/m3,ρ2=1 179 kg/m3[28],海水密度ρ=1 025 kg/m3．二关节机械臂的理想轨迹指令分别是θd1=cos(πt),θd2=sin(πt),二关节机械臂的初始位置和初始速度设置为

3.2 PD控制及滑模控制仿真

3.2.1 PD控制

图2 二关节机械臂PD控制不同控制参数下轨迹跟踪与输入力矩Fig.2 Trajectory trackings and input torques of the 2-joint manipulator under different control parameters of the PD control注 为了解释图中的颜色,读者可以参考本文的电子网页版本,后同．

由于机械臂的两个关节中第1个关节离基座更近,受力也更大更复杂,因此在这里仅给出更具代表性和说服力的关节1的轨迹跟踪和输入力矩图．由图2可以看出,该二关节机械臂模型在PD控制方法下基本无法达到精确的轨迹跟踪．当控制参数很大时,轨迹跟踪的误差相对较小,但是此时输入控制力矩的初始启动力矩已经非常大,已经失去了实际意义和可行性,所以PD控制方法不适用于此模型．

3.2.2 滑模控制

选用式(30)—(33)所示的4种趋近律进行滑模控制对比,控制参数取ξ=0.5,α=0.5,k=5,均匀海流速度仍取为u=0.5 m/s,仿真结果如图3所示．

图3 4种趋近律下二关节机械臂滑模控制轨迹跟踪与输入力矩Fig.3 Trajectory trackings and input torques of the 2-joint manipulator under 4 reaching laws of the sliding mode control

如图3,还是仅给出关节1的情况．可以看出,指数趋近律和改进指数趋近律情况下,系统可以在很短的时间内精确跟踪理想轨迹;等速趋近律和幂次趋近律情况下,需要较长时间才可以精确跟踪理想轨迹,幂次趋近律略快于等速趋近律．指数趋近律和改进指数趋近律初始阶段的输入力矩比较小,小于等速趋近律和幂次趋近律,改进指数趋近律在控制抖振方面略优于指数趋近律．因此,选取总体控制效果最佳的改进指数趋近律在不同的控制参数下继续进行实验．

仍然仅给出关节1的情况,由图4可以看出,随着控制参数的增大,达到精确轨迹跟踪的时间基本一致,初始输入力矩逐渐增大,达到稳定后几种情况相差不大．选用初始输入力矩较小情况下的参数ξ=1,k=10,在静水与不同流速u的均匀海流情况下进行仿真对比．

图4 改进指数趋近律在不同控制参数下的二关节机械臂滑模控制轨迹跟踪与输入力矩Fig.4 Trajectory trackings and input torques of the 2-joint manipulator under different control parameters of the improved exponential reaching law of the sliding mode control

如图5所示给出了两个关节的情况,在控制参数为ξ=1,k=10的改进指数趋近律的滑模控制下,静水与不同海流流速情况下均可以快速地实现精确轨迹跟踪．对于输入力矩,在均匀海流情况下,当速度较小时,略大于静水工况,速度为零时的均匀海流也就变成了静水;当速度比较大时,比静水工况大很多,速度越大,输入力矩越大．

图5 静水、不同流速均匀海流下二关节机械臂滑模控制轨迹跟踪与输入力矩Fig.5 Trajectory trackings and input torques of the 2-joint manipulator under still water and uniform ocean currents with different velocities of the sliding mode control

前面仿真时机械臂两个关节的理想轨迹θd1=cos(ωt)和θd2=sin(ωt)中ω取的是π,现将ω取不同的值来使机械臂来跟踪频率不同的理想轨迹,图6分别是机械臂跟踪不同频率的理想轨迹时两个关节的跟踪误差．

图6 不同频率理想轨迹下机械臂两个关节的轨迹跟踪误差Fig.6 Trajectory tracking errors of 2 joints of the manipulator under ideal trajectories with different frequencies

如图6所示,各个频率下机械臂关节1的轨迹跟踪误差基本都在2%～3%之间,关节2的误差更小．研究还发现当理想轨迹的频率为π时,均匀海流速度取许多不同的值时如0.5 m/s,1 m/s,2 m/s等,机械臂关节1的轨迹跟踪误差都为3%左右,关节2的误差同样很小．因此,无论是不同流速的均匀海流还是不同频率的机械臂理想轨迹,机械臂两个关节的轨迹跟踪误差都很小,可以达到较为理想的控制效果．

4 结 论

本文针对基座固定的水下机械臂的水动力特性进行了深入研究,考虑了海流载荷对机械臂动力学建模的影响．基于Morison方程计算海流对机械臂的水阻力和惯性力,然后分别应用Lagrange法和Newton-Euler法推导出均匀海流环境下二关节机械臂的动力学模型,两种方法对比验证了该模型的正确性和可靠性．应用MATLAB软件针对此模型分别进行PD控制和滑模控制仿真实验．从仿真结果可以看出:PD控制下轨迹跟踪存在误差,且控制参数较大时初始输入力矩非常大,表明PD控制不适用于此模型的控制．在不同趋近律、不同控制参数下进行滑模控制,通过仿真对比选出最优趋近律和控制参数,应用此控制系统在静水和不同流速均匀海流环境下进行仿真实验,研究发现此滑模控制系统在各种工况下均可以快速地实现精确轨迹跟踪,同时输入力矩也在合理的范围内．在不同频率的理想轨迹下也都可以快速地实现精确跟踪,所以无论是不同频率的理想轨迹还是不同流速的均匀海流,都可以实现误差很小的精确跟踪．本文推导的机械臂动力学模型可将具有随机性质的海流扰动转化为确定的海流载荷而考虑,故应用此动力学模型可使机械臂轨迹跟踪更精确进而达到预期的工作目标．

致谢本文作者衷心感谢上海交通大学双一流建设项目(WF220401013)对本文的资助．