0 引言
球形机器人凭借其球形滚动外壳、内部单摆式驱动结构以及整体重心靠下的球形结构特征,具有结构简单、转弯半径小、运动速度快、运动方向容易控制和不容易侧翻倾覆等优点,能更好地适应野外复杂地形[1]。同时球形机器人的通信、驱动、检测等装置被内置于封闭球形外壳中,避免了与外界环境直接接触造成损坏的危害[2-4]。因此,球形机器人在野外复杂环境中有着巨大的探测[5]、巡检[6]、反恐救灾[7]等应用价值。
然而球形机器人动力学模型欠驱动、强耦合、非线性、非链式以及受非完整约束的特点[8-9],使得在球形机器人的运动控制问题上存在无法直接应用很多处理链式化系统控制方法的问题。目前国内外学者针对球形机器人轨迹跟踪问题已开展了大量研究,主要有反馈线性化结合模糊控制方法[10]、反步法结合自适应控制算法[11]、模糊导向策略结合分层滑模算法[12]、双环PD控制算法结合RBF神经网络[13]等方法设计了轨迹跟踪控制器。这些轨迹跟踪方法在一定程度上实现了对所设定参考轨迹的跟踪,但对系统建模的精准度有较高的要求,而且算法的设计和调试难度也比较大,需要对多种参数进行调节和优化。这些因素会导致所设计轨迹跟踪控制器在面临系统模型复杂度较高、模型不完整以及有外部扰动时出现跟踪稳定性变差的问题,跟踪精度也会有所欠缺。而模型预测控制(model predictive control,MPC)算法具有处理复杂非线性和多变量系统模型轨迹跟踪控制问题的性能[14-15],对于系统建模误差和测量误差等还具备一定的鲁棒性。MPC算法能在对控制变量、控制增量限制的情况下,通过对系统未来状态和输入变量进行优化计算,确保轨迹跟踪精度和保持系统的稳定性。
基于上述认识,为解决现存球形机器人轨迹跟踪控制方法精度不高、跟踪稳定性欠缺的问题,本研究设计了一种基于MPC算法的球形机器人轨迹跟踪方法。首先运用拉格朗日方法对单摆驱动式球形机器人进行运动学与动力建模,并对建立的动力学模型进行合理简化;然后在简化的球形机器人动力学模型和运动学模型的基础上设计MPC轨迹跟踪控制器,其中包括对目标函数的设计以及约束条件的确定;最后进行仿真实验进行轨迹跟踪效果验证,结果表明与传统的PID轨迹跟踪算法对比具有更良好的轨迹跟踪性能。
1 单摆驱动式球形机器人动力学建模
1.1 球形机器人结构模型
单摆驱动式球形机器人是由驱动重摆改变球形机器人质心位置进而使球形机器人滚动的机器人,主要由球壳、连接圆盘、中心框架、长轴电机、短轴电机、联轴器、单摆质量块等组成。具体机械结构三维模型如图1所示,由图中可见当球形机器人的长轴电机输出转矩时,将使单摆质量块等绕长轴转动从而改变球形机器人质心位置向前进行直线运动。当长短轴电机同时输出转矩时,将能改变球形机器人向前滚动姿态从而实现球形机器人转向运动。
图1 单摆驱动式球形机器人三维模型
Fig.1 Three-dimensional model of a pendulum-driven spherical robot
由单摆驱动式球形机器人三维结构模型可知,其能简化为球壳、中心框架、单摆3部分。球壳作为整体机器人的载体,能绕着中间框架短边对称轴(长轴)在水平面上旋转滚动。中间框架则通过长轴旋转副与球壳连接,绕着球壳长轴即自身长轴在旋转,可以简化为围绕着球壳长轴旋转的刚体。重摆则可以简化为中央框架下绕着中央框架长边对称轴(短轴)进行转动的质点。则球形机器人结构简图以及坐标关系如图2所示。其中各坐标系定义如下:Oxyz为大地坐标系,原点为O点;AxByBzB为固连在球壳的坐标系,原点A位于球壳中心;AxCyCzC为固连在框架中心的坐标系,原点A同样位于球壳中心。其中轴zB与轴zC重合。具体各参数符号含义如表1所示。
表1 球形机器人各参数符号定义
Table 1 Definition of symbols for parameters of a spherical robot
参数符号各符号物理含义α,β,γ球形机器人的进动角、章动角、自转角θ中心框架相对于球壳的旋转角φ单摆质量块相对于中心框架的旋转角X,Y,Z大地坐标系中的球心A坐标m1均质球壳的质量m2中心框架的质量,包括框架板、电机等m3单摆的质量,包括配重块、电池等mm1、m2和m3之和r球壳半径l单摆摆长τ1,τ2长短轴电机驱动力矩I1xx,I1yy,I1zz球壳的主转动惯量I2xx,I2yy,I2zz中心框架的主转动惯量
图2 球形机器人结构简图以及坐标
Fig.2 Schematic diagram and coordinates of a spherical robot structure
1.2 球形机器人运动学方程
定义大地坐标系Oxyz的单位向量为(i, j,k),定义球壳坐标系AxByBzB单位向量为(l,m,n),令Sα=sinα和Cα=cosα,则球壳坐标系AxByBzB变换到大地坐标系Oxyz的旋转矩阵
以及两者单位向量转换关系如下:
(1)
将进动角速度
章动角速度
和自转角速度
投影到大地坐标系上能求得球形机器人角速度ω,由式(1)得:
(2)
假设球形机器人在水平面上滚动时无滑动,则球壳在水平面接触点Q的速度vQ=0,则由球心速度v与Q点速度关系得:
vQ=v+ω×rQ
(3)
式(3)中:
rQ=-rk
rQ为球壳A点到与水平面接触点Q点的矢量,显然由式(2)和式(3)可以求得球形机器人运动学方程如下:
(4)
(5)
(6)
1.3 球形机器人动力学方程
由于球形机器人被简化为球壳、中心框架和单摆3部分,在采用拉格朗日方法对其动力学的建模过程中,需要分别对以上3部分进行动能和势能分析,并将求得的动能与势能作差得到球壳、中心框架和单摆的拉格朗日函数。最终整合为球形机器人系统的拉格朗日函数,同时采用具有非完整约束的拉格朗日方程,可以求得球形机器人总体系统的动力学方程形式如下:
(7)
其中,M(q)∈R7×7为惯性矩阵,
为离心力和哥氏力非线性项,其他各项具体形式如下:
由式(7)知该动力学模型存在状态变量较多,方程非线性、复杂度大以及变量耦合度强的特点,因此不适用于直接设计轨迹跟踪控制器。文献[16]给出了球形机器人动力学模型合理简化、线性化的方案。因为球形机器人单摆质量块在球形机器人总质量中占比较大,所以在球形机器人运动过程中,单摆偏离Z轴的角度很小,因此合理假设θ≈0和
又因为球形机器人在实际运动过程中直线行走角速度远大于姿态角速度,并且单摆基本保持在垂直位置,所以可以合理假设sinβ≈1,cosβ≈0,最终得到简化、线性化后的球形机器人动力学模型如下:
(8)
(9)
(10)
其中, f为球形机器人绕轴zB旋转受到的阻力矩。
2 轨迹跟踪控制器设计
2.1 模型预测控制原理
模型预测控制原理如图3所示。在K时刻,根据动力学模型反馈的状态变量,结合参考轨迹对应的参考状态变量、参考控制变量,得到的误差模型通过离散化处理建立预测时域内的二次规划问题进行解算,得出控制时域内的最佳预测控制量,将其输入动力学模型可以得到预测轨迹进而逼近参考轨迹。在下一个滚动更新周期前,以上个周期中的第一个预测控制量作为动力学模型的输入得到的下一个时刻的状态量,继续与下一时刻参考轨迹所对应的参考状态量,输入到预测模型,并重新解算二次规划问题的目标函数得到最佳控制量。通过上述滚动优化的流程从而不断实现对参考轨迹的精准跟踪。
图3 模型预测控制原理示意图
Fig.3 Schematic diagram of model predictive control principle
2.2 球形机器人轨迹跟踪预测模型
由于球形机器人运动轨迹与球形机器人自身姿态角紧密相关,因此球形机器人的误差模型需要考虑进动角α和章动角β与参考轨迹转化的参考进动角αr和参考章动角βr的偏差关系。综合球形机器人运动学模型式(4)、式(5)和动力学模型式(8)、式(9)进行变换得到新的球形机器人动力学模型形式如下:
(11)
式(11)中:
a=I1yy+I2yy+mr2
b=I1zz+mr2
式(11)表明球形机器人动力学模型本质上可表达为一个以
为控制量和以χ(X,Y,α,β)为状态量的控制模型。由式(10)可知自转角速度与长轴力矩τ1相关,故模型控制量可转换为u(τ1,τ2)进行控制。式(11)模型一般形式可由下式表示:
(12)
由于给定参考轨迹能通过球形机器人运动学模型进行运算得到状态参考量,因此参考轨迹上每一个点都能满足式(12),所以状态参考量χr=[Xr,Yr,αr,βr]T与控制参考量
的关系表达式可表达如下:
(13)
将式(12)运用泰勒公式在参考轨迹点处展开,保留低级项和忽略高级项,得到式子如下:
(14)
将式(14)减去式(13)可以得到线性化后的球形机器人误差模型,模型具体形式如下所示:
(15)
式中:
为了将线性化后的球形机器人误差模型应用于轨迹跟踪器的设计,还需要对式(15)进行离散化处理。
(16)
由式(16)可以求得K+1时刻状态量误差表达式,即预测模型表达式如下:
(17)
式(17)中:
其中T为采样时间。
2.3 目标函数的设计
为了方便后续目标函数对每个采样周期内的控制增量进行限制,重新构造一个新的状态变量
可将式(17)转换为一个新的状态空间方程:
q(K+1)=A2q(K)+B2Δu(K)
Q(K)=Cq(K)
(18)
式(18)中:
Δu(K)=u(K)-u(K-1)
令Np为预测步数,Nc为控制步数,(Np≥Nc+1),则预测步数内各预测输出量表达式为
ξ=ΨQ+ΘΔU
(19)
式(19)中:
(20)
目标函数需要确保球形机器人能快速平稳对参考轨迹进行跟踪,采用如下二次型函数作为目标函数的形式:
(21)
式(21)中:第一项反映的是对轨迹的跟踪能力,第二项反映的是对控制增量的约束,防止控制量突变导致轨迹不能连续平滑,第三项为松弛项,能防止求解过程没有可行解的情况出现。其中矩阵W为半正定预测时域权重矩阵,矩阵R为正定控制时域权重矩阵,ρ为权重系数,ε是松弛因子。将式(19)代入式(20)并将其转化为二次规划问题,同时考虑系统控制量和控制增量的约束条件,整理得:
(22)
式(21)中:
U=1NC⊗u(K-1)
(23)
其中, 1Nc是行数为NC的列向量,符号⊗为克罗内克积。在第一个采样时间T里,对式(21)的二次规划问题进行求解得到最优控制增量序列为{Δu(K), Δu(K+1),…,Δu(K+NC-1)},取第一项控制增量通过式(22)计算出K时刻的控制量,将其输入动力学模型从而计算得到预测轨迹,在下一个采样时间T里重复上述过程继续获得最优控制量从而实现对系统的最优控制。球形机器人模型预测控制轨迹跟踪流程如图4所示。
u(K)=u(K-1)+Δu(K)
(24)
图4 模型预测控制轨迹跟踪流程
Fig.4 Flow chart of trajectory tracking process for model predictive control
3 轨迹跟踪仿真结果与分析
3.1 仿真实验动力学模型参数
为了验证所设计球形机器人轨迹跟踪控制器的有效性与合理性,通过Matlab语言编写Matlab-Function程序作为球形机器人动力学模型,编写S-Function程序作为MPC控制器,最后采用Matlab/Simulink软件平台搭建整体仿真模型进行仿真测试。仿真模型中的球形机器人结构参数为:球壳质量为1 kg,中心框架质量为1 kg,单摆质量块为4 kg,球壳外直径为0.3 m以及单摆摆长为0.1 m,路面摩擦系数为0.03。
由于双移线能够适应包括直线段、圆弧和曲线等不同形状的路径,因此广泛应用于各种类型的轨迹跟踪控制任务中作为轨迹跟踪曲线。本研究也采用双移线作为参考轨迹,其具体函数表达形式如下所示:
(25)
式(23)中:
(26)
其中, 参数xref和yref指的是双移线参考轨迹上每一点横向和纵向位置,xref在后续仿真实验中取值区间为[0,120]。而参数dx1和dy1用于控制参考轨迹第一段曲线横向和纵向形状的变化。当dx1越小,第一段曲线横向上的位置变化越缓慢,反之变化越迅速;当dy1越小,第一段曲线纵向上的位置越高,反之越低。参数dx2和dy2对参考轨迹第二段曲线形状控制原理与参数dx1和dy1相同。
3.2 轨迹跟踪仿真实验
在大地坐标系之下,球形机器人初始状态为:
球形机器人参考自转角速度通过球形机器人运动学结合参考轨迹计算得到,初始长短轴电机力矩均为0。MPC轨迹跟踪控制器参数分别为:采样时间T为0.01 s,预测步数Np为30,控制步数Nc为15,预测时域权重矩阵系数W为1,控制时域权重矩阵系数R为50,权重系数ρ为1,松弛因子ε取10。而传统PID方法应用在球形机器人轨迹跟踪中,根据简化后的球形机器人动力学模型分为长轴速度控制与短轴姿态控制。综合考虑PID控制三环节优缺点,长轴速度控制选取了PI控制方法,仿真参量为
短轴姿态控制选取串级PD控制方法,外层PD仿真参量为Kpα = 11,Kdα = 3,内层PD仿真参量为Kpβ = 8,Kdβ = 10。在给定同样初始条件的情况下,MPC轨迹跟踪控制方法与传统PID控制方法对比结果如图5所示。
图5 轨迹跟踪效果对比
Fig.5 Comparison chart of trajectory tracking results
由图5的轨迹跟踪效果图可见,PID轨迹跟踪方法在接近轨迹弯道处时会出现较大的偏差,需要一段距离过后进行轨迹修正,在最后一个弯道处偏差增大,没有与参考轨迹贴合。相比之下MPC轨迹跟踪方法更能贴合参考轨迹,在参考轨迹的全过程中都有很好的跟踪效果,在弯道处也只出现轻微的偏差。通过图6和图7的横向误差和纵向误差对比图更能直观看到,PID轨迹跟踪方法在横向跟踪过程中会留有静差,而在纵向跟踪过程中的误差则越来越大,相比之下MPC轨迹跟踪方法无论在横向还是纵向跟踪都具有更小的跟踪误差,误差始终能保持在0左右。
图6 横向跟踪误差对比
Fig.6 Comparison chart of lateral tracking error
图7 纵向跟踪误差对比
Fig.7 Comparison chart of longitudinal tracking error
由图8和图9可以看到,无论是PID轨迹跟踪方法还是MPC轨迹跟踪方法都具有几乎相同的姿态角跟踪性能,跟踪误差都能保持在10-3数量级之中,都具有较好的姿态跟踪效果。
图8 进动角跟踪误差对比
Fig.8 Comparison chart of precession angle tracking error
图9 章动角跟踪误差对比
Fig.9 Comparison chart of nutation angle tracking error
综合对比之下,本研究设计的球形机器人MPC轨迹跟踪控制器对比传统的PID方法具有更好的跟踪参考轨迹性能,横向最大误差为5.675×10-3 m,相比PID方法减少了0.664 4 m。纵向最大误差为4.073×10-2 m,相比PID方法减少了0.112 1 m。姿态角跟踪过程中,由于跟踪误差曲线数值都保持在10-3数量级中,跟踪效果都达到很精确的水平。而在对参考进动角的跟踪过程中,MPC方法误差均值为2.787×10-5 rad, PID方法误差均值为1.290×10-3 rad;在对参考章动角的跟踪过程中,MPC方法误差均值为4.105×10-4 rad,PID方法误差均值为1.007×10-3 rad;并且MPC方法跟踪误差最终能收敛到0附近,PID方法则一直存在一定的误差值。通过分析姿态角跟踪误差曲线均值,可以发现MPC方法具有更小的误差数值,所以在姿态角跟踪过程中MPC方法具有更精准的跟踪效果。因此本研究设计的球形机器人MPC轨迹跟踪方法具有更好的稳定性、收敛性以及更高的跟踪精度。
4 结论
本研究基于MPC原理设计了球形机器人轨迹跟踪控制器,通过仿真发现,所提MPC方法在横向跟踪方面最大误差相比传统PID方法减少了0.664 4 m,纵向跟踪最大误差相比传统PID方法减少了0.112 1 m,跟踪误差曲线基本在0附近波动;在进动角和章动角跟踪方面,所提MPC方法的跟踪误差有着更好的收敛效果,传统方法则有静差的存在。因此,本研究所提方法在球形机器人位置和姿态跟踪方面,相比传统PID方法具有更精准的跟踪效果,并且表现出更良好的跟踪稳定性。
[1] WU H S,LI B Y,WANG F T,et al.Design and analysis of the rolling and jumping compound motion robot[J].Applied Sciences,2021,22(11):10667.
[2] 马龙,孙汉旭,宋荆洲,等.一种球形机器人高速直线运动的自适应控制方法[J].振动与冲击2021,40(6):201-211.MA Long,SUN Hanxu,SONG Jingzhou,et al.Adaptive control method for the high speed linear motion of a spherical robot[J].Journal of Vibration and Shock,2021,40(6):201-211.
[3] 于涛,王益博,孙汉旭,等.基于干扰观测器的球形移动机器人直线运动控制[J].中国测试,2019,45(9):123-129.YU Tao,WANG Yibo,SUN Hanxu,et al.Linear motion control of a spherical mobile robot based on disturbance observer[J].China Measurement &Testing Technology,2019,45(9):123-129.
[4] KAO W F,HSU C F.Motion control design for dyn-amicspherical mobile robot via fuzzy control approach[C]//2019 International Conference on Machine Learning andCybemetics.Kobe,2019:8949225.
[5] 李勐,刘弋锋,袁柳,等.一种小型模块化球形探测机器人系统设计实现[J].科学技术与工程,2019,19(20):252-257.LI Meng,LIU Yifeng,YUAN Liu,et al.Design and implementation of a small modular spherical exploratory robot system[J].Science Technology and Engineering,2019,19(20):252-257.
[6] 陈佳坪,王妍玮,谭庆吉,等.种植园巡检机器人结构设计[J].现代农业研究,2021,27(1):119-122.CHEN Jiaping,WANG Yanwei,TAN Qingji,et al.Structural design of plantation inspection robot[J].Modern Agricultural Research,2021,27(1):119-122.
[7] QU Y,LIU R,CHEN R,et al.Development of a reconfigurable miniature throwable robot for indoor surveillance[C].//2012 7th IEEE Conference on industrial electronics and applications.[v.1].:IEEE,2012:502-506.
[8] 战强,李伟.球形移动机器人的研究进展与发展趋势[J].机械工程学报,2019,55(9):1-17.ZHAN Qiang,LI Wei.Research progress and development trend of spherical mobile robots[J].Journal of Mechanical Engineering,2019,55(9):1-17.
[9] 李艳生,杨美美,孙汉旭,等.一种摆式球形机器人水中俯仰运动的稳定控制方法[J].振动与冲击,2018,37(13):149-154,189.LI Yansheng,YANG Meimei,SUN Hanxu,et al.A stability control method for pitching motion in water of a pendulum type spherical robot[J].Journal of Vibrationand Shock,2018,37(13):149-154,189.
[10] KAYACAN E,BAYRAKTAROGLU Z Y,SAEYS W.Modeling and control of a spherical rolling robot:a decoupled dynamics approach[J].Robotica,2012,30(4):671-680.
[11] BAI Y,SVININ M,YAMAMOTO M.Adaptive trajectory tracking control for the ball-pendulum system with time varying uncertainties[C]//2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS).IEEE,2017.
[12] YAO C,QIANG Z,XIX.Path tracking control of a spherical mobile robot[J].Mechanism &Machine Theory,2012,51(none):58-73.
[13] ZHENG M H,ZHAN Q,LIU J K,et al.Trajectory tracking of a spherical robot based on an rbf neural network[J].Advanced Materials Research,2012,383-390:631-637.
[14] KANJANAWANISHKUL K,ZELLA.Path following for an omnidirectional mobile robot based on model predictive control[J].IEEE,2009.
[15] 刘学文,任兴贵,许诺,等.多功能室外智能移动机器人避障轨迹自动规划方法[J].兵器装备工程学报,2022,43(10):201-206.LIU Xuewen, REN Xinggui, XU Nuo, et al.Automatic obstacle avoidance trajectory planning method for multifunctional outdoor intelligent mobile robot[J].Journal of Ordnance Equipment Engineering,2022,43(10):201-206.
[16] 叶平,韩亮亮,张天石,等.具有立体视觉的球形机器人及其运动控制[J].机械工程学报,2013,49(11):8-15.YE Ping,HAN Liangliang,ZHANG Tianshi,et al.Design and motion control of ball-shaped robot with stereo vision[J].Journal of Mechanical Engineering,2013,49(11):8-15.