2. 国家金属采矿工程技术研究中心, 湖南 长沙 410012;
3. 湖南大学汽车车身先进设计制造国家重点实验室, 湖南 长沙 410082
2. National Engineering Research Center for Metal Mining, Changsha 410012, China;
3. Key Laboratory of Advanced Design and Manufacturing for Vehicle Body, Hunan University, Changsha 410082, China
0 引言
21世纪,随着陆地矿区资源的不断枯竭,人类对于矿物能源的需求又日渐上升,能源危机迫在眉睫,海洋赋存有丰富的矿物资源,已知可开采的资源有多金属结核、富钴结壳和多金属硫化物[1-3]。目前已经勘探的海洋资源存储量就已经是陆地上的几百倍,因此海洋采矿亟待发展。我国深海采矿目前处于关键技术的研发与试验阶段,我国的深海采矿系统如图 1所示,海底采矿车从水面支撑系统的母船上吊入海底进行自行式作业。由于多金属结核矿区底质为稀软地质,为最大程度地减少沉陷滑移的影响,因此多金属结核矿区的海底采矿车为履带式海底采矿车。
目前,海底采矿车的轨迹跟踪算法已得到广泛研究。中南大学李力、刘少军基于Adams/ATV与Simulink共同搭建机械控制联合仿真平台,利用模糊比例—积分—微分(proportion integration differentiation,PID)控制左右履带的速度进行直线行驶时的单边越障工况的仿真,利用点镇定控制理论完成了海底采矿车作业轨迹跟踪仿真[4-6]。实际海底采矿车不一定能按预设轨迹作业,可能会遇到障碍物需要绕行,也可能出现受困的工况。中南大学戴瑜教授基于RecurDyn搭建了海底采矿车的机械模型,与Simulink共同搭建机械控制联合仿真平台,利用模糊PID进行海底采矿车的预设轨迹跟踪,在直线轨迹跟踪时可以取得很理想的跟踪效果,但是在转弯时误差较大[7]。
现有的海底采矿车轨迹跟踪算法难以实现对预设轨迹的精准跟踪。而模型预测控制(MPC)和滑模变结构控制(SMC)等基于现代控制理论的轨迹跟踪算法在自动驾驶和水下车辆的轨迹跟踪中已得到成功应用[8-10]。吉林大学张家旭等人基于带有前轮偏角扰动项的四轮车的运动学模型,设计了带有饱和积分项的滑模控制器,通过李雅普诺夫函数证明了路径跟踪算法稳定性,在弯道超车和高速工况路径追踪的Carsim & Simulink联合仿真实验结果中表现出了较好的轨迹跟踪效果[11-12]。文[13]基于平衡车的动力学模型,运用分层滑模控制分布控制平衡车的控制执行器的输入量,实现了对预设轨迹的精准跟踪。文[14]基于自动驾驶车辆的动力学模型运用线性时变模型预测控制算法(LTV-MPC)在Carsim & Simulink联合仿真平台进行双移线轨迹跟踪。文[15]通过贝塞尔曲线拟合的方法生成实时的期望轨迹,利用非线性模型预测控制(NMPC)进行轨迹跟踪,相比较LTV-MPC,NMPC的轨迹跟踪精度更高。文[16]提出了一种应用于水下车辆轨迹跟踪的基于李雅普诺夫理论的模型预测控制器,给出了详细的建模过程和引入反馈控制器的稳定性分析,通过对比仿真实验验证了算法的可行性。
当外界干扰较或模型不精确时,单一的轨迹跟踪算法可能无法实现对预设轨迹的精准跟踪。通过模型预测控制实现外环速度控制,而在控制器的执行输入时采用滑模控制的轨迹跟踪算法被成功应用于存在强扰动环境下的水下车辆和机器臂轨迹跟踪[17-19]。
模型预测控制和滑模控制离不开准确的车辆动力学模型或运动学模型。然而,在海底多金属结核矿区的稀软底质上,海底采矿车容易发生履带打滑、瞬时转动中心不规则变化和车体沉陷的现象,导致运动学或动力学模型不准确。同时,海底作业不确定因素较多,要求海底采矿车能够实时地响应轨迹跟踪算法的控制输入,采用文[17-19]的轨迹跟踪方法,不可避免地累积了两个算法的计算时间,会导致轨迹算法实时性较差。
为此,本文基于海底采矿车运动学模型,设计了一种滑模控制(SMC)与模型预测控制(MPC)相结合的轨迹跟踪方法,该方法首先构建可快速收敛轨迹跟踪误差的SMC控制率,而后采用少预测、控制时域的MPC进一步优化SMC控制率,以期在提升轨迹跟踪精度的同时,保证良好的算法实时性。
1 海底采矿车运动学模型海底采矿车搭载多普勒测速仪、光纤陀螺、履带驱动电机编码器等传感器,实时获取海底采矿车的运动信息。光纤陀螺给出精准的海底采矿车航向角φ和角速度ω。多普勒测速仪给出履带车速度v。履带驱动电机编码器给出海底采矿车左右履带驱动轮的转速ω1和ω2。海底采矿车质心的坐标(x,y) 通过海底采矿车搭载的超短基线声学定位系统获取。
海底采矿车作业的行驶速度通常不超过1 m/s,故在建模时可以不考虑离心力及瞬时转向中心的变化。参照现有的履带车运动学模型,结合海底采矿车的实际运动状态,建立海底采矿车的简化运动学模型。海底采矿车以左右履带驱动轮的转速ω1和ω2为控制量,状态量为海底采矿车质心的坐标(x,y)和航向角φ,如图 2所示。
海底采矿车的运动学关系如式(1)所示:
(1) |
式中,r为履带驱动轮半径,i1和i2为左右履带打滑率,B为左右履带板的距离。
当海底采矿车进行轨迹跟踪时,将海底采矿车视为差速移动机器人,其运动学方程如式(2)所示。基于当前时刻的状态量误差,得到质心速度v和质心角速度ω的控制率U(t)=[v(t),ω(t)],最后将所得的控制率通过式(1)转换成海底采矿车左右履带的实际转速输入ω1和ω2。
(2) |
滑模控制(SMC)具有快速收敛误差且计算时间较短的特点,但是在真实系统中所采用的离散趋近率存在抖振现象。而线性时变模型预测控制(LTV-MPC)作为一种特殊的最优控制具有预测优化的特点。本文基于SMC设计一种快速收敛跟踪误差的控制率,通过选择合适的预测时域和控制时域,基于LTV-MPC对该控制率进行预测优化,达到消除控制率可能带来的抖振现象以提高轨迹跟踪精度,而LTV-MPC通过求解标准的二次规划在线计算时间较短,可以满足真实系统对算法实时性的要求。
海底采矿车通过测控系统实时反馈带有过程噪声的状态量X(t)=[x0(t),y0(t),φ0(t)]T和控制输入U(t)=[v(t),ω(t)]T,经过自适应卡尔曼滤波器降噪处理后输出XKF(t)=[x(t),y(t),φ(t)]T,基于与同时刻参考轨迹的参考状态量Xref(t)生成误差反馈信息生成下一时刻的滑模控制率U(t+1)=[v(t+1),ω(t+1)]T,通过模型预测控制滚动优化,得到控制率U*(t+1)=[v*(t+1),ω*(t+1)]T,最后基于运动学关系式1输出下一时刻左右履带驱动轮的转速ω1(t+1)和ω2(t+1),其中履带打滑率计算方法采用文[20]中的计算方法,控制算法示意图如图 3所示。
2.1 滑模控制率设计为了更直观地描述海底采矿车的运动状态,可以通过某种映射将海底采矿车的全局坐标投影至海底采矿车的局部坐标系中,通过正交旋转矩阵即可完成该映射,当前时刻的姿态误差由式(3)所示,海底采矿车的姿态误差如图 4所示。
在坐标系中,为海底采矿车当前时刻的位姿,为海底采矿车当前时刻的参考位姿。
(3) |
对式(3)进行微分:
(4) |
通过该映射关系,可以更直观地表达当前海底采矿车位置同目标姿态的误差,以便于设计控制律。
定义滑模切换函数:
(5) |
采用如下滑模趋近率:
(6) |
其中,k1,k2为正数,0 < α1 < 1,0 < α2 < 1,sat(s)为饱和函数:
(7) |
可以使原本不连续的系统变为连续平滑的系统,以达到有效缓解滑模控制中的抖振现象的目的。
由式(3)~式(5)和式(7)可得滑模控制率:
(8) |
由系统存在滑模态的条件为[21]
(9) |
故该设计满足滑模态的存在条件,可以保证滑模控制率收敛。
2.2 滑模控制率稳定性证明选李雅普诺夫候选函数为
(10) |
当系统位于滑模面上时,有:
(11) |
故式(10)可变形为
(12) |
将式(12)微分后联立式(4)和式(11):
(13) |
易得
本文采用线性时变模型预测控制(LTV-MPC)对式(8)的滑模控制率U进行预测优化。LTV-MPC在每一时刻的参考轨迹点处利用雅可比线性化的方法对式(2)进行离散化处理,以每一时刻的状态量偏差X(t|t)和控制量偏差X(t|t)为输入,如式(14)所示:
(14) |
式中T为采样周期,其中:
线性时变模型预测控制的目标函数如式(15)所示:
(15) |
式中,F、E分别为状态量和控制量的误差权重矩阵,Sp和Sc为预测时域和控制时域。
参考文[24]将代价函数(15)转变为标准二次型。通过求解标准的二次规划来求解式(15)的优化问题,即可得到控制律
(16) |
海底采矿车主要沿着直线轨迹进行自行式采集作业,故在海底采矿车进行直线行驶作业时,在无限预测时域、有限控制域Sc内证明直线行驶时的名义模型稳定性,将式(14)作小角度假设,此时At和Bt为
(17) |
在无限预测时域、有限控制域Sc内,目标函数(16)改写为
(18) |
当t>Sc:
(19) |
计算
(20) |
其中:
(21) |
当(AtSc,At)可观时,式(21)中的P存在正定解,且假设矩阵P是不变的。
在J(t)中
(22) |
将式(22)代入式(18):
(23) |
定义1 定义优化目标函数:
(24) |
且满足J*(t)≤J(t)。其中:
通过求解(15)的优化问题,可以得到控制率κ*使得:
(25) |
定义2 [23] 如果连续函数α:[0,a)→[0,∞)严格递增,α(0)=0,则α属于κ类函数。
定理1 [23] 考虑线性自治离散系统:
(26) |
其中,f关于X连续,且f(0)=0。对于系统(26),如果存在正定有界函数V(X)满足:
a) V(0)=0且对于任意的X≠0有V(0)>0.
b) 在平衡点X附近,V(X)是连续的.
c) 沿着系统的轨迹,有:
其中,α0(·)是一个κ类函数,则系统是稳定的。
将式(25)代入式(14),可得到形如(26)的自治离散系统:
(27) |
其中,平衡点Xe=[0, 0, 0]T。
下一时刻的目标函数:
(28) |
将式(24)代入式(28),有:
(29) |
即:
(30) |
由定义2,易知
本文基于RecurDyn建立海底采矿车的仿真模型,在Simulink中搭建仿真控制系统,用来对本文所提出的海底采矿车轨迹跟踪控制算法进行验证。多金属结核海底采矿车的预设采集轨迹如图 5所示。实际上海底采矿车可能会面临无法越过的坡,投影后可近似认为是圆形,避障轨迹由3个等曲率的圆弧组成,如图 6所示。
3.1 RecurDyn海底采矿车机械模型海底采矿车的机械模型如图 7所示,模型参数记录在表 1中,其采用真实海底采矿车的参数进行标定。
为了使仿真结果更真实可信,在RecurDyn建模时,通过调整设置土壤抗剪切角等参数,将履带与地面接触的地面模拟成多金属结核矿区的稀软底质,如图 8所示。
3.2 预设轨迹跟踪数值仿真结果为了验证该轨迹跟踪算法是否能跟踪预设轨迹,分别做了如下两种预设轨迹的数值仿真实验:1)预设轨迹无障碍物;2)预设轨迹存在障碍物。其中LTV-MPC的预测时域=25和控制时域=2。计算机处理器为Inter® Core(TM)i7-9750H CPU @2.60 GHz 2.59 GHz。
3.2.1 SMPC算法预测时域确定SMPC的轨迹跟踪精度和计算耗时与预设时域有关,为了确定最合适的SMPC中预测时域,改变SMPC的预测时域进行跟踪预设轨迹存在障碍物的对比仿真实验,其中SMPC的控制时域,仿真结果记录在表 2中。
Sp | 累积耗时/s | 偏差均方差/m | 累积偏差/m |
4 | 358 | 0.129 | 13.117 |
5 | 362 | 0.096 | 9.707 |
6 | 364 | 0.084 | 8.498 |
7 | 370 | 0.076 | 7.641 |
8 | 375 | 0.067 | 6.833 |
9 | 383 | 0.064 | 6.462 |
10 | 415 | 0.064 | 6.462 |
11 | 431 | 0.064 | 6.461 |
12 | 502 | 0.064 | 6.461 |
由表 2可知,随着预测时域的增加,轨迹偏差会减小,同时SMPC的在线计算时间会增加。当预测时域增加到9步后,再增加预测时域,轨迹跟踪的累积偏差减少得不明显,相反计算时间增加得很快。故在SMPC中将预测时域Sp设置为9可以在没有过长的在线计算时间的前提下精准地跟踪预设轨迹。
3.2.2 预设轨迹无障碍物由图 9可知,LTV-MPC、SMC和SMPC轨迹跟踪算法可以跟踪预设的轨迹,在海底采矿车完成转向后,3种轨迹跟踪算法轨迹跟踪的误差也均可以收敛,但由表 4所示,3种轨迹跟踪算法的计算耗时存在差异,轨迹跟踪精度不一。
由图 10可知,虽然在海底采矿车跟踪直线轨迹时,3种控制算法的轨迹偏差相差不大,轨迹偏差可以收敛,而转弯时的轨迹偏差虽然无法收敛,但是保持在可以接受的范围内。不难发现,在转弯的时候,LTV-MPC轨迹跟踪时的位置偏差较大。相比于LTV-MPC,虽然SMC的轨迹跟踪偏差已经有所降低,但是SMPC的轨迹偏差比SMC的偏差更小。因此SMPC能够最精准地跟踪转弯时的圆形轨迹。
LTV-MPC算法能否精准预设轨迹,不仅与代价函数(14)的权重矩阵和有关,更与预测时域Sp和控制时域Sc有关,严格来说,LTV-MPC的预测时域和控制时域越长,轨迹跟踪精度越高,但这会增加算法的在线计算时间。由表 3可知,SMPC的累积偏差最小,且SMPC的滚动优化时的预测时域Sp=9和控制时域Sp=2,与LTV-MPC的预测时域Sp=25和控制时域Sc=2相比,从而降低了在线计算时间,SMPC的计算耗时比LTV-MPC少了约17 s。故相比于单一的LTV-MPC和SMC,SMPC能取得更好的轨迹跟踪效果,且在线计算时间较短。
3.2.3 预设轨迹存在障碍物由图 11可知,LTV-MPC、SMC和SMPC的轨迹与期望的轨迹偏差不大,在避障后均可以跟踪预设轨迹,且误差可以收敛,故3种算法均能够跟踪期望的轨迹,但由表 3可知,SMPC的根据跟踪偏差相比单一的两种算法,更能精准地跟踪预设轨迹。
由图 11和图 12可知,在跟踪直线轨迹时,3种算法均能精准地跟踪预设轨迹。在跟踪等曲率三圆弧的避障轨迹时,LTV-MPC的轨迹偏差最大,SMC的轨迹偏差相对较小,SMPC的轨迹跟踪偏差最小,因此SMPC在避障时的轨迹跟踪效果最理想。
由3.2.1节分析可知,海底采矿车跟踪圆弧轨迹时的轨迹跟踪误差无法收敛,故海底采矿车在跟踪连续的等曲率三圆弧避障轨迹时,会不可避免地累积误差,预测时域Sp和控制时域Sc越长,相对的累积误差也会越多。由表 4可知,SPC的累积偏差均远小于LTV-MPC和SMC的累积偏差,因此通过减少滚动优化时的预测时域Sp和控制时域Sc,可以减少海底采矿车跟踪等曲率三圆弧时累积的偏差,进而实现海底采矿车对预设轨迹的精准跟踪,且计算时间较短。故当海底采矿车需要避障时,SMPC最适合用于海底采矿车跟踪预设轨迹。
4 结论本文针对单一轨迹跟踪算法难以实现海底采矿车对期望轨迹特别是圆弧轨迹的精准跟踪的问题,提出了一种海底采矿车的滑模预测控制轨迹跟踪算法。针对现有的先模型预测控制外环速度控制,后滑模控制内环动力学输入控制在线计算时间过长的问题,提出了先通过滑模控制收敛轨迹跟踪误差,后采用少预测时域的模型预测控制方法优化滑模控制率,实现了在不额外增加计算时间的前提下对期望轨迹的精确跟踪。基于李雅普诺夫第二法证明了滑模控制率收敛,且模型预测控制所得到的优化控制率是稳定的,保证了控制系统的闭环稳定性。
[1] | Lesage M, Juliani C, Ellefmo S L. Economic block model development for mining seafloor massive sulfides[J]. Minerals, 2018. DOI:10.3390/min8100468 |
[2] | Volkmann S E, Kuhn T, Lehnen F. A comprehensive approach for a techno-economic assessment of nodule mining in the deep sea[J]. Mineral Economics, 2018, 31(3): 319–336. DOI:10.1007/s13563-018-0143-1 |
[3] |
戴瑜, 刘少军.
深海采矿机器人研究: 现状与发展[J]. 机器人, 2013, 35(3): 363–375.
Dai Y, Liu S J. Researches on deep ocean mining robots: Status and development[J]. Robot, 2013, 35(3): 363–375. |
[4] |
韩庆珏, 刘少军.
深海履带车的路径跟踪控制算法[J]. 中南大学学报(自然科学版), 2015, 46(2): 472–478.
Han Q J, Liu S J. Path tracking control algorithm of the deep sea tracked vehicle[J]. Journal of Southern University (Natural Science Edition), 2015, 46(2): 472–478. |
[5] |
李力, 张正, 陈铭.
基于Lyapunov理论的海底采矿车点镇定控制[J]. 中南大学学报(自然科学版), 2014, 45(8): 2624–2628.
Li L, Zhang Z, Chen M. Point stabilization of seabed mining vehicle based on Lyapunov theory[J]. Journal of Southern University (Natural Science Edition), 2014, 45(8): 2624–2628. |
[6] |
李力, 邹砚湖.
海底采矿车路径跟踪的变论域模糊控制[J]. 中南大学学报(自然科学版), 2012, 43(2): 489–496.
Li L, Zou Y H. Tracking moving path of seabed mining vehicle based on theory of variable universe fuzzy control[J]. Journal of Southern University (Natural Science Edition), 2012, 43(2): 489–496. |
[7] | Dai Y, Su Q, Zhang Y. A new dynamic model and trajectory tracking control strategy for deep ocean mining vehicle[J]. Ocean Engineering, 2020. DOI:10.1016/j.oceaneng.2020.108162 |
[8] |
熊璐, 杨兴, 卓桂荣.
无人驾驶车辆的运动控制发展现状综述[J]. 机械工程学报, 2020, 56(10): 127–143.
Xiong L, Yang X, Zhuo G R. Review on motion control of autonomous vehicles[J]. Journal of Mechanical Engineering, 2020, 56(10): 127–143. |
[9] | Nayl T, Nikolakopoulos G, Gustafsson T, et al. Design and experimental evaluation of a novel sliding mode controller for an articulated vehicle[J]. Robotics and Autonomous Systems, 2018. DOI:10.1016/j.robot.2018.01.006 |
[10] |
龚建伟.
无人驾驶车辆模型预测控制[M]. 第2版. 北京: 北京理工大学出版社, 2020.
Gong J W. Model predictive control for self-driving vehicle[M]. 2nd ed. Beijing: Beijing University of Technology Press, 2020. |
[11] |
张家旭, 周时莹, 施正堂.
采用滑模条件积分的无人驾驶汽车弯道超车路径规划与跟踪控制[J]. 控制理论与应用, 2021, 38(2): 197–205.
Zhang J X, Zhou S Y, Shi Z T. Path planning and tracking control for corner overtaking of driverless vehicle using sliding mode technique with conditional integrators[J]. Control Theory and Applications, 2021, 38(2): 197–205. |
[12] |
张家旭, 王欣志, 赵健.
汽车高速换道避让路径规划与离散滑模跟踪控制[J]. 吉林大学学报(工学版), 2021, 51(3): 1081–1090.
Zhang J X, Wang X Z, Zhao J. Path planning and discrete sliding mode tracking control for high-speed lane changing collision avoidance of vehicle[J]. Journal of Jilin University (Engineering), 2021, 51(3): 1081–1090. |
[13] | Chen L, Wang H, Huang Y, et al. Robust hierarchical sliding mode control of a two-wheeled self-balancing vehicle using perturbation estimation[J/OL]. Mechanical Systems and Signal Processing, 2020. [2020-11-16]. https://doi.org/10.1016/j.ymssp.2019.106584. |
[14] |
潘世举, 李华, 苏致远.
基于跟踪误差模型的智能车辆轨迹跟踪方法[J]. 汽车工程, 2019, 41(9): 1021–1027.
Pan S J, Li H, Su Z Y. Trajectory tracking method for intelligent vehicles based on tracking-error model[J]. Automotive Engineering, 2019, 41(9): 1021–1027. |
[15] | Al-Mayyahi A, Aldair A A, Rashid A T. Intelligent control of mobile robot via waypoints using nonlinear model predictive controller and quadratic bezier curves algorithm[J]. Journal of Electrical Engineering & Technology, 2020, 15(4): 1857–1870. |
[16] | Shen C, Shi Y, Buckham B. Trajectory tracking control of an autonomous underwater vehicle using lyapunov-based model predictive control[J]. IEEE Transactions on Industrial Electronics, 2018, 65(7): 5796–5805. DOI:10.1109/TIE.2017.2779442 |
[17] | Saback R M, Conceicao A G S, Santos T L M, et al. Nonlinear model predictive control applied to an autonomous underwater vehicle[J]. IEEE Journal of Oceanic Engineering, 2020, 45(3): 799–812. DOI:10.1109/JOE.2019.2919860 |
[18] | Yao X, Wang X, Zhang L, et al. Model predictive and adaptive neural sliding mode control for three-dimensional path following of autonomous underwater vehicle with input saturation[J]. Neural Computing & Applications, 2020, 32(22): 16875–16889. |
[19] | Incremona G P, Ferrara A, Magni L. MPC for robot manipulators with integral sliding modes generation[J]. IEEE-ASME Transactions on Mechatronics, 2017, 22(3): 1299–1307. DOI:10.1109/TMECH.2017.2674701 |
[20] |
张敏. 基于模糊神经网络的海底采矿车路径跟踪行走控制研究[D]. 长沙: 中南大学, 2010. Zhang M. Fuzzy neural network based path tracking travel control for seabed mining vehicles[D]. Changsha: Central South University, 2010. |
[21] |
刘金琨.
滑模变结构控制MATLAB仿真基本理论与设计方法[M]. 第3版. 北京: 清华大学出版社, 2015.
Liu J K. Sliding mode control design and MATLAB simulation: The basic theory and design method[M]. 3rd ed. Beijing: Tsinghua University Press, 2015. |
[22] |
张嗣瀛.
现代控制理论[M]. 第2版. 北京: 清华大学出版社, 2017.
Zhang S Y. Modern control theory[M]. 2nd ed. Beijing: Tsinghua University Press, 2017. |
[23] |
陈虹.
模型预测控制[M]. 北京: 科学出版社, 2013.
Chen H. Model predictive control[M]. Beijing: Science Press, 2013. |