CN113721607B - 轮式机器人轨迹跟踪最优控制方法 - Google Patents

轮式机器人轨迹跟踪最优控制方法 Download PDF

Info

Publication number
CN113721607B
CN113721607B CN202110937365.7A CN202110937365A CN113721607B CN 113721607 B CN113721607 B CN 113721607B CN 202110937365 A CN202110937365 A CN 202110937365A CN 113721607 B CN113721607 B CN 113721607B
Authority
CN
China
Prior art keywords
wheeled robot
actual
robot
virtual
error
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.)
Active
Application number
CN202110937365.7A
Other languages
English (en)
Other versions
CN113721607A (zh
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.)
Yanshan University
Original Assignee
Yanshan University
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 Yanshan University filed Critical Yanshan University
Priority to CN202110937365.7A priority Critical patent/CN113721607B/zh
Publication of CN113721607A publication Critical patent/CN113721607A/zh
Application granted granted Critical
Publication of CN113721607B publication Critical patent/CN113721607B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle

Landscapes

  • Engineering & Computer Science (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (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控制演变过来，其采取PID误差反馈控制的核心理念，不依赖于系 统的精确数学模型，具有很强的抗干扰能力，因此可以用于轮式机器人的轨迹跟踪控制。 自抗扰控制器主要包括三个部分：跟踪微分器，扩张状态观测器和非线性状态误差反馈控 制器(非线性组合)。跟踪微分器解决由不连续或带随机噪声的量测信号，合理提取连续信 号(跟踪给定)及微分信号的问题。根据微分输出与最速控制综合函数，安排闭环系统的 过渡过程。扩张状态观测器将影响被控对象输出的内部和外部扰动作用扩张成新的状态变 量，通过特殊的反馈机制观测被扩张的总扰动信号。非线性状态误差反馈控制器根据跟踪 微分器获得的给定信号和给定信号微分与扩张状态观测器观测到的系统输出和输出导数的 误差，进行控制和扰动补偿，非线性控制方法通过fal函数或最速控制综合函数Fhan进行 构造。
最优控制是现代控制理论的核心。最优控制是指在一定条件下，在完成所要求的控制 任务时，使系统的某种性能指标具有最优值。根据系统不同的用途，提出各种不用的性能 指标。最优控制的设计就是选择最优控制，使某一种性能指标为最小。线性二次型最优控 制设计是基于状态空间技术来设计一个优化的动态控制器。系统模型是用状态空间形式给 出的线性系统，其目标函数是状态和控制输入的二次型函数。二次型问题就是在线性系统 约束条件下选择控制输入使二次型目标函数达到最小。
固定时间稳定性理论由Andrieu在2008年首次提出，固定时间收敛在保证被控系统 有限时间收敛的同时，还要保证系统的收敛时间能依据系统参数进行估计，即收敛时间估 计不依赖系统初值。固定时间收敛系统不但收敛速度快，鲁棒性强，而且有根据参数估计 收敛时间的优点。
发明内容
本发明的目的在于提供一种基于双闭环控制策略的轮式机器人轨迹跟踪最优控制方 法。该方法通过给出的性能指标函数获得最优控制增量，解决轮式机器人在受到外界干扰 以及内部参数不确定下依然能够跟随给定轨迹的问题。
为实现上述目的，本发明采用以下技术方案：一种轮式机器人轨迹跟踪最优控制方法， 其包括如下步骤：
S1、建立实际轮式机器人和虚拟轮式机器人的运动学模型，得出实际轮式机器人和虚 拟轮式机器人轨迹跟踪误差模型；
S2、设计实际轮式机器人和虚拟轮式机器人轨迹跟踪误差模型的滑模面，根据积分滑 模控制理论设计运动学控制器，使得轮式机器人的位置误差渐近收敛；
S3、综合考虑外界干扰以及轮式机器人内部模型不确定性，建立轮式机器人的动力学 模型；
S4、设计固定时间非线性扩张状态观测器，观测轮式机器人速度并估计外界干扰和内 部不确定性，保证观测误差在固定时间内收敛到零；
S5、基于线性二次型最优控制理论设计动力学控制器，最小化给定性能指标函数，求出 控制输入力矩，使轮式机器人能够跟随上给定参考速度。
本发明所述步骤S1建立实际轮式机器人和虚拟轮式机器人的运动学模型，得出实际轮 式机器人和虚拟轮式机器人轨迹跟踪误差模型，具体步骤如下：
S1.1、建立实际轮式机器人和虚拟轮式机器人的运动学模型；
S1.2、通过坐标转换，得出实际轮式机器人和虚拟轮式机器人轨迹跟踪误差模型；
ex＝(xr-x)cosθ+(yr-y)sinθ
ey＝(x-xr)sinθ+(yr-y)cosθ
eθ＝θr-θ
其中，ex、ey、eθ为轮式机器人真实位姿与虚拟位姿之间的偏差；x、y为实际轮式机器人在全局坐标系下的位置，θ为实际轮式机器人在全局坐标系下的角度信息，三个量共同组成了实际轮式机器人的位姿信息；xr，yr为虚拟轮式机器人在全局坐标系下的位置，θr为虚拟轮式机器人在全局坐标系下的角度信息，三个量共同组成了虚拟轮式机器人的位姿信息；
将跟踪误差模型求导得：
Figure BDA0003213760840000031
Figure BDA0003213760840000032
Figure BDA0003213760840000033
其中，w、v为实际轮式机器人的线速度、角速度，wr、vr为虚拟轮式机器人的线速度、角速度。
本发明所述步骤S2设计实际轮式机器人和虚拟轮式机器人轨迹跟踪误差模型的滑模 面，根据积分滑模控制理论设计运动学控制器，使得轮式机器人的位置误差渐近收敛，具 体方法如下：
S2.1、根据步骤S1建立的实际轮式机器人和虚拟轮式机器人轨迹跟踪误差模型，以及 积分滑模控制理论设计轨迹跟踪误差模型的滑模面s＝[s1 s2]T：
s1＝ex+k1∫ex
s2＝eθ+k2sign(eθ)∫|ey|+k3∫eθ
其中，ex、ey、eθ为轮式机器人真实位姿与虚拟位姿之间的偏差；k1、k2和k3分别 为大于零的可调参数，sign为符号函数，其具体表达式如下：
Figure BDA0003213760840000034
S2.2、根据滑模面，设计轮式机器人的运动学控制器如下：
vk＝vrcoseθ+wey+k1ex+k4sgn(s1)
wk＝wr+k2sign(eθ)|ey|+k3eθ+k5sgn(s2)
其中，sgn(s)＝|s|sign(s)，k4、k5分别为大于0的可调参数；
vk、wk为运动学控制器输出的线速度和角速度，可表示为uk＝[vk wk]T；
vr、wr为参考轨迹的线速度和角速度；w为实际轮式机器人的线速度；
s1、s2为滑动模态；当s1收敛为零，则轨迹跟踪横向误差ex趋于零；当s2趋近于零，在稳态下有
Figure BDA0003213760840000041
由于ey总是有界的，则/>
Figure BDA0003213760840000042
和eθ符号相反，从而使eθ为零，最终由于s2和eθ趋近于零，ey趋于零。
本发明所述步骤S3建立轮式机器人的动力学模型的具体步骤如下：
S3.1、根据轮式机器人的运动机理，建立动力学模型如下:
Figure BDA0003213760840000043
其中，u＝[v w]T为实际轮式机器人的速度信息；
Figure BDA0003213760840000044
其中，m为轮式机器人质量，I为轮式机器人转动惯量，h为轮式机器人质心到几 何中心的距离，n为轮式机器人驱动轮之间的距离，r为轮式机器人的车轮半径；d为轮式 机器人所受外部扰动，假设外部扰动的一阶导数存在且有界，τ＝[τL τR]为左右轮的控制 力矩输入；
S3.2、将动力学模型转换为如下形式：
Figure BDA0003213760840000045
其中，B＝M-1b，D＝M-1d；
S3.3、将步骤S3.2转换后的动力学模型扩张为如下二阶系统：
Figure BDA0003213760840000046
Figure BDA0003213760840000047
其中，x1＝u(u＝[v w]T是实际轮式机器人的速度信息)，x2＝D，
Figure BDA0003213760840000048
是有界变量。/>
本发明所述步骤S4设计固定时间非线性扩张状态观测器，观测轮式机器人速度并估计 外界干扰和内部不确定性，保证观测误差在固定时间内收敛到零，具体步骤如下：
S4.1、设计固定时间非线性扩张状态观测器：
Figure BDA0003213760840000051
其中，e1为速度观测误差；x1为轮式机器人的实际速度信息，z1为x1估计值，z2为 系统所受总扰动的估计值；τ＝[τL τR]为左右轮的控制力矩输入，β1、β2为大于零的可调 观测器参数；ffal(e1，a1，a2，δ)具体形式如下：
Figure BDA0003213760840000052
其中，a1、a2、δ均为大于0的可调参数；
S4.2、证明所设计固定时间非线性扩张状态观测器的收敛特性；
定义e2＝z2-D为扰动观测误差，对误差求导得：
Figure BDA0003213760840000053
Figure BDA0003213760840000054
当e1≥δ时有：
Figure BDA0003213760840000055
因
Figure BDA0003213760840000056
有界，基于固定时间稳定性理论改进fal函数的非线性扩张状态观测器固定时间 收敛。
本发明所述步骤S5基于线性二次型最优控制理论设计动力学控制器，最小化给定性能 指标函数，求出控制输入力矩，使轮式机器人能够跟随上给定参考速度，即v对vk、w对wk的跟踪，具体方法如下：
S5.1、根据动力学模型建立速度误差的状态方程为：
Figure BDA0003213760840000057
其中，ec＝u-uk，u表示实际轮式机器人的速度信息，uk是运动学控制器输出的 速度信息；Δu＝τr-τ为控制律误差向量，τ＝[τL τR]为左右轮的实际控制力矩输入，τr是基于运动学控制器计算的力矩输入信息；A为零矩阵；B＝M-1b。
S5.2、通过速度误差的状态方程给出性能指标函数J，
Figure BDA0003213760840000061
其中，Q∈R2×2为半正定的误差权重矩阵，
Figure BDA0003213760840000064
为速度误差代价，R∈R2×2为 正定的对角型的控制律误差权重矩阵，ΔuTRΔu为控制律误差代价；
S5.3、最小化性能指标函数获得最优的动力学控制器如下：
Figure BDA0003213760840000062
其中，Δu*为最优控制增量，M∈R2×2；
uk＝[vk wk]T，vk、wk为运动学控制器输出的线速度和角速度；
根据最优控制理论，最优控制增量Δu*表示为：
Δu*＝-Kec＝-R-1BTPec
其中，K为通过性能指标函数求得的最优反馈增益矩阵，P为常数正定矩阵，且P满足黎卡提(Riccati)代数方程：
PA+ATP-PBR-1BTP+Q＝0；
对于动力学模型和上述性能指标函数，选择合适的权重矩阵Q和R，求解一个最优控制增量Δu*，进而获得最优控制律
Figure BDA0003213760840000063
使得系统性能指标函数J的数值最小，即用最优的控制能量，使得速度误差ec保持在零值附近
本发明提出了采用积分滑模控制方法设计的运动学控制器与基于线性二次型最优控制 理论设计的动力学控制器相结合的双闭环轨迹跟踪控制策略。利用固定时间非线性扩张状 态观测器对系统外部扰动和内部不确定性进行估计并补偿到动力学控制器，进一步对双闭 环系统稳定性和算法迭代可行性进行了理论分析，仿真结果验证了所提方法的有效性和鲁 棒性。本发明采用固定时间非线性扩张状态观测器结合LQR(线性二次型调节器)最优控 制设计动力学控制器，可以通过调节性能指标函数系数平衡控制品质与输入能量大小，进 而减少了执行机构负担，因此本发明更符合实际应用场景。
本发明与现有技术相比本发明具有以下优点：
1、实现了轮式机器人准确跟踪给定参考轨迹且满足指定性能指标。
2、首次提出了基于固定时间稳定性理论改进fal函数的非线性扩张状态观测器，估计 轮式机器人的速度、外界干扰和内部不确定，为控制补偿提供参考数据。
3、设计的基于线性二次型最优控制理论的动力学控制器，使得轮式机器人用最小的控 制增量快速跟随给定速度，减少执行机构负担。
附图说明
图1为轮式机器人轨迹跟踪示意图；
图2为本发明双闭环控制策略原理图；
图3为本发明采用双闭环控制策略实现轮式机器人轨迹跟踪最优控制方法流程图；
图4为本发明实施例圆形轨迹跟踪图；
图5为本发明实施例轨迹跟踪误差曲线图；
图6为本发明实施例固定时间非线性扩张状态观测器对扰动的估计图。
具体实施方式
下面结合附图和实施例对本发明的结构及特征进行详细说明。需要说明的是，可以对 此处公开的实施例做出各种修改，因此，说明书中公开的实施例不应该视为对本发明的限 制，而仅是作为实施例的范例，其目的是使本发明的特征显而易见。
本发明公开的基于双闭环控制策略的轮式机器人轨迹跟踪最优控制方法，是通过将位 置环(即外环)的运动学控制器与速度环(即内环)的动力学控制器相结合实现轮式机器 人轨迹跟踪的最优控制，其方法为：
S1、建立实际轮式机器人和虚拟轮式机器人的运动学模型，得出实际轮式机器人和虚 拟轮式机器人轨迹跟踪误差模型。
本发明首先建立实际轮式机器人和虚拟轮式机器人的运动学模型，然后，通过坐标转 换，建立轮式机器人轨迹跟踪误差模型。将轮式机器人轨迹跟踪问题转换为实际轮式机器 人与虚拟轮式机器人的位置误差最小问题。
具体步骤如下：
S1.1、建立实际轮式机器人和虚拟轮式机器人的运动学模型。
图1为轮式机器人轨迹跟踪示意图。如图所示，在轮式机器人轨迹跟踪图中，全局坐 标系{x，O，y}是绝对的、静止的坐标系，局部坐标{X，Pm，Y}表示实际轮式机器人主体的方向和 位置。轮式机器人要跟踪的参考轨迹由计算机产生，为方便后续模型建立与控制器设计， 假设参考轨迹由虚拟轮式机器人产生，且虚拟轮式机器人与实际轮式机器人满足同一运动 规则。
实际轮式机器人的运动学模型如下：
Figure BDA0003213760840000081
虚拟轮式机器人的运动学模型如下：
Figure BDA0003213760840000082
其中，u＝[v w]T、ur＝[vr wr]T分别代表实际轮式机器人的线速度、角速度以及虚拟轮式机器人的线速度、角速度；
其中，q＝[x y θ]T、qr＝[xr yr θr]T，x，y为实际轮式机器人在全局坐标系下的位置，θ为实际轮式机器人的角度信息，三个量共同组成了实际轮式机器人的位姿信息；xr，yr为虚拟轮式机器人在全局坐标系下的位置，θr为虚拟轮式机器人的角度信息，三个量共同组成了虚拟轮式机器人的位姿信息；
其中，
Figure BDA0003213760840000083
为实际轮式机器人和虚拟轮式机 器人运动满足非完整性约束条件，横向速度为零，即/>
Figure BDA0003213760840000084
S1.2、通过坐标转换，得出实际轮式机器人和虚拟轮式机器人轨迹跟踪误差模型。
从全局坐标系到局部坐标系的变换矩阵定义如下：
Figure BDA0003213760840000091
根据实际轮式机器人运动学模型(1)、虚拟轮式机器人运动学模型(2)以及坐标转换 矩阵(3)，定义e＝[ex ey eθ]T为轮式机器人轨迹跟踪误差，建立如下实际轮式机器人和虚 拟轮式机器人轨迹跟踪误差模型：
ex＝(xr-x)cosθ+(yr-y)sinθ
ey＝(x-xr)sinθ+(yr-y)cosθ
eθ＝θr-θ (4)
其中，ex、ey、eθ为轮式机器人真实位姿与虚拟位姿之间的偏差；x、y为实际轮式 机器人在全局坐标系下的位置，θ为实际轮式机器人在全局坐标系下的角度信息，三个量共同组成了实际轮式机器人的位姿信息xr，yr为虚拟轮式机器人在全局坐标系下的位置，θr为虚拟轮式机器人在全局坐标系下的角度信息，三个量共同组成了虚拟轮式机器人的位姿信息。
将跟踪误差模型求导得：
Figure BDA0003213760840000092
其中，
Figure BDA0003213760840000093
和/>
Figure BDA0003213760840000094
分别为ex、ey和eθ的导数，w、v为实际轮式机器人的线速度、 角速度，wr、vr为虚拟轮式机器人的线速度、角速度。
S2、设计实际轮式机器人和虚拟轮式机器人轨迹跟踪误差模型的滑模面，根据积分滑 模控制理论设计运动学控制器，使得轮式机器人的位置误差渐近收敛。
具体方法如下：
S2.1、根据步骤S1建立的实际轮式机器人和虚拟轮式机器人轨迹跟踪误差模型，以及 积分滑模控制理论设计轨迹跟踪误差模型的滑模面s＝[s1 s2]T：
s1＝ex+k1∫ex
s2＝eθ+k2sign(eθ)∫|ey|+k3∫eθ (6)
其中，ex、ey、eθ为轮式机器人真实位姿与虚拟位姿之间的偏差；k1、k2和k3分别 为大于零的可调参数，sign为符号函数，其具体表达式如下：
Figure BDA0003213760840000101
S2.2、根据滑模面，设计轮式机器人的运动学控制器如下：
Figure BDA0003213760840000102
其中，sgn(s)＝|s|sign(s)，k4、k5分别为大于0的可调参数；
vk、wk为运动学控制器(即外环控制器)输出的线速度和角速度，可表示为uk＝[vkwk]T；vr、wr为参考轨迹的线速度和角速度；w为实际轮式机器人的线速度；
s1、s2为滑动模态；对于轮式机器人来说，当s1收敛为零，则轨迹跟踪横向误差ex趋于零；当s2趋近于零，在稳态下有
Figure BDA0003213760840000103
由于ey总是有界的，则/>
Figure BDA0003213760840000104
和eθ符号相反，从而使eθ为零，最终由于s2和eθ趋近于零，ey趋于零。
值得注意的是，对于步骤S1中建立的运动学模型，运动学模型的控制输入为v、w，而给出的控制器输入为vk、wk，这里假设vk＝v、wk＝w，即假设动力学控制器可以实 现完美的速度跟踪，保证速度环稳定。
利用李亚普诺夫第二法，验证运动学控制器的有效性。设计如下李雅普诺夫函数：
Figure BDA0003213760840000105
其中，s＝[s1 s2]T，对其求导得：
Figure BDA0003213760840000106
根据李雅普诺夫稳定性可知，所设计的运动学控制器能够保证跟踪误差趋于零。
S3、综合考虑外界干扰以及轮式机器人内部模型不确定性，建立轮式机器人的动力学 模型。
具体步骤如下：
S3.1、根据轮式机器人的运动机理，建立动力学模型如下:
Figure BDA0003213760840000111
其中，
Figure BDA0003213760840000112
其中，m为轮式机器人质量，I为轮式机器人转动惯量，h为轮式机器人质心到几 何中心的距离，n为轮式机器人驱动轮之间的距离，r为轮式机器人的车轮半径；d为轮式 机器人所受外部扰动，假设外部扰动的一阶导数存在且有界，τ＝[τL τR]为左右轮的控制 力矩输入。
S3.2、将动力学模型(8)转换为如下形式：
Figure BDA0003213760840000113
其中，B＝M-1b，D＝M-1d。
S3.2、将动力学模型(9)扩张为如下二阶系统：
Figure BDA0003213760840000114
其中，x1＝u(u＝[v w]T是实际轮式机器人的速度信息)，x2＝D，
Figure BDA0003213760840000115
是有界变 量。
S4、设计固定时间非线性扩张状态观测器，观测轮式机器人速度并估计外界干扰和内 部不确定性，保证观测误差在固定时间内收敛到零。
具体步骤如下：
S4.1、设计固定时间非线性扩张状态观测器：
Figure BDA0003213760840000116
其中，e1为速度观测误差；x1为轮式机器人的实际线速度和角速度(x1是一个两行一 列的列向量)，z1为x1估计值，z2为系统所受总扰动的估计值；τ＝[τL τR]为左右轮的 控制力矩输入，β1、β2为大于零的可调观测器参数；ffal(e1，a1，a2，δ)具体形式如下：
Figure BDA0003213760840000121
其中，a1、a2、δ均为大于0的可调参数。
S4.2、验证所设计固定时间非线性扩张状态观测器的收敛特性。
定义e2＝z2-D为扰动观测误差，对误差求导得：
Figure BDA0003213760840000122
Figure BDA0003213760840000123
当e1≥δ时有：
Figure BDA0003213760840000124
因
Figure BDA0003213760840000125
有界，根据现有文献可得当观测误差如(12)所示时，基于固定时间稳定性理论 改进fal函数的非线性扩张状态观测器固定时间收敛。
S5、基于线性二次型最优控制理论设计动力学控制器，最小化给定性能指标函数，求出 控制输入力矩，使轮式机器人能够跟随上给定参考速度，即v对vk、w对wk的跟踪；
S5.1、根据动力学模型建立速度误差的状态方程
Figure BDA0003213760840000126
/>
其中，ec＝u-uk(u表示实际轮式机器人的速度信息，uk是运动学控制器输出的 速度信息)，Δu＝τr-τ为控制律误差向量，τ＝[τL τR]为左右轮的实际控制力矩输入， τr是基于运动学控制器计算的力矩输入信息，A为零矩阵，B＝M-1b。
设计左右轮的控制力矩输入，使得机器人实际的线速度、角速度跟随给定参考轨迹的 线速度、角速度。
S5.2、通过速度误差的状态方程(13)给出性能指标函数，
Figure BDA0003213760840000131
其中，ec＝uk-z1，Δu＝τr-τ为控制律误差向量，R∈R2×2，Q∈R2×2为半正 定的误差权重矩阵，
Figure BDA0003213760840000132
为速度误差代价，R∈R2×2为正定的对角型的控制律误差 权重矩阵，ΔuTRΔu为控制律误差代价。
S5.3、最小化性能指标函数获得的最优控制器如下：
Figure BDA0003213760840000133
其中，Δu*为最优控制增量，M∈R2×2。
根据最优控制理论，最优控制增量Δu*表示为：
Δu*＝-Kec＝-R-1BTPec (15)
其中，K为通过性能指标函数求得的最优反馈增益矩阵，P为常数正定矩阵，且P满足 黎卡提(Riccati)代数方程：
PA+ATP-PBR-1BTP+Q＝0。
对于具有非完整约束的轮式机器人系统，考虑固定时间非线性扩张状态观测器、位置 环(外环)的积分滑模控制器、性能指标函数，则速度环(内环)的最优控制器能够保证双闭环控制系统的稳定性。
本发明动力学控制器基于线性二次型最优控制理论设计，它包括前馈控制和反馈控制， 前馈控制指
Figure RE-GDA0003277664830000134
(运动学控制器的微分)，反馈控制指最优控制增量Δu*。
下面通过李雅普诺夫稳定性理论验证闭环系统的稳定性。设计如下李雅普诺夫函数：
Figure BDA0003213760840000135
其中，M为常数矩阵，对李雅普诺夫函数V2求导得
Figure BDA0003213760840000136
根据轮式机器人线性动力学模型得到
Figure BDA0003213760840000141
根据Δu＝τr-τ，把最优控制器(15)和(16)进一步代入
Figure BDA0003213760840000142
可以得到/>
Figure BDA0003213760840000143
因此，
Figure BDA0003213760840000144
双闭环系统稳定。
综上可知轮式移动机器人的轨迹跟踪控制任务是：
对于动力学模型和上述性能指标函数，选择合适的权重矩阵Q和R，求解一个最优控制增量Δu*，进而获得最优控制律
Figure BDA0003213760840000145
使得系统性能指标函数J的 数值最小。即用最优的控制能量，使得速度误差ec保持在零值附近。
实施例
为验证本发明提出的针对轮式机器人的双闭环轨迹跟踪控制策略的有效性，给出了 MATLAB数值仿真结果作为验证，说明在受到外界干扰和系统内部参数不确定情况下，轮式 机器人依然能沿着虚拟轨迹正常行驶，具体如下：
本仿真中，步骤S1.1虚拟参考轨迹为圆形，参考轨迹的线速度为1.4m/s，角速度为0.7rad/s。
步骤S2.1、运动学控制器参数为：k1＝2.1、k2＝6.3、k3＝3、k3＝0.5、k3＝0.7；
步骤S3.1、轮式机器人质量为10kg，差速轮距离为0.2m，转动惯量为5kg.m2，施加的外部干扰和内部不确定为d(t)＝[sin 2t cos 2t]T，系统采样周期为0.01s。
步骤S4.1、固定时间非线性扩张状态观测器的参数为：β1＝8、β1＝5、α1＝0.8、 α2＝1.2、δ＝0.01。
步骤S5.2、动力学控制器中权重矩阵为：
Figure BDA0003213760840000146
图4为圆形跟踪轨迹图，轮式机器人初始位置为：q(t0)＝[1.2 -0.2 0]T，跟踪圆形的半径为2m。
图5为轨迹跟踪误差曲线图，在t＝2s左右，跟踪误差ex、ey、eθ均收敛到零。
图6为固定时间非线性扩张状态观测器对扰动的估计图。从图5可知，本发明步骤S4.1 设计的观测器可以准确估计轮式机器人外部扰动，为动力学控制器设计提供有效的数据信 息。
本发明未尽事宜为公知技术。
上述实施例只为说明本发明的技术构思及特点，其目的在于让熟悉此项技术的人士能 够了解本发明的内容并据以实施，并不能以此限制本发明的保护范围。凡根据本发明精神 实质所作的等效变化或修饰，都应涵盖在本发明的保护范围之内。

Claims (4)

1.一种轮式机器人轨迹跟踪最优控制方法，其特征在于：其包括如下步骤：
S1、建立实际轮式机器人和虚拟轮式机器人的运动学模型，得出实际轮式机器人和虚拟轮式机器人轨迹跟踪误差模型；
S2、设计实际轮式机器人和虚拟轮式机器人轨迹跟踪误差模型的滑模面，根据积分滑模控制理论设计运动学控制器，使得轮式机器人的位置误差渐近收敛；
S3、综合考虑外界干扰以及轮式机器人内部模型不确定性，建立轮式机器人的动力学模型；
S4、设计固定时间非线性扩张状态观测器，观测轮式机器人速度并估计外界干扰和内部模型不确定性，保证观测误差在固定时间内收敛到零；
步骤S4设计固定时间非线性扩张状态观测器，观测轮式机器人速度并估计外界干扰和内部模型不确定性，保证观测误差在固定时间内收敛到零，具体步骤如下：
S4.1、设计固定时间非线性扩张状态观测器：
Figure FDA0003967731930000011
其中，e1为速度观测误差；x1为实际轮式机器人的速度信息，z1为x1估计值，z2为系统所受总扰动的估计值；τ＝[τL τR]为左右轮的控制力矩输入，β1、β2为大于零的可调观测器参数；ffal(e1,a1,a2,δ)具体形式如下：
Figure FDA0003967731930000012
其中，a1、a2、δ均为大于0的可调参数；
S4.2、证明所设计固定时间非线性扩张状态观测器的收敛特性；
定义e2＝z2-D为扰动观测误差，对误差求导得：
Figure FDA0003967731930000021
Figure FDA0003967731930000022
当e1≥δ时有：
Figure FDA0003967731930000023
因
Figure FDA0003967731930000024
有界，基于固定时间稳定性理论改进fal函数的非线性扩张状态观测器固定时间收敛；
S5、基于线性二次型最优控制理论设计动力学控制器，最小化给定性能指标函数,求出控制输入力矩，使轮式机器人能够跟随上给定参考速度；
步骤S5基于线性二次型最优控制理论设计动力学控制器，最小化给定性能指标函数,求出控制输入力矩，使轮式机器人能够跟随上给定参考速度，具体方法如下：
S5.1、根据动力学模型建立速度误差的状态方程
Figure FDA0003967731930000025
其中，ec＝u-uk，u表示实际轮式机器人的速度信息，uk是运动学控制器输出的速度信息；Δu＝τr-τ为控制律误差向量，τ＝[τL τR]为左右轮的实际控制力矩输入，τr是基于运动学控制器计算的力矩输入信息；A为零矩阵；B＝M-1b；
S5.2、通过速度误差的状态方程给出性能指标函数J：
Figure FDA0003967731930000026
其中，Q∈R2×2为半正定的误差权重矩阵，
Figure FDA0003967731930000027
为速度误差代价，R∈R2×2为正定的对角型的控制律误差权重矩阵，ΔuTRΔu为控制律误差代价；
S5.3、最小化性能指标函数获得最优的动力学控制器如下：
Figure FDA0003967731930000028
其中，Δu*为最优控制增量，M∈R2×2；
uk＝[vk wk]T，vk、wk为运动学控制器输出的线速度和角速度；
根据最优控制理论，最优控制增量Δu*表示为：
Δu*＝-Kec＝-R-1BTPec
其中，K为通过性能指标函数求得的最优反馈增益矩阵，P为常数正定矩阵，且P满足黎卡提(Riccati)代数方程：
PA+ATP-PBR-1BTP+Q＝0；
对于动力学模型和上述性能指标函数，选择合适的权重矩阵Q和R，求解一个最优控制增量Δu*，进而获得最优控制律
Figure FDA0003967731930000031
使得系统性能指标函数J的数值最小，即用最优的控制能量，使得速度误差ec保持在零值附近；
其中，B＝M-1b，D＝M-1d，
Figure FDA0003967731930000033
是有界变量；
Figure FDA0003967731930000032
其中，m为轮式机器人质量，I为轮式机器人转动惯量，h为轮式机器人质心到几何中心的距离，n为轮式机器人驱动轮之间的距离，r为轮式机器人的车轮半径；d为轮式机器人所受外部扰动，假设外部扰动的一阶导数存在且有界，τ＝[τL τR]为左右轮的控制力矩输入。
2.根据权利要求1所述的轮式机器人轨迹跟踪最优控制方法，其特征在于：步骤S1建立实际轮式机器人和虚拟轮式机器人的运动学模型，得出实际轮式机器人和虚拟轮式机器人轨迹跟踪误差模型，具体步骤如下：
S1.1、建立实际轮式机器人和虚拟轮式机器人的运动学模型；
S1.2、通过坐标转换，得出实际轮式机器人和虚拟轮式机器人轨迹跟踪误差模型；
ex＝(xr-x)cosθ+(yr-y)sinθ
ey＝(x-xr)sinθ+(yr-y)cosθ
eθ＝θr-θ
其中，ex、ey、eθ为轮式机器人真实位姿与虚拟位姿之间的偏差；x、y为实际轮式机器人在全局坐标系下的位置，θ为实际轮式机器人在全局坐标系下的角度信息，三个量共同组成了实际轮式机器人的位姿信息；xr,yr为虚拟轮式机器人在全局坐标系下的位置，θr为虚拟轮式机器人在全局坐标系下的角度信息，三个量共同组成了虚拟轮式机器人的位姿信息；
将跟踪误差模型求导得：
Figure FDA0003967731930000041
Figure FDA0003967731930000042
Figure FDA0003967731930000043
其中，w、v为实际轮式机器人的线速度、角速度，wr、vr为虚拟轮式机器人的线速度、角速度。
3.根据权利要求2所述的轮式机器人轨迹跟踪最优控制方法，其特征在于：步骤S2设计实际轮式机器人和虚拟轮式机器人轨迹跟踪误差模型的滑模面，根据积分滑模控制理论设计运动学控制器，使得轮式机器人的位置误差渐近收敛，具体方法如下：
S2.1、根据步骤S1建立的实际轮式机器人和虚拟轮式机器人轨迹跟踪误差模型，以及积分滑模控制理论设计轨迹跟踪误差模型的滑模面s＝[s1 s2]T：
s1＝ex+k1∫ex
s2＝eθ+k2sign(eθ)∫|ey|+k3∫eθ
其中，ex、ey、eθ为轮式机器人真实位姿与虚拟位姿之间的偏差；k1、k2和k3分别为大于零的可调参数，sign为符号函数，其具体表达式如下：
Figure FDA0003967731930000044
S2.2、根据滑模面，设计轮式机器人的运动学控制器如下：
vk＝vrcoseθ+wey+k1ex+k4sgn(s1)
wk＝wr+k2sign(eθ)|ey|+k3eθ+k5sgn(s2)
其中，sgn(s)＝|s|sign(s)，k4、k5分别为大于0的可调参数；
vk、wk为运动学控制器输出的线速度和角速度，可表示为uk＝[vk wk]T；
vr、wr为参考轨迹的线速度和角速度；w为实际轮式机器人的线速度；
s1、s2为滑动模态；当s1收敛为零，则ex趋于零；当s2趋近于零，在稳态下有
Figure FDA0003967731930000051
由于ey总是有界的，则
Figure FDA0003967731930000052
和eθ符号相反，从而使eθ为零，最终由于s2和eθ趋近于零，ey趋于零。
4.根据权利要求2或3所述的轮式机器人轨迹跟踪最优控制方法，其特征在于：步骤S3建立轮式机器人的动力学模型的具体步骤如下：
S3.1、根据轮式机器人的运动机理，建立动力学模型如下:
Figure FDA0003967731930000053
其中，u＝[v w]T为实际轮式机器人的速度信息；
S3.2、将动力学模型转换为如下形式：
Figure FDA0003967731930000054
S3.3、将步骤S3.2转换后的动力学模型扩张为如下二阶系统：
Figure FDA0003967731930000055
Figure FDA0003967731930000056
其中，x1＝u，x2＝D。
CN202110937365.7A 2021-08-16 2021-08-16 轮式机器人轨迹跟踪最优控制方法 Active CN113721607B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110937365.7A CN113721607B (zh) 2021-08-16 2021-08-16 轮式机器人轨迹跟踪最优控制方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110937365.7A CN113721607B (zh) 2021-08-16 2021-08-16 轮式机器人轨迹跟踪最优控制方法

Publications (2)

Publication Number Publication Date
CN113721607A CN113721607A (zh) 2021-11-30
CN113721607B true CN113721607B (zh) 2023-03-24

Family

ID=78676008

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110937365.7A Active CN113721607B (zh) 2021-08-16 2021-08-16 轮式机器人轨迹跟踪最优控制方法

Country Status (1)

Country Link
CN (1) CN113721607B (zh)

Families Citing this family (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114442491A (zh) * 2022-04-08 2022-05-06 杭州国辰机器人科技有限公司 室内机器人的局部路径规划和跟踪控制方法、装置及介质
CN115185273B (zh) * 2022-07-06 2024-06-21 天津大学 一种空地异构多机器人轨迹跟踪平台
CN115071732A (zh) * 2022-07-14 2022-09-20 东风商用车有限公司 一种基于lqr的smc商用车智能驾驶横向控制方法
CN115167139A (zh) * 2022-07-27 2022-10-11 兰州理工大学 一种基于新型运动轨迹规划的三维天车递归滑模控制方法
CN116372941B (zh) * 2023-06-05 2023-08-15 北京航空航天大学杭州创新研究院 机器人参数标定方法及装置和轮式机器人
CN117706910B (zh) * 2023-12-27 2024-06-07 哈尔滨工业大学 基于滑模控制的不同最大速度的机器人集群覆盖方法及系统

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104635738A (zh) * 2014-11-21 2015-05-20 沈阳工业大学 不确定康复步行训练机器人的精确轨迹跟踪最优控制方法
CN104932506A (zh) * 2015-06-09 2015-09-23 东南大学 基于快速终端滑模的轮式移动机器人轨迹跟踪方法
CN108614425A (zh) * 2018-06-06 2018-10-02 黄山学院 移动机器人自适应积分滑模控制方法
CN109283841A (zh) * 2017-07-20 2019-01-29 中国空气动力研究与发展中心计算空气动力研究所 关节柔性和臂杆柔性的空间机械臂控制方法
CN110095983A (zh) * 2019-04-22 2019-08-06 浙江工业大学 一种基于路径参数化的移动机器人预测跟踪控制方法
WO2021152047A1 (en) * 2020-01-28 2021-08-05 Five AI Limited Planning in mobile robots

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104635738A (zh) * 2014-11-21 2015-05-20 沈阳工业大学 不确定康复步行训练机器人的精确轨迹跟踪最优控制方法
CN104932506A (zh) * 2015-06-09 2015-09-23 东南大学 基于快速终端滑模的轮式移动机器人轨迹跟踪方法
CN109283841A (zh) * 2017-07-20 2019-01-29 中国空气动力研究与发展中心计算空气动力研究所 关节柔性和臂杆柔性的空间机械臂控制方法
CN108614425A (zh) * 2018-06-06 2018-10-02 黄山学院 移动机器人自适应积分滑模控制方法
CN110095983A (zh) * 2019-04-22 2019-08-06 浙江工业大学 一种基于路径参数化的移动机器人预测跟踪控制方法
WO2021152047A1 (en) * 2020-01-28 2021-08-05 Five AI Limited Planning in mobile robots

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
基于扩张状态观测器的轮式移动机器人全阶滑模控制;郭一军等;《重庆邮电大学学报(自然科学版)》;20170615(第03期);第382-388页 *

Also Published As

Publication number Publication date
CN113721607A (zh) 2021-11-30

Similar Documents

Publication Publication Date Title
CN113721607B (zh) 轮式机器人轨迹跟踪最优控制方法
CN108875253B (zh) 基于干扰观测器的欠驱动吊车系统的终端滑模消摆控制方法及系统
Li et al. Trajectory tracking control for wheeled mobile robots based on nonlinear disturbance observer with extended Kalman filter
CN110181510B (zh) 一种基于时延估计与模糊逻辑的机械臂轨迹跟踪控制方法
CN103728988B (zh) 基于内模的scara机器人轨迹跟踪控制方法
CN103433924A (zh) 串联机器人高精度位置控制方法
CN105772917A (zh) 一种三关节点焊机器人轨迹跟踪控制方法
CN111831002B (zh) 一种基于预设性能的超高声速飞行器姿态控制方法
CN112936277B (zh) 一种水下机器人-机械手系统固定时间轨迹跟踪方法
Liu et al. Robust adaptive output feedback tracking control for flexible-joint robot manipulators based on singularly perturbed decoupling
Hu et al. Impedance sliding mode control with adaptive fuzzy compensation for robot-environment interacting
CN115576341A (zh) 基于函数微分和自适应变增益的无人机轨迹跟踪控制方法
CN114509949A (zh) 一种机器人预定性能的控制方法
Jin et al. Physical-informed neural network for MPC-based trajectory tracking of vehicles with noise considered
CN113835339B (zh) 一种轮式移动机器人超螺旋滑模轨迹跟踪方法
Yang et al. Research on manipulator trajectory tracking based on adaptive fuzzy sliding mode control
Chang et al. Adaptive tracking controller based on the PID for mobile robot path tracking
CN116360258A (zh) 基于固定时间收敛的高超声速变形飞行器抗干扰控制方法
CN116068893A (zh) 一种机械臂自适应分数阶滑模控制方法、设备及介质
Dian et al. A Novel Disturbance-Rejection Control Framework for Cable-Driven Continuum Robots With Improved State Parameterizations
CN115473467A (zh) 基于模糊观测器的柔性关节机械臂指令滤波反步控制方法
Li et al. Neural adaptive tracking control for wheeled mobile robots
CN112147894B (zh) 基于运动学和动力学模型的轮式移动机器人主动控制方法
CN114167725A (zh) 协作机器人轨迹跟踪控制方法及系统
Yang et al. Multi-degree-of-freedom joint nonlinear motion control with considering the friction effect

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
GR01 Patent grant
GR01 Patent grant