CN116165895A - 一种全方位移动机器人的鲁棒模型预测控制方法 - Google Patents

一种全方位移动机器人的鲁棒模型预测控制方法 Download PDF

Info

Publication number
CN116165895A
CN116165895A CN202310163972.1A CN202310163972A CN116165895A CN 116165895 A CN116165895 A CN 116165895A CN 202310163972 A CN202310163972 A CN 202310163972A CN 116165895 A CN116165895 A CN 116165895A
Authority
CN
China
Prior art keywords
mobile robot
error
actual
omnibearing
control
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202310163972.1A
Other languages
English (en)
Inventor
唐昊
李修欣
王舒润
戴飞
王彬
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Hefei University of Technology
Original Assignee
Hefei University of Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Hefei University of Technology filed Critical Hefei University of Technology
Priority to CN202310163972.1A priority Critical patent/CN116165895A/zh
Publication of CN116165895A publication Critical patent/CN116165895A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B13/00—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion
    • G05B13/02—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric
    • G05B13/04—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators
    • G05B13/042—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators in which a parameter or coefficient is automatically adjusted to optimise the performance
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Health & Medical Sciences (AREA)
  • Artificial Intelligence (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Evolutionary Computation (AREA)
  • Medical Informatics (AREA)
  • Software Systems (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Feedback Control In General (AREA)

Abstract

本发明属于机器人智能控制领域，涉及一种基于模型预测控制的全方位移动机器人轨迹跟踪方法。首先，根据全方位移动机器人的运动特性，建立全方位移动机器人的运动学模型；接着，通过对运动学模型的线性化和离散化等方式，建立全方位移动机器人的误差运动学模型；其次，基于模型预测控制方法设计全方位移动机器人的运动学控制器；再次，设计扩张状态观测器来估计状态量与干扰量；然后，设计积分控制量以完全消除系统的稳态误差；最后，通过运动学控制器求得全方位移动机器人的实际速度和实际位姿，完成全方位移动机器人对期望轨迹的精准跟踪。该方法应用于全方位移动机器人的轨迹跟踪控制，可以更好地处理运动约束问题，并且能够削弱系统模型中存在的非线性、耦合性以及不确定性等因素的影响，提高了系统的动态控制效果和鲁棒性。

Description

一种全方位移动机器人的鲁棒模型预测控制方法
技术领域
本发明属于机器人智能控制领域，更具体地，涉及一种全方位移动机器人的鲁棒模型预测控制方法。
背景技术
随着工农业、商业、安全防护以及航空航天等领域的逐步智能化，机器人将在各个领域发挥着至关重要作用。移动机器人是日常生活中最常见的一类机器人，它具有高度的灵活性和自主性，能够实现环境感知、自主导航、安全避障等功能。
轮式移动机器人是移动机器人中极为经典的一类机器人，其研究与应用获得了飞速发展。轮式移动机器人由于其结构简单、运动灵活、自主调节能力强、承载能力大等优点，成为了应用最为广泛的机器人。传统的移动机器人只有两个自由度，无法横向移动，因此在狭窄环境下的灵活性和机动性有限。全方位移动机器人在平面内具有三个自由度，能够站在自身姿态保持不变的情况下沿任意方向移动，因此非常适合工作空间狭窄以及对机器人灵活性要求较高的场景。
全方位移动机器人是一种典型的非完整约束系统，其运动控制问题十分复杂。轨迹跟踪控制是全方位移动机器人运动控制的重点内容，其目的在于设计有效的控制方法使得全方位移动机器人的实际轨迹能够快速稳定地跟踪预定轨迹。全方位移动机器人的传统轨迹跟踪控制方法有PID控制、反演法、自适应控制、智能控制等。尽管这些方法都取得了不错的效果，但是当系统中存在模型不确定性以及外部扰动时，这些算法的控制效果往往不够理想，并且这些方法无法较好地处理全方位移动机器人运动过程中的状态约束以及控制约束。鲁棒模型预测控制方法具有良好的动态控制效果，并且可以削弱系统模型中存在的非线性、耦合性以及不确定性等因素的影响，同时能够更好地处理系统中的各种约束条件。因此，对全方位移动机器人的模型预测控制方法进入深入研究，具有重要的理论意义和应用价值。
发明内容
针对现有技术的不足，本发明提供了一种全方位移动机器人的鲁棒模型预测控制方法，该方法能够实现全方位移动机器人对期望轨迹的快速、精准、稳定的跟踪控制。本发明利用全方位移动机器人的运动学模型建立误差运动学模型，以此设计运动学控制器，降低了控制器设计的复杂程度；利用模型预测控制来进行轨迹跟踪控制，可以更好地处理全方位移动机器人在运动过程中的各种约束条件，使其具有良好的动态控制效果；利用扩张状态观测器，极大地提高了系统的鲁棒性，并且解决了状态不可测的问题；利用引入积分控制量的方式，完全消除了系统的静态误差。
为解决上述技术问题，本发明提供了如下技术方案：
一种全方位移动机器人的鲁棒模型预测控制方法，该方法包括以下步骤：
步骤1：根据全方位移动机器人的运动特性，建立全方位移动机器人的运动学模型；
步骤2：利用线性化和离散化方法，建立全方位移动机器人的误差运动学模型；
步骤3：基于模型预测控制方法设计全方位移动机器人的运动学控制器；
步骤4：设计扩张状态观测器，估计状态量与干扰量；
步骤5：设计积分控制量，消除系统的稳态误差。
步骤6：通过运动学控制器求得全方位移动机器人的实际速度和实际位姿，实现全方位移动机器人对期望轨迹的精准跟踪。
本技术方案进一步的优化，步骤1中全方位移动机器人的运动学模型：
全方位移动机器人在任意时刻的实际位姿向量为q＝[x y φ]T，其中x，y，φ分别表示全方位移动机器人的实际横向位置、实际纵向位置以及实际姿态角。全方位移动机器人在任意时刻的实际速度向量为v＝[vx vy ω]T，其中vx，vy，ω分别表示全方位移动机器人的实际横向速度、实际纵向速度以及实际姿态角速度。
在机器人坐标系中，全方位移动机器人在任意时刻的实际位姿向量为qc＝[xc ycφc]T，其中xc，yc，φc分别表示全方位移动机器人在机器人坐标系下的实际横向位置、实际纵向位置以及实际姿态角。在机器人坐标系中，全方位移动机器人在任意时刻的实际速度向量为vc＝[vcx vcy ωc]T，其中vcx，vcy，ωc分别表示全方位移动机器人在机器人坐标系下的实际横向速度、实际纵向速度以及实际姿态角速度。
通过分析坐标转换关系，可以推导出如下关系：
qc＝T(φ)q (24)
vc＝T(φ)v (25)
其中
Figure BDA0004095234920000031
表示坐标系转换矩阵。
全方位移动机器人在任意时刻的四个轮子的旋转角速度向量为
Figure BDA0004095234920000032
θi(i＝1,2,3,4)表示全方位移动机器人四个轮子的旋转角速度。
全方位移动机器人的运动学模型描述的是机器人的实际速度与四个轮子的实际转速之间的关系，其正逆运动学模型可分别表示为：
Figure BDA0004095234920000033
Figure BDA0004095234920000034
其中D为全方位移动机器人的长度2D1与宽度2D2相加之和的一半，R为全方位移动机器人的四个轮子的半径。
本技术方案进一步的优化，步骤2中全方位移动机器人的误差运动学模型的建立如下：
定义全方位移动机器人在世界坐标系下的期望位姿向量为qr＝[xr yr φr]T，其中xr，yr，φr分别表示期望横向位置、期望纵向位置以及期望姿态角；全方位移动机器人在世界坐标系下的期望速度向量为vr＝[vxr vyr ωr]T，其中vxr，vyr，ωr分别表示期望横向速度、期望纵向速度以及期望姿态角速度；
定义全方位移动机器人在机器人坐标系下的期望位姿向量为qcr＝[xcr ycr φcr]T，其中xcr，ycr，φcr分别表示期望横向位置、期望纵向位置以及期望姿态角；全方位移动机器人在机器人坐标系下的期望速度向量为vcr＝[vcxr vcyr ωcr]T，其中vcxr，vcyr，ωcr分别表示期望横向速度、期望纵向速度以及期望姿态角速度；
全方位移动机器人在世界坐标系中和在机器人坐标系中的位姿误差向量分别为：
Figure BDA0004095234920000041
Figure BDA0004095234920000042
其中xe，ye，φe分别表示全方位移动机器人的横向位置误差、纵向位置误差以及姿态角误差；xce，yce，φce分别表示全方位移动机器人在机器人坐标系下的横向位置误差、纵向位置误差以及姿态角误差。
根据公式(24)可得如下方程：
qce＝T(φ)qe (30)
对位姿误差向量qce求导，并将公式(30)代入计算可得如下方程：
Figure BDA0004095234920000043
其中
Figure BDA0004095234920000044
表示全方位移动机器人在世界坐标系下的速度误差向量，且有
Figure BDA0004095234920000045
在公式(31)中，若分别以qce和
Figure BDA0004095234920000046
作为状态量X和控制量U，且输出量Y＝qe，则可得如下误差状态空间方程：
Figure BDA0004095234920000047
其中
Figure BDA0004095234920000048
通过欧拉方法可以将上述误差状态方程离散化，可得全方位移动机器人的误差运动学模型为：
Figure BDA0004095234920000049
其中k为采样时刻，ΔT为采样时间间隔，
Figure BDA0004095234920000051
Figure BDA0004095234920000052
本技术方案进一步的优化，步骤3中运动学控制器的设计如下：
设N为模型预测控制的预测时域，k为系统当前所处时刻，则根据公式(33)可得系统在k时刻的预测模型如下所示：
XN＝ANX(k|k)+BNUN(34)
其中
Figure BDA0004095234920000053
Figure BDA0004095234920000054
目标函数可以表达为状态量和控制量的二次函数，其形式如下所示：
Figure BDA0004095234920000055
其中
Figure BDA0004095234920000056
而
Figure BDA0004095234920000057
和
Figure BDA0004095234920000058
分别表示状态量和输入量的权重矩阵，代表了每个状态量和每个输入量在优化过程中的重要程度，并且满足Q≥0,P≥0。
将预测模型(34)带入目标函数(35)，则系统的目标函数可以重新表达为如下形式：
Figure BDA0004095234920000059
其中
Figure BDA00040952349200000510
矩阵为一个正定的Hessian矩阵，表示二次规划中的二次部分；
Figure BDA0004095234920000061
表示二次规划中的线性部分；
Figure BDA0004095234920000062
表示二次规划中与UN无关的标量。
系统在k时刻的最优控制序列可以通过下式求得：
Figure BDA0004095234920000063
系统在每一采样时刻都需要通过优化过程求得最优控制序列，在求得UN *之后仅取第一个元素U*(k)作为k时刻的系统控制量误差，而忽略最优控制序列UN *中的其他值，因此这一优化过程被称为滚动优化。
若系统的实际控制量u为机器人的实际速度
Figure BDA0004095234920000064
则有
Figure BDA0004095234920000065
所以系统在k时刻的实际控制量为：
umpc(k)＝ur(k)-U*(k) (38)
对于任何真实世界的系统，控制输入都受到物理限制，而且控制输入也需要一定的平滑性和连续性，因此系统存在如下约束条件：
Figure BDA0004095234920000066
本技术方案进一步的优化，步骤4中扩张状态观测器的设计如下：
根据公式(32)和(33)，建立考虑扰动的误差运动学模型为：
Figure BDA0004095234920000067
其中d(k)表示系统的干扰量。
系统的扩张状态观测器可以设计为：
Figure BDA0004095234920000068
其中
Figure BDA0004095234920000069
表示状态的估计量，
Figure BDA00040952349200000610
表示干扰的估计量，H1＝diag(ξ1ω0 ξ1ω0 ξ1ω0)和H2＝diag(ξ2ω0 ξ2ω0 ξ2ω0)表示观测器增益矩阵，ω0表示观测器带宽，ξ1和ξ2表示观测器增益系数，且ω0，ξ1和ξ2均为正实数。
定义状态估计误差为
Figure BDA00040952349200000611
干扰估计误差为
Figure BDA00040952349200000612
结合公式(40)和(41)，可以计算得到：
Figure BDA0004095234920000071
将上式转化成成矩阵形式如下所示：
Figure BDA0004095234920000072
其中I3×3代表三维单位矩阵，03×3代表三维零矩阵,
Figure BDA0004095234920000073
为了确保扩张状态观测器误差能够逐渐趋近于零，需要对观测器的稳定性进行判断。根据离散系统的判稳定理，当矩阵G的特征值均小于1时，观测器误差必定能够趋于稳定。因此，通过设计合理的观测器增益矩阵H1和H2使得矩阵G的特征值均小于1，则能够保证所设计扩张状态观测器的稳定性。
利用观测器估计得到的状态量，可以在状态量无法测量的情况下实现模型预测控制。同时，将观测器估计得到的干扰量以补偿的形式作为控制量的一部分，能够抵消干扰量对系统性能的不利影响，极大地提高系统的鲁棒性。因此，系统的控制量更新为如下形式：
Figure BDA0004095234920000074
本技术方案更进一步的优化，步骤5中积分控制量的设计如下：
为了能够消除系统的稳态误差，可以使用积分器校正全方位移动机器人的位姿误差，即在控制量中添加误差积分项。在系统运行的初始阶段，使用模型预测控制来使系统快速跟踪给定值；当控制量接近给定值时，在原本控制律的基础上加入积分控制来消除稳态误差，提高控制系统精度和鲁棒性。
由于在系统的误差运动学模型中，系统误差为qe＝[xe ye φe]T，则误差积分项可以表示为：
Figure BDA0004095234920000075
其中KI＝diag(k1i k2i k3i)表示积分项的系数矩阵，且KI为正定矩阵。
设系统进行控制律切换的误差临界值为，则系统最终的控制律如下所示：
Figure BDA0004095234920000081
与现有技术相比，上述技术方案的有益效果主要表现在：
1、模型预测控制可以更好地处理系统中的各种约束条件；
2、模型预测控制具有良好动态控制效果，能够实现近似最优控制；
3、将扩张状态观测器与模型预测控制结合，极大地增强了控制系统的鲁棒性和抗扰性，同时通过输出反馈的方式解决了系统状态不可测的问题；
5、通过引入积分控制量，提高了系统的精度和鲁棒性。
附图说明
图1为全方位移动机器人的结构示意图；
图2为全方位移动机器人的鲁棒模型预测控制流程图；
图3为模型预测控制流程图；
图4全方位移动机器人的鲁棒模型预测控制系统的结构框图；
图5是全方位移动机器人对圆形轨迹的跟踪效果图。
具体实施方式
为详细说明技术方案的技术内容、构造特征、所实现目的及效果，以下结合具体实施方式并配合附图详予说明。
本发明公开了一种全方位移动机器人的鲁棒模型预测控制方法，该方法可以更好地处理全方位移动机器人的运动约束问题，包括状态量和控制量的条件约束、数值限制、平滑性和连续性等；同时，该方法不依赖精确的数学模型，能够削弱系统模型中存在的非线性、耦合性以及不确定性等因素的影响，提高了系统的精准性、鲁棒性和经济性。
参阅图1所示，全方位移动机器人的结构示意图。全方位移动机器人包括四个麦克纳姆轮，并且每个轮子分别由一个独立的电机驱动。麦克纳姆轮的辊子轴线与轮毂轴线夹角均为α＝45°，得益于麦克纳姆轮的这种特殊结构，该移动机器人可以通过四个轮子的速度组合实现全方位移动。全方位移动机器人的位姿在其运动过程中具有各自的作用，因此实现位姿的精准控制需要建立精确的系统坐标系。世界坐标系O-XY固定不变，表示全方位移动机器人所处的运动环境；机器人坐标系Oc-XcYc以机器人的几何中心Oc为原点，会随着移动机器人的运动而不断变化，表示全方位移动机器人所处的位置。一般地，可以把移动机器人几何中心Oc的速度视等效为移动底盘的运动速度。
全方位移动机器人的纵向同侧麦轮轴心之间的距离为其长度2D1，横向同侧麦轮轴心之间的距离为其宽度2D2，并且D＝D1+D2。麦克纳姆轮轴心到与地面接触点的距离为其半径，并且移全方位动机器人四个麦轮的半径均为R。
参阅图2所示，全方位移动机器人的鲁棒模型预测控制流程图，该方法包括如下步骤：
步骤1：根据全方位移动机器人的运动特性，建立全方位移动机器人的运动学模型；
全方位移动机器人在任意时刻的实际位姿向量为q＝[x y φ]T，其中x，y，φ分别表示全方位移动机器人的实际横向位置、实际纵向位置以及实际姿态角。全方位移动机器人在任意时刻的实际速度向量为v＝[vx vy ω]T，其中vx，vy，ω分别表示全方位移动机器人的实际横向速度、实际纵向速度以及实际姿态角速度。
在机器人坐标系中，全方位移动机器人在任意时刻的实际位姿向量为qc＝[xc ycφc]T，其中xc，yc，φc分别表示全方位移动机器人在机器人坐标系下的实际横向位置、实际纵向位置以及实际姿态角。在机器人坐标系中，全方位移动机器人在任意时刻的实际速度向量为vc＝[vcx vcy ωc]T，其中vcx，vcy，ωc分别表示全方位移动机器人在机器人坐标系下的实际横向速度、实际纵向速度以及实际姿态角速度。
通过分析坐标转换关系，全方位移动机器人在世界坐标系下的位姿向量q与机器人坐标系下的位姿误差向量qc具有如下关系：
qc＝T(φ)q (47)
其中
Figure BDA0004095234920000091
表示坐标系转换矩阵。
同样地，全方位移动机器人在世界坐标系下速度向量v与机器人坐标系下的速度向量vc具有如下关系：
vc＝T(φ)v (48)
全方位移动机器人在任意时刻的四个轮子的旋转角速度向量为
Figure BDA0004095234920000101
θi(i＝1,2,3,4)表示全方位移动机器人四个轮子的旋转角速度。
全方位移动机器人的运动学模型描述的是机器人的实际速度与四个轮子的实际转速之间的关系，其正逆运动学模型可分别表示为：
Figure BDA0004095234920000102
Figure BDA0004095234920000103
步骤2：利用线性化和离散化方法，建立全方位移动机器人的误差运动学模型；
全方位移动机器人的误差运动学模型实际上是分别以位姿误差和速度误差为状态量和控制量的离散状态空间方程。
定义全方位移动机器人在世界坐标系下的期望位姿向量为qr＝[xr yr φr]T，其中xr，yr，φr分别表示期望横向位置、期望纵向位置以及期望姿态角；全方位移动机器人在世界坐标系下的期望速度向量为vr＝[vxr vyr ωr]T，其中vxr，vyr，ωr分别表示期望横向速度、期望纵向速度以及期望姿态角速度；
定义全方位移动机器人在机器人坐标系下的期望位姿向量为qcr＝[xcr ycr φcr]T，其中xcr，ycr，φcr分别表示期望横向位置、期望纵向位置以及期望姿态角；全方位移动机器人在机器人坐标系下的期望速度向量为vcr＝[vcxr vcyr ωcr]T，其中vcxr，vcyr，ωcr分别表示期望横向速度、期望纵向速度以及期望姿态角速度；
全方位移动机器人在世界坐标系中和在机器人坐标系中的位姿误差向量分别为：
Figure BDA0004095234920000104
Figure BDA0004095234920000111
其中xe，ye，φe分别表示全方位移动机器人的横向位置误差、纵向位置误差以及姿态角误差；xce，yce，φce分别表示全方位移动机器人在机器人坐标系下的横向位置误差、纵向位置误差以及姿态角误差。
根据公式(47)可得如下方程：
qce＝T(φ)qe (53)
对位姿误差向量qce求导，并将公式(53)代入计算可得如下方程：
Figure BDA0004095234920000112
其中
Figure BDA0004095234920000113
表示全方位移动机器人在世界坐标系下的速度误差向量，且有
Figure BDA0004095234920000114
在公式(54)中，若分别以qce和
Figure BDA0004095234920000115
作为状态量X和控制量U，且输出量Y＝qe，则可得如下误差状态空间方程：
Figure BDA0004095234920000116
其中
Figure BDA0004095234920000117
通过欧拉方法可以将上述误差状态方程离散化，可得全方位移动机器人的误差运动学模型为：
Figure BDA0004095234920000118
其中k为采样时刻，ΔT为采样时间间隔，
Figure BDA0004095234920000119
Figure BDA0004095234920000121
步骤3：基于模型预测控制方法设计全方位移动机器人的运动学控制器：
模型预测控制可以通过模型来预测系统在未来某一有限时间段内的表现来进行优化控制，通过在每个采样时刻求解有限时域内的最优控制问题来获得当前控制策略。在每一采样时刻，最优控制问题的初始状态是控制过程的当前状态，并且仅使用求得的最优控制序列的第一项作为当前时刻的控制量。这是该算法和预先计算控制律的算法之间的根本区别。模型预测控制的控制思想和具体控制系统与数学模型关联不大，但是其控制作用的实现则与模型相关。模型预测控制以最优控制为基础，但在优化控制系统时，并没有使用全局优化，而是使用滚动有限时段优化，因此具有较强的适应性和鲁棒性。该模型控制效果良好，有效地解决了控制过程中的非线性和并行性问题，可以方便地处理受控变量的控制界限。参阅图3所示，一般模型预测控制主要包括四个部分：预测模型、目标函数、滚动优化、反馈校正。
设N为模型预测控制的预测时域，k为系统当前所处时刻，则根据公式(56)可得系统在k时刻的预测模型如下所示：
XN＝ANX(k|k)+BNUN(57)
其中
Figure BDA0004095234920000122
Figure BDA0004095234920000123
模型预测控制采用有限时域范围内的滚动优化代替最优控制中的全局优化，这使得它具有最优控制所不具有的良好全局适应性。目标函数也称为代价函数，用于评估系统的性能表现，因此目标函数的设计是滚动优化过程的重点。由于优化控制系统的目标是利用更小的控制量使得状态量趋于0，因此可以将目标函数表达为状态量和控制量的二次函数，其形式如下所示：
Figure BDA0004095234920000131
其中
Figure BDA0004095234920000132
和
Figure BDA0004095234920000133
分别表示状态量和输入量的权重矩阵，代表了每个状态量和每个输入量在优化过程中的重要程度，并且满足Q≥0,P≥0。
将目标函数进一步转化为矩阵的形式可得：
Figure BDA0004095234920000134
其中
Figure BDA0004095234920000135
将预测模型(57)带入目标函数(59)可得：
Figure BDA0004095234920000136
由于在k时刻，X(k|k)、AN、
Figure BDA0004095234920000137
均为确定量，故
Figure BDA0004095234920000138
为与UN无关的标量，在优化过程中可以忽略。因此目标函数J是关于UN的二次型，可以将优化问题转化为一个标准的二次规划问题。
系统的目标函数可以表达为如下形式：
Figure BDA0004095234920000139
其中
Figure BDA00040952349200001310
矩阵为一个正定的Hessian矩阵，表示二次规划中的二次部分；
Figure BDA00040952349200001311
表示二次规划中的线性部分；
Figure BDA00040952349200001312
表示二次规划中与UN无关的标量。
系统在k时刻的最优控制序列可以通过下式求得：
Figure BDA00040952349200001313
系统在每一采样时刻都需要通过优化过程求得最优控制序列，在求得UN *之后仅取第一个元素U*(k)作为k时刻的系统控制量误差，而忽略最优控制序列UN *中的其他值，因此这一优化过程被称为滚动优化。
若系统的实际控制量u为机器人的实际速度
Figure BDA0004095234920000141
则有
Figure BDA0004095234920000142
所以系统在k时刻的实际控制量为：
umpc(k)＝ur(k)-U*(k)(63)
由于模型失准或者外界扰动的影响，基于模型的预测值可能会偏离实际值，若不加以修正，进一步的优化则会建立在虚假预测值的基础上，因此必须将系统的测量值反馈到模型中进行修正，这一过程称为反馈校正。在每一采样时刻，将控制量输入系统后可以得到移动机器人下一时刻的实际位姿，实际位姿与参考位姿进行对比可以得到位姿误差，将该误差通过加权的方式反馈到系统在当前时刻的预测模型中，进而完成系统的反馈校正。反馈校正环节可以在线对模型误差进行一定范围的修正，能够提高控制系统的鲁棒性。
此外，全方位移动机器人在运动过程中会受到各种各样的运动约束，在进行模型预测控制时需要考虑这些约束条件。对于任何真实世界的系统，控制输入都受到物理限制，而且控制输入也需要一定的平滑性和连续性，因此系统存在如下约束条件：
Figure BDA0004095234920000143
步骤4：设计扩张状态观测器，估计状态量与干扰量；
在真实环境中往往存在许多无法测量的未知扰动，这些扰动的存在会极大地降低系统的鲁棒性。在模型预测控制中，系统性能以来于一个足够准确的系统模型，尽管滚动优化和反馈校正使得模型预测控制具有一定的鲁棒性，但仍然不足以完全解决系统扰动问题。
通常情况下，系统的状态信息无法直接通过传感器测量而直接获取，而模型预测控制的实现离不开系统状态量。由于系统的输出量一般可以直接测量得到，因此可以利用输出反馈的方式来获取系统状态的估计量。
为了同时解决上述两个问题，可以设计扩张状态观测器来估计系统的状态量和干扰量。
根据公式(55)和(56)，建立考虑扰动的误差运动学模型为：
Figure BDA0004095234920000151
其中d(k)表示系统的干扰量。
系统的扩张状态观测器可以设计为：
Figure BDA0004095234920000152
其中
Figure BDA0004095234920000153
表示状态的估计量，
Figure BDA0004095234920000154
表示干扰的估计量，H1＝diag(ξ1ω0 ξ1ω0 ξ1ω0)和H2＝diag(ξ2ω0 ξ2ω0 ξ2ω0)表示观测器增益矩阵，ω0表示观测器带宽，ξ1和ξ2表示观测器增益系数，且ω0，ξ1和ξ2均为正实数。
定义状态估计误差为
Figure BDA0004095234920000155
干扰估计误差为
Figure BDA0004095234920000156
结合公式(65)和(66)，可以计算得到：
Figure BDA0004095234920000157
将上式转化成成矩阵形式如下所示：
Figure BDA0004095234920000158
其中I3×3代表三维单位矩阵，03×3代表三维零矩阵,
Figure BDA0004095234920000159
为了确保扩张状态观测器误差能够逐渐趋近于零，需要对观测器的稳定性进行判断。根据离散系统的判稳定理，当矩阵G的特征值均小于1时，观测器误差必定能够趋于稳定。因此，通过设计合理的观测器增益矩阵H1和H2使得矩阵G的特征值均小于1，则能够保证所设计扩张状态观测器的稳定性。
利用观测器估计得到的状态量，可以在状态量无法测量的情况下实现模型预测控制。同时，将观测器估计得到的干扰量以补偿的形式作为控制量的一部分，能够抵消干扰量对系统性能的不利影响，极大地提高系统的鲁棒性。因此，系统的控制量更新为如下形式：
Figure BDA00040952349200001510
步骤5：设计积分控制量，消除系统的稳态误差。
为了能够完全消除系统的稳态误差，可以使用积分器校正全方位移动机器人的位姿误差，即在控制量中添加误差积分项。在系统运行的初始阶段，使用模型预测控制来使系统快速跟踪给定值；当控制量接近给定值时，在原本控制律的基础上加入积分控制来消除稳态误差，提高控制系统精度和鲁棒性。
由于在系统的误差运动学模型中，系统误差为qe＝[xe ye φe]T，则误差积分项可以表示为：
Figure BDA0004095234920000161
其中KI＝diag(k1i k2i k3i)表示积分项的系数矩阵，且KI为正定矩阵。
设系统进行控制律切换的误差临界值为，则系统最终的控制律如下所示：
Figure BDA0004095234920000162
最终可以得到完整的控制系统结构框图，如图4所示。
步骤6：通过运动学控制器求得全方位移动机器人的实际速度和实际位姿，完成全方位移动机器人对期望轨迹的跟踪。
全方位移动机器人的鲁棒模型预测控制方法的仿真结果如图5所示。
从图中可以看出，对于较为复杂的圆形轨迹，全方位移动机器人的位置误差和姿态角误差都可以快速地收敛到期望轨迹上，并且即使在干扰存在的情况下实际轨迹也不会偏离给定轨迹。因此，本发明提出的方法能够实现全方位移动机器人的精确、快速、稳定的轨迹跟踪。
需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者终端设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者终端设备所固有的要素。在没有更多限制的情况下，由语句“包括……”或“包含……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者终端设备中还存在另外的要素。此外，在本文中，“大于”、“小于”、“超过”等理解为不包括本数；“以上”、“以下”、“以内”等理解为包括本数。
尽管已经对上述各实施例进行了描述，但本领域内的技术人员一旦得知了基本创造性概念，则可对这些实施例做出另外的变更和修改，所以以上所述仅为本发明的实施例，并非因此限制本发明的专利保护范围，凡是利用本发明说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本发明的专利保护范围之内。

Claims (6)

1.一种全方位移动机器人的鲁棒模型预测控制方法，其特征在于，该方法包括以下步骤：
步骤1：根据全方位移动机器人的运动特性，建立全方位移动机器人的运动学模型；
步骤2：利用线性化和离散化方法，建立全方位移动机器人的误差运动学模型；
步骤3：基于模型预测控制方法设计全方位移动机器人的运动学控制器；
步骤4：设计扩张状态观测器，估计状态量与干扰量；
步骤5：设计积分控制量，消除系统的稳态误差。
步骤6：通过运动学控制器求得全方位移动机器人的实际速度和实际位姿，实现全方位移动机器人对期望轨迹的精准跟踪。
2.如权利要求1所述的全方位移动机器人的鲁棒模型预测控制方法，其特征在于，
步骤1所述全方位移动机器人的运动学模型：
全方位移动机器人在任意时刻的实际位姿向量为q＝[x y φ]T，其中x，y，φ分别表示全方位移动机器人的实际横向位置、实际纵向位置以及实际姿态角。全方位移动机器人在任意时刻的实际速度向量为v＝[vx vy ω]T，其中vx，vy，ω分别表示全方位移动机器人的实际横向速度、实际纵向速度以及实际姿态角速度。
在机器人坐标系中，全方位移动机器人在任意时刻的实际位姿向量为qc＝[xc yc φc]T，其中xc，yc，φc分别表示全方位移动机器人在机器人坐标系下的实际横向位置、实际纵向位置以及实际姿态角。在机器人坐标系中，全方位移动机器人在任意时刻的实际速度向量为vc＝[vcx vcy ωc]T，其中vcx，vcy，ωc分别表示全方位移动机器人在机器人坐标系下的实际横向速度、实际纵向速度以及实际姿态角速度。
通过分析坐标转换关系，可以推导出如下关系：
qc＝T(φ)q (1)
vc＝T(φ)v (2)
其中
Figure FDA0004095234910000011
表示坐标系转换矩阵。
全方位移动机器人在任意时刻的四个轮子的旋转角速度向量为
Figure FDA0004095234910000021
θi(i＝1,2,3,4)表示全方位移动机器人四个轮子的旋转角速度。
全方位移动机器人的运动学模型描述的是机器人的实际速度与四个轮子的实际转速之间的关系，其正逆运动学模型可分别表示为：
Figure FDA0004095234910000022
Figure FDA0004095234910000023
其中D为全方位移动机器人的长度2D1与宽度2D2相加之和的一半，R为全方位移动机器人的四个轮子的半径。
3.如权利要求1所述的全方位移动机器人的鲁棒模型预测控制方法，其特征在于，
步骤2所述全方位移动机器人的误差运动学模型：
定义全方位移动机器人在世界坐标系下的期望位姿向量为qr＝[xr yr φr]T，其中xr，yr，φr分别表示期望横向位置、期望纵向位置以及期望姿态角；全方位移动机器人在世界坐标系下的期望速度向量为vr＝[vxr vyr ωr]T，其中vxr，vyr，ωr分别表示期望横向速度、期望纵向速度以及期望姿态角速度；
定义全方位移动机器人在机器人坐标系下的期望位姿向量为qcr＝[xcr ycr φcr]T，其中xcr，ycr，φcr分别表示期望横向位置、期望纵向位置以及期望姿态角；全方位移动机器人在机器人坐标系下的期望速度向量为vcr＝[vcxr vcyr ωcr]T，其中vcxr，vcyr，ωcr分别表示期望横向速度、期望纵向速度以及期望姿态角速度；
全方位移动机器人在世界坐标系中和在机器人坐标系中的位姿误差向量分别为：
Figure FDA0004095234910000024
Figure FDA0004095234910000031
其中xe，ye，φe分别表示全方位移动机器人的横向位置误差、纵向位置误差以及姿态角误差；xce，yce，φce分别表示全方位移动机器人在机器人坐标系下的横向位置误差、纵向位置误差以及姿态角误差。
根据公式(1)可得如下方程：
qce＝T(φ)qe (7)
对位姿误差向量qce求导，并将公式(7)代入计算可得如下方程：
Figure FDA0004095234910000032
其中
Figure FDA0004095234910000033
表示全方位移动机器人在世界坐标系下的速度误差向量，且有
Figure FDA0004095234910000034
在公式(8)中，若分别以qce和
Figure FDA0004095234910000035
作为状态量X和控制量U，且输出量Y＝qe，则可得如下误差状态空间方程：
Figure FDA0004095234910000036
其中
Figure FDA0004095234910000037
通过欧拉方法可以将上述误差状态方程离散化，可得全方位移动机器人的误差运动学模型为：
Figure FDA0004095234910000038
其中k为采样时刻，ΔT为采样时间间隔，
Figure FDA0004095234910000039
Figure FDA0004095234910000041
4.如权利要求1所述的全方位移动机器人的鲁棒模型预测控制方法，其特征在于，
步骤3所述的运动学控制器：
设N为模型预测控制的预测时域，k为系统当前所处时刻，则根据公式(10)可得系统在k时刻的预测模型如下所示：
XN＝ANX(k|k)+BNUN(11)
其中
Figure FDA0004095234910000042
Figure FDA0004095234910000043
目标函数可以表达为状态量和控制量的二次函数，其形式如下所示：
Figure FDA0004095234910000044
其中
Figure FDA0004095234910000045
而
Figure FDA0004095234910000046
和
Figure FDA0004095234910000047
分别表示状态量和输入量的权重矩阵，代表了每个状态量和每个输入量在优化过程中的重要程度，并且满足Q≥0,P≥0。
将预测模型(11)带入目标函数(12)，则系统的目标函数可以重新表达为如下形式：
Figure FDA0004095234910000048
其中
Figure FDA0004095234910000049
矩阵为一个正定的Hessian矩阵，表示二次规划中的二次部分；
Figure FDA00040952349100000410
表示二次规划中的线性部分；
Figure FDA00040952349100000411
表示二次规划中与UN无关的标量。
系统在k时刻的最优控制序列可以通过下式求得：
Figure FDA0004095234910000051
系统在每一采样时刻都需要通过优化过程求得最优控制序列，在求得UN *之后仅取第一个元素U*(k)作为k时刻的系统控制量误差，而忽略最优控制序列UN *中的其他值，因此这一优化过程被称为滚动优化。
若系统的实际控制量u为机器人的实际速度
Figure FDA0004095234910000052
则有
Figure FDA0004095234910000053
所以系统在k时刻的实际控制量为：
Figure FDA0004095234910000054
对于任何真实世界的系统，控制输入都受到物理限制，而且控制输入也需要一定的平滑性和连续性，因此系统存在如下约束条件：
Figure FDA0004095234910000055
5.如权利要求1所述的全方位移动机器人的鲁棒模型预测控制方法，其特征在于，
步骤4所述的扩张状态观测器：
根据公式(9)和(10)，建立考虑扰动的误差运动学模型为：
Figure FDA0004095234910000056
其中d(k)表示系统的干扰量。
系统的扩张状态观测器可以设计为：
Figure FDA0004095234910000057
其中
Figure FDA0004095234910000058
表示状态的估计量，
Figure FDA0004095234910000059
表示干扰的估计量，H1＝diag(ξ1ω0 ξ1ω0 ξ1ω0)和H2＝diag(ξ2ω0 ξ2ω0 ξ2ω0)表示观测器增益矩阵，ω0表示观测器带宽，ξ1和ξ2表示观测器增益系数，且ω0，ξ1和ξ2均为正实数。
定义状态估计误差为
Figure FDA00040952349100000510
干扰估计误差为
Figure FDA00040952349100000511
结合公式(17)和(18)，可以计算得到：
Figure FDA0004095234910000061
将上式转化成成矩阵形式如下所示：
Figure FDA0004095234910000062
其中I3×3代表三维单位矩阵，03×3代表三维零矩阵,
Figure FDA0004095234910000063
为了确保扩张状态观测器误差能够逐渐趋近于零，需要对观测器的稳定性进行判断。根据离散系统的判稳定理，当矩阵G的特征值均小于1时，观测器误差必定能够趋于稳定。因此，通过设计合理的观测器增益矩阵H1和H2使得矩阵G的特征值均小于1，则能够保证所设计扩张状态观测器的稳定性。
利用观测器估计得到的状态量，可以在状态量无法测量的情况下实现模型预测控制。同时，将观测器估计得到的干扰量以补偿的形式作为控制量的一部分，能够抵消干扰量对系统性能的不利影响，极大地提高系统的鲁棒性。因此，系统的控制量更新为如下形式：
Figure FDA0004095234910000064
6.如权利要求1所述的全方位移动机器人的鲁棒模型预测控制方法，其特征在于，
步骤5所述的积分控制量：
由于在系统的误差运动学模型中，系统误差为qe＝[xe ye φe]T，则误差积分项可以表示为：
Figure FDA0004095234910000065
其中KI＝diag(k1i k2i k3i)表示积分项的系数矩阵，且KI为正定矩阵。
设系统进行控制律切换的误差临界值为，则系统最终的控制律如下所示：
Figure FDA0004095234910000066
CN202310163972.1A 2023-02-24 2023-02-24 一种全方位移动机器人的鲁棒模型预测控制方法 Pending CN116165895A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310163972.1A CN116165895A (zh) 2023-02-24 2023-02-24 一种全方位移动机器人的鲁棒模型预测控制方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310163972.1A CN116165895A (zh) 2023-02-24 2023-02-24 一种全方位移动机器人的鲁棒模型预测控制方法

Publications (1)

Publication Number Publication Date
CN116165895A true CN116165895A (zh) 2023-05-26

Family

ID=86421693

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310163972.1A Pending CN116165895A (zh) 2023-02-24 2023-02-24 一种全方位移动机器人的鲁棒模型预测控制方法

Country Status (1)

Country Link
CN (1) CN116165895A (zh)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116841328A (zh) * 2023-09-01 2023-10-03 中国科学院空天信息创新研究院 一种无人农机横向和纵向控制的方法、装置与电子设备
CN117055361A (zh) * 2023-10-12 2023-11-14 纳博特南京科技有限公司 基于滑膜模型预测控制的移动机器人控制方法
CN117075525A (zh) * 2023-10-12 2023-11-17 纳博特南京科技有限公司 基于约束型模型预测控制的移动机器人控制方法

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116841328A (zh) * 2023-09-01 2023-10-03 中国科学院空天信息创新研究院 一种无人农机横向和纵向控制的方法、装置与电子设备
CN116841328B (zh) * 2023-09-01 2023-11-10 中国科学院空天信息创新研究院 一种无人农机横向和纵向控制的方法、装置与电子设备
CN117055361A (zh) * 2023-10-12 2023-11-14 纳博特南京科技有限公司 基于滑膜模型预测控制的移动机器人控制方法
CN117075525A (zh) * 2023-10-12 2023-11-17 纳博特南京科技有限公司 基于约束型模型预测控制的移动机器人控制方法
CN117075525B (zh) * 2023-10-12 2023-12-19 纳博特南京科技有限公司 基于约束型模型预测控制的移动机器人控制方法
CN117055361B (zh) * 2023-10-12 2023-12-19 纳博特南京科技有限公司 基于滑模模型预测控制的移动机器人控制方法

Similar Documents

Publication Publication Date Title
Boukattaya et al. On trajectory tracking control for nonholonomic mobile manipulators with dynamic uncertainties and external torque disturbances
CN116165895A (zh) 一种全方位移动机器人的鲁棒模型预测控制方法
Hu et al. Robust H∞ output-feedback control for path following of autonomous ground vehicles
Li et al. Vision-based model predictive control for steering of a nonholonomic mobile robot
Dai et al. Force control for path following of a 4WS4WD vehicle by the integration of PSO and SMC
Coelho et al. Path-following control of mobile robots in presence of uncertainties
Fang et al. Adaptive stabilization for uncertain nonholonomic dynamic mobile robots based on visual servoing feedback
Cui et al. An adaptive unscented kalman filter-based controller for simultaneous obstacle avoidance and tracking of wheeled mobile robots with unknown slipping parameters
Koubaa et al. Adaptive sliding-mode dynamic control for path tracking of nonholonomic wheeled mobile robot
Shi-Cai et al. Robust leader-follower formation control of mobile robots based on a second order kinematics model
Marin et al. Event-based localization in ackermann steering limited resource mobile robots
Yuan et al. Whole-body control of an autonomous mobile manipulator using model predictive control and adaptive fuzzy technique
Wu et al. Vision-based neural predictive tracking control for multi-manipulator systems with parametric uncertainty
Hu et al. Impedance sliding mode control with adaptive fuzzy compensation for robot-environment interacting
Shen et al. Leader-follower formation control without leader’s velocity information
Ribeiro et al. Nonlinear model predictive visual path following control to autonomous mobile robots
Lashkari et al. Development of a new robust controller with velocity estimator for docked mobile robots: Theory and experiments
Mutti et al. Improved tracking and docking of industrial mobile robots through UKF vision-based kinematics calibration
Korayem et al. Finite-time feedback linearization (FTFL) controller considering optimal gains on mobile mechanical manipulators
Murugendran et al. Modeling and path-following for a snake robot with active wheels
Jin et al. Path following control of mobile robot using lyapunov techniques and pid cntroller
CN109648566B (zh) 电机参数未知的全方位移动机器人的轨迹跟踪控制方法
Wang et al. Visual regulation of a nonholonomic wheeled mobile robot with two points using Lyapunov functions
CN112051736B (zh) 一种基于单控制器的轨迹跟踪控制系统
Dang et al. Dynamic virtual target guidance algorithm for path following control of a 4WD4WS mobile robot

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination