基于MPC的移动机器人轨迹跟踪研究
Research on Trajectory Tracking of Mobile Robot Based on MPC
DOI: 10.12677/csa.2025.154081, PDF, HTML, XML,    科研立项经费支持
作者: 董力元, 李硕儒:淮北理工学院电子与信息工程学院,安徽 淮北
关键词: 移动机器人轨迹跟踪模型预测控制二次规划Mobile Robot Trajectory Tracking Model Predictive Control Quadratic Programming
摘要: 针对移动机器人轨迹跟踪问题,提出一种基于模型预测控制(MPC)的跟踪控制方案。首先,对移动机器人运动学模型进行线性化处理,得到其线性预测模型。其次,考虑其输入及状态约束,设计线性二次型控制器,将二次型最优问题转化为二次规划问题求解。基于实时反馈校正,提高了移动机器人轨迹跟踪精度。最后进行了计算机仿真验证,证明了移动机器人轨迹控制系统有效性。
Abstract: Aiming at the trajectory tracking problem of mobile robot, a tracking control scheme based on model predictive control (MPC) is proposed. Firstly, the kinematic model of the mobile robot is linearized to obtain its linear prediction model. Secondly, considering its input and state constraints, a linear quadratic controller is designed, and the quadratic optimal problem is transformed into a quadratic programming problem. Based on real-time feedback correction, the trajectory tracking accuracy of mobile robot is improved. Finally, computer simulation is carried out to verify the effectiveness of the mobile robot trajectory control system.
文章引用:董力元, 李硕儒. 基于MPC的移动机器人轨迹跟踪研究[J]. 计算机科学与应用, 2025, 15(4): 87-94. https://doi.org/10.12677/csa.2025.154081

1. 引言

在实际工作中,轮式移动机器人会受到许多约束,比如启动加速度不宜过大,转弯时角速度不能太大,线速度大小是有限的。而在性能方面,又要求控制器满足实时性和精确性,即机器人能在较短时间内准确到达期望位置,并且考虑到机器人所在复杂多变的环境。

近年来涌现了大量的轮式移动机器人的轨迹跟踪控制算法。杨敏等人针对轮式移动机器人的轨迹跟踪问题,提出了一种基于PI型滑模的控制策略[1]。韩龙等人提出了基于迭代的控制策略,提高移动机器人轨迹跟踪的速度[2]。马东等基于轮子打滑的动力学模型,提出了一种基于RBF神经网络自适应PD轨迹跟踪控制器[3]。谭永宏等人针对轮式移动机器人的实际轨迹跟踪问题,提出了一种基于模糊神经网络的跟踪控制器[4]。但是大部分都没有考虑轮式移动机器人存在饱和约束的情况。极小部分考虑到轮式移动机器人饱和约束情况,例如文献[5]-[9],但是这些方法设计的控制器比较复杂,对系统模型参数满足的条件要求严格。因此,研究饱和约束条件下的轮式移动机器人轨迹跟踪很有必要。

在过去30多年,模型预测控制被应用于复杂的工艺,并获得巨大的成功,充分印证了处理复杂约束优化问题的优点。大量的预测控制权威性文献都无一例外地指出,预测控制最大的吸引力在于它具有显式处理约束的能力,这种能力来自其基于模型对系统未来动态行为的预测,通过把约束加到未来的输入、输出或状态变量上,可以把约束显式表示在一个在线求解的二次规划或非线性规划问题中[10]。模型预测控制不需要精确的模型,采用滚动优化弥补了模型失配,干扰等因素引起的不确定性,尤为重要的是它能有效处理多变量、有约束问题。许多学者基于模型预测控制,研究了轮式移动机器人的轨迹跟踪问题。针对轮式移动机器人编队控制,Gu等人提出了一种基于模型预测控制的方案,通过向成本函数添加终端状态惩罚并且将终端状态区域添加到优化约束来保证该控制方案的稳定性[11]。Yan等人提出了一种模型预测控制方案,用于跟踪欠驱无人车。Johansen针对移动机器人非线性系统求解问题,提出了一种基于分区的mpQP解计算多参数非线性规划问题[12]。Chen等人提出了一种基于神经网络的非线性模型预测,提高轨迹跟踪的精度[13]。Summers等人基于自适应多尺度基函数的模型预测控制[14]。Wang等提出了一种基于格轨迹PWA的模型预测控制器,提高了非线性MPC的在线求解效率[15]。当没有明确考虑外部干扰时,所提出的MPC方法使用在一般投影网络的单层递归神经网络迭代地求解公式化二次规划(QP)问题,当考虑扰动时,通过使用双层递归神经网络迭代地求解重构的最小极大优化问题[16]

执行器饱和是实际工程系统中常见的问题,绝大多数执行机构会出现饱和现象,且这种现象是不容易避免的。如若控制系统执行机构的输入信息量达到一定限制,就进入了饱和状态,若再增加输入量不仅对执行器的输出产生不了任何影响,而且会导致控制系统的动态性能降低,或者使得闭环系统整体动态性能恶化。从理论研究和实际应用的角度可以看出饱和问题的出现使得系统控制问题变得复杂。因此,对约束系统的稳定性问题研究具有重要意义。本文针对带有速度饱和约束的轮式机器人的轨迹跟踪问题进行了研究。

2. 问题描述

在轨迹跟踪中,我们只需知道位姿q的坐标和方位角,我们重新定义轮式移动机器人位姿为 q = [ x   y   θ   ] T ,则有

{ x ˙ = v cos θ y ˙ = v sin θ θ ˙ = w (1)

其中 v m i n v v m a x w m i n w w m a x ,即轮式移动机器人控制输入Z满足的约束条件为:

z min z z max (2)

假设轮式移动机器人在全局坐标系下的参考轨迹定义如下:

{ x ˙ r = v r cos θ r y ˙ r = v r sin θ r θ ˙ r = w r (3)

本章轨迹跟踪控制目标是在给定的初始误差条件下,运动学控制输入 Z 能够在有限时间内使得 ( x   y   θ ) ( x r y r θ r ) ,且 Z 满足约束条件。

3. 移动机器人运动学建模

分别将式(1)和(3)写成如下形式:

q ˙ = f ( q , z ) (4)

q ˙ r = f ( q r , z r ) (5)

对式(4)在 ( q r , z r ) 处进行泰勒展开,省略其高阶无穷小,可得

q ˙ = f ( q r , z r ) + f q , r × ( q q r ) + f z , r × ( z z r ) (6)

其中: f q , r f z , r 分别是 f 相对 q z 的雅可比矩阵

f q , r = f ( q , z ) q | q = q r ,   z = z r

f z , r = f ( q , z ) z | q = q r ,   z = z r

将式(6)减式(5),可得轮式移动机器人的线性跟踪误差模型

q ˜ ˙ = f q , r × q ˜ + f z , r × z ˜ (7)

其中:

q ˜ = q q r ,   z ˜ = z z r

对式(7)离散化得

q ˜ ( k + 1 ) = A ( k ) q ˜ ( k ) + B ( k ) z ˜ ( k ) (8)

其中:T为采样时间

A ( k ) [ 1 0 v r ( k ) sin θ r ( k ) T 0 1 v r ( k ) cos θ r ( k ) T 0 0 1 ] ,   B ( k ) [ cos θ r ( k ) T 0 sin θ r ( k ) T 0 0 T ]

4. 模型预测跟踪控制器

将(7)作为预测模型,则时刻轮式移动机器人的优化性能指标定义为

Φ ( k ) = j = 1 N [ q ˜ T ( k + j | k ) Q q ˜ ( k + j | k ) + z ˜ T ( k + j 1 | k ) R z ˜ ( k + j 1 | k ) ] (9)

其中:N为预测时域,QR为权值矩阵。

式(9)最优解 z ˜ * 定义如下:

z ˜ * = arg min z ˜ { Φ ( k ) } (10)

通过在每一采样时刻k,求取能使性能指标 Φ ( k ) 最小的最优控制序 z ˜ * ( { z ˜ * ( k | k ) z ˜ * ( k | N 1 ) } ),只取 z ˜ * 中的第1个值作为模型预测控制输入。移动机器人的模型预测控制结构如图1

Figure 1. Model predictive control

1. 模型预测控制

定义预测时域N内的移动机器人的跟踪误差和控制变量误差为

q ¯ ( k + 1 ) [ q ˜ ( k + 1 | k ) q ˜ ( k + 2 | k ) q ˜ ( k + N | k ) ] ,   z ¯ ( k ) [ z ˜ ( k | k ) z ˜ ( k + 1 | k ) z ˜ ( k + N 1 | k ) ]

因此,式(9)可重写如下:

Φ ( k ) = q ¯ T ( k + 1 ) Q ¯ q ¯ ( k + 1 ) + z ¯ T ( k ) R ¯ z ¯ ( k ) (11)

其中:

Q ¯ [ Q 0 0 0 Q 0 0 0 Q ] ,   R ¯ [ R 0 0 0 R 0 0 0 R ]

由式(8)可知

{ q ˜ ( k + 1 | k ) = A ( k | k ) q ˜ ( k | k ) + B ( k | k ) z ˜ ( k | k ) q ˜ ( k + 2 | k ) = A ( k + 1 | k ) q ˜ ( k + 1 | k ) + B ( k + 1 | k ) z ˜ ( k + 1 | k ) q ˜ ( K + N | k ) = A ( k + N 1 | k ) q ˜ ( k + N 1 | k ) + B ( k + N 1 | k ) z ˜ ( k + N 1 | k ) = A ( k + N 1 | k ) A ( k | k ) q ˜ ( k | k ) + A ( k + N 1 | k ) A ( k | k ) B ( k | k ) z ˜ ( k | k ) + + B ( k | N 1 k ) z ˜ ( k + N 1 | k )

q ¯ ( k + 1 ) 可重写如下:

q ¯ ( k + 1 | k ) = A ¯ ( k ) q ˜ ( k | k ) + B ¯ ( k ) z ¯ ( k ) (12)

其中:

A ¯ ( k ) = [ A ( k | k ) A ( k + 1 | k ) A ( k | k ) A ( k + N 1 | k ) A ( k + 1 | k ) A ( k | k ) ]

B ¯ ( k ) = [ B ( k | k ) 0 0 A ( k + 1 | k ) B ( k | k ) B ( k + 1 | k ) 0 α ( k ,   1 ) B ( k | k ) α ( k ,   2 ) B ( k + 1 | k ) B ( k + N 1 | k ) ]

α ( k , j ) 定义如下:

α ( k , j ) i = j N 1 A ( k + i | k ) ,   j = 1 N 1

将式(12)代入式(11)得

Φ ( k ) = [ A ¯ ( k ) q ˜ ( k | k ) + B ¯ ( k ) z ¯ ( k ) ] T Q ¯ [ A ¯ ( k ) q ˜ ( k | k ) + B ¯ ( k ) z ¯ ( k ) ] + z ¯ T ( k ) R ¯ z ¯ ( k ) = [ q ˜ Τ ( k | k ) A ¯ T ( k ) + z ¯ T ( k ) B ¯ T ( k ) ] Q ¯ [ A ¯ ( k ) q ˜ ( k | k ) + B ¯ ( k ) z ¯ ( k ) ] + z ¯ T ( k ) R ¯ z ¯ ( k ) = q ˜ Τ ( k | k ) A ¯ T ( k ) Q ¯ A ¯ ( k ) q ˜ ( k | k ) + q ˜ Τ ( k | k ) A ¯ T ( k ) Q ¯ B ¯ ( k ) z ¯ ( k ) + z ¯ T ( k ) B ¯ T ( k ) Q ¯ A ¯ ( k ) q ˜ ( k | k )   + z ¯ T ( k ) B ¯ T ( k ) Q ¯ B ¯ ( k ) z ¯ ( k ) + z ¯ T ( k ) R ¯ z ¯ ( k ) = z ¯ T ( k ) [ R ¯ + B ¯ T ( k ) Q ¯ B ¯ ( k ) ] z ¯ ( k ) + q ˜ Τ ( k | k ) A ¯ T ( k ) Q ¯ A ¯ ( k ) q ˜ ( k | k )   + 2 q ˜ ( k | k ) A ¯ T ( k ) Q ¯ B ¯ ( k ) z ¯ ( k ) = 1 2 z ¯ T ( k ) H ( k ) z ¯ ( k ) + f T ( k ) z ¯ ( k ) + d ( k )

其中:

H ( k ) 2 ( R ¯ + B ¯ T ( k ) Q ¯ B ¯ ( k ) ) f ( k ) 2 B ¯ T ( k ) Q ¯ A ¯ ( k ) q ˜ Τ ( k | k ) d ( k ) q ˜ Τ ( k | k ) A ¯ T ( k ) Q ¯ A ¯ ( k ) q ˜ ( k | k )

矩阵H是一个正定的Hessian矩阵,描述 z ¯ ( k ) 的二次部分。向量f描述了 z ¯ ( k ) 的线性部分,d z ¯ ( k ) 无关。我们成功地把性能指标转化为 z ¯ ( k ) 的二次规划形式。

考虑到Z的饱和约束条件,式(10)中最优问题可重述为:

z ˜ * = arg min { Φ ( k ) } ,   z ˜ min z ˜ z ˜ max (13)

其中:

z ˜ min = z min z r z ˜ max = z max z r

对二次规划求解,得到控制时域内的控制增量序列 z ˜ k = [ z ˜ * k | k    z ˜ * k + 1 | k   z ˜ * k + N c 1 | k ] Τ 。将 z ˜ * k | k 作为当前时刻的控制增量,则控制量如下:

z = z r + z ˜ * k | k (14)

定理:对MPC式(13)求解控制增量序列 z ˜ k = [ z ˜ * k | k    z ˜ * k + 1 | k   z ˜ * k + N c 1 | k ] Τ 的作用下,移动机器人位姿跟踪误差系统式(7)是渐进稳定。

证明:记 z ˜ k * = [ z ˜ * k | k    z ˜ * k + 1 | k   z ˜ * k + N c 1 | k ] Τ ,它表示移动机器人在k时刻,位姿跟踪误差为 q ˜ ( k ) ,预测时域为 N p ,控制时域为 N c 的最优控制增量序列。则最优控制下的移动机器人位姿跟踪误差序列描述如下:

q ˜ k * = [ q ˜ * k | k    q ˜ * k + 1 | k   q ˜ * k + N p 1 | k ] Τ (15)

k时刻,定义李雅普诺夫函数如下:

V 0 * = j = 1 N p q ˜ Τ k + j | k Q q ˜ k + j | k + j = 0 N c 1 z ˜ k + j | k Τ R z ˜ k + j | k (16)

k + 1 时刻有:

V 1 * = j = 1 N p q ˜ Τ k + 1 + j | k Q q ˜ k + 1 + j | k + j = 0 N c 1 z ˜ k + 1 + j | k Τ R z ˜ k + 1 + j | k (17)

对式(17)整理可得:

V 1 * = j = 1 N p q ˜ Τ k + j | k Q q ˜ k + j | k + j = 0 N c 1 z ˜ k + j | k Τ R z ˜ k + j | k q ˜ Τ k + 1 | k Q q ˜ k + 1 | k z ˜ k + 1 | k Τ R z ˜ k + 1 | k         + q ˜ Τ k + N p + 1 | k Q q ˜ k + N p + 1 | k z ˜ k + N c + 1 | k Τ R z ˜ k + N c + 1 | k         (18)

当系统式(7)进入稳态时, z ˜ k + N c + 1 | k Τ R z ˜ k + N c + 1 | k = 0 q ˜ Τ k + N p + 1 | k Q q ˜ k + N p + 1 | k = 0 ,我们重写式(18)如下:

V 1 * = V 0 * z ˜ k + 1 | k Τ R z ˜ k + 1 | k q ˜ Τ k + 1 | k Q q ˜ k + 1 | k (19)

由于QR均为正定矩阵,则 z ˜ k + 1 | k Τ R z ˜ k + 1 | k q ˜ Τ k + 1 | k Q q ˜ k + 1 | k 均为非负数,进一步可得

V 1 * V 0 * = z ˜ k + 1 | k Τ R z ˜ k + 1 | k q ˜ Τ k + 1 | k Q q ˜ k + 1 | k 0 (20)

因此,整个闭环系统是渐进稳定的。证毕。

5. 仿真验证

为了验证本章所提出的模型预测轨迹跟踪控制器的可行性,我们选择直线和圆形两种参考轨迹进行了MATLAB2016b/simulink仿真实验。模型预测控制的采样时间 Δ T = 0.05 ,预测时域 N p = 40 ,权值矩阵 Q = d i a g ( 1 ,   1 ,   1 ) R = d i a g ( 0.5 ,   0.5 ) 。设移动机器人控制输入满足的约束条件为: 0   m / s v 1.5   m / s 1   r a d / s w 1   r a d / s

1) 直线轨迹跟踪控制

移动机器人期望线速度 v r = 0.5   m / s ,期望角速度 w r = 0   r a d / s ,其初始参考位姿和实际位姿分别为 q r = [ 0.71    π / 3 ] T ,   q = [ 0.3   1   π ] T 。移动机器人的直线轨迹跟踪仿真结果如下图所示,其中图2图3图4分别为移动机器人的轨迹跟踪误差曲线图、轨迹跟踪位姿曲线图、轨迹跟踪控制曲线图。

Figure 2. Track tracking error

2. 轨迹跟踪误差

Figure 3. Trajectory tracking pose

3. 轨迹跟踪位姿

Figure 4. Trajectory tracking control

4. 轨迹跟踪控制

图2可以看出,移动机器人能够在较短时间内跟踪误差收敛到零,说明模型预测轨迹跟踪控制器具有收敛速度快的特点;从图3可以看出,模型预测轨迹跟踪控制器对机器人轨迹跟踪有良好的控制效果,移动机器人的实际轨迹都能够平滑逼近其参考轨迹;从图4可以看出,模型预测控制输入量平稳地约束在指定范围内。

基金项目

安徽省高等学校科学研究项目2024AH051652。

参考文献

[1] 杨敏, 梅劲松, 廖里程. 非完整轮式移动机器人反演滑模轨迹跟踪控制器设计[J]. 机械制造与自动化, 2015, 44(5): 152-154, 196.
[2] 韩龙, 刘国栋. 一种基于迭代学习的移动机器人轨迹跟踪控制方法[J]. 计算机系统应用, 2012, 21(4): 59-63.
[3] 马东, 董力元, 王立玲, 等. 移动机器人RBF神经网络自适应PD跟踪控制[J]. 控制工程, 2020, 27(12): 2092-2098.
[4] 谭永宏, 陈爱武. 轮式移动机器人的模糊轨迹跟踪控制[J]. 制造业自动化, 2009, 31(2): 55-58.
[5] Lee, T.C., Song, K.T., Lee, C.H. and Teng, C.C. (2001) Tracking Control of Unicycle-Modeled Mobile Robots Using a Saturation Feedback Controller. IEEE Transactions on Control Systems Technology, 9, 305-318.
https://doi.org/10.1109/87.911382
[6] Chen, X., Jia, Y. and Matsuno, F. (2014) Tracking Control for Differential-Drive Mobile Robots with Diamond-Shaped Input Constraints. IEEE Transactions on Control Systems Technology, 22, 1999-2006.
https://doi.org/10.1109/tcst.2013.2296900
[7] Wang, C. (2008) Semiglobal Practical Stabilization of Nonholonomic Wheeled Mobile Robots with Saturated Inputs. Automatica, 44, 816-822.
https://doi.org/10.1016/j.automatica.2007.07.013
[8] Chen, H., Wang, C., Zhang, B. and Zhang, D. (2012) Saturated Tracking Control for Nonholonomic Mobile Robots with Dynamic Feedback. Transactions of the Institute of Measurement and Control, 35, 105-116.
https://doi.org/10.1177/0142331211431719
[9] 熊蓉, 詹建波, 汤卿. 移动机器人速度加速度饱和约束下的时间最优控制[J]. 控制与决策, 2014, 29(1): 118-122.
[10] Kuhne, F., Lages, W.F. and da Silva Jr., J.G. (2004) Model Predictive Control of a Mobile Robot Using Linearization. Proceedings of Mechatronics and Robotics 2004, 525-530.
[11] Chang Boon Low, and Danwei Wang, (2008) Gps-based Path Following Control for a Car-Like Wheeled Mobile Robot with Skidding and Slipping. IEEE Transactions on Control Systems Technology, 16, 340-347.
https://doi.org/10.1109/tcst.2007.903100
[12] Johansen, T.A. (2002) On Multi-Parametric Nonlinear Programming and Explicit Nonlinear Model Predictive Control. Proceedings of the 41st IEEE Conference on Decision and Control, 2002, Las Vegas, 10-13 December 2002, 2768-2773.
https://doi.org/10.1109/cdc.2002.1184260
[13] Chen, S., Saulnier, K., Atanasov, N., Lee, D.D., Kumar, V., Pappas, G.J., et al. (2018) Approximating Explicit Model Predictive Control Using Constrained Neural Networks. 2018 Annual American Control Conference (ACC), Milwaukee, 27-29 June 2018, 1520-1527.
https://doi.org/10.23919/acc.2018.8431275
[14] Summers, S., Jones, C.N., Lygeros, J. and Morari, M. (2011) A Multiresolution Approximation Method for Fast Explicit Model Predictive Control. IEEE Transactions on Automatic Control, 56, 2530-2541.
https://doi.org/10.1109/tac.2011.2146990
[15] Wang, K., Xu, Z., Zhang, K., Huang, Y. and Xu, J. (2023) Lattice Piecewise Affine Approximation of Explicit Nonlinear Model Predictive Control with Application to Trajectory Tracking of Mobile Robot. IET Control Theory & Applications, 18, 149-159.
https://doi.org/10.1049/cth2.12553
[16] Kang, H., Park, C. and Hyun, C. (2016) Alternative Identification of Wheeled Mobile Robots with Skidding and Slipping. International Journal of Control, Automation and Systems, 14, 1055-1062.
https://doi.org/10.1007/s12555-015-0095-y

Baidu
map