轨迹跟踪控制方法、装置及移动机器人与流程

1.本技术涉及移动机器人技术领域，特别针对轨迹跟踪控制方法、装置及移 动机器人。


背景技术：

2.随着家居生活日益智能化，由于移动机器人(wmr)融合了传感器技术、控制 技术、计算机技术等多种技术的相关知识和最新研究成果，以其方便快捷、多 功能、高智能而受到越来越广泛的关注。移动机器人中例如扫地机器人、拖地 机器人和服务机器人等具备自行走功能的机器人是典型的非完整系统，其运动 控制问题是目前研究的重点和难点之一。
3.在实现过程中，发明人发现传统技术中至少存在如下问题：目前对移动机 器人运动控制方案的研究大多建立在“纯滚动无滑动”理想约束的基础上。然 而在实际运行时，由于轮胎变形、路面潮湿或结冰、快速转弯等因素的影响， 容易出现轮子纵向打滑(纵滑)或侧向打滑(侧滑)的情况，此时会破坏系统 的非完整约束，使移动机器人的运动控制设计变得更为复杂，导致移动机器人 失控。


技术实现要素：

4.基于此，有必要针对上述纵滑和侧滑扰动导致的移动机器人失控问题，提 供一种轨迹跟踪控制方法、装置及移动机器人。
5.为实现上述目的，本发明实施例提供了一种轨迹跟踪控制方法，包括：
6.获取移动机器人的当前速度数据和当前位姿数据；所述当前速度数据为基 于预设的动力学模型处理得到；所述当前位姿数据为基于预设的运动学模型处 理得到；
7.基于预先建立的运动学控制模块，对所述移动机器人的参考位姿数据和当 前位姿数据进行处理，得到位姿误差数据，结合所述移动机器人的参考速度数 据得到所述移动机器人的辅助速度用于控制所述移动机器人，使其位姿误差数 据收敛至第一阈值；
8.基于预先建立的动力学控制模块，对所述移动机器人的辅助速度数据和当 前速度数据进行处理，得到速度误差数据，并结合实际速度及集总扰动的估计 值，得到驱动电机控制电压用于驱动所述移动机器人，使其速度误差数据收敛 至第二阈值。
9.在其中一个实施例中，所述预先建立的运动学控制模块的步骤包括：
10.对所述移动机器人的参考位姿数据和当前位姿数据进行处理，得到位姿误 差数据；结合所述移动机器人的参考速度数据，利用反步法得到所述辅助速度 数据。
11.在其中一个实施例中，所述辅助运动学控制模块为通过以下函数得到：
[0012][0013]
其中vc、ωc分别为设计的移动机器人辅助线速度和角速度，vr、ωr分别为移 动机器人的参考线速度和参考角速度，k1、k2、k3为辅助运动学控制律控制参数， 且均为正常数，ex
为移动机器人纵向误差，ey为移动机器人侧向误差，e
θ
为移 动机器人方向误差。
[0014]
在其中一个实施例中，所述的当前速度数据为基于预设的动力学模型处理 得到的步骤包括：
[0015]
获取驱动电机控制电压；
[0016]
根据电机模型，对所述驱动电机控制电压进行处理，得到驱动轮输入力矩；
[0017]
根据所述动力学模型，对所述驱动轮输入力矩和所述预设集总扰动进行处 理，得到所述当前速度数据。
[0018]
在其中一个实施例中，所述电机模型为通过以下函数得到：
[0019]
τ＝k1u
a-k2xz
[0020]
其中，τ＝[τrτ
l
]
t
，τr为移动机器人右轮输入力矩，τ
l
为移动机器人左轮输 入力矩；ua＝[u
ar u
al
]
t
，u
ar
为右轮驱动电机控制电压，u
al
为左轮驱动电机控制 电压；k1＝nk
t
/ra，k2＝n2k
t
kb/ra，ra为左、右轮驱动电机的电枢电阻，k
t
为 左、右轮驱动电机的转矩常数，kb为左、右轮驱动电机的反电动势常数，n为机 械齿轮减速比；r为移动机器人驱动轮半径，b为两驱动轮之间 的二分之一距离；z为移动机器人的实际速度。
[0021]
在其中一个实施例中，所述动力学模型为通过以下函数得到：
[0022][0023]
其中，ua＝[u
ar u
al
]
t
，u
ar
为右轮驱动电机控制电压，u
al
为左轮驱动电机控 制电压；k1＝nk
t
/ra，k2＝n2k
t
kb/ra，ra为左、右轮驱动电机的电枢电阻，k
t
为左、右轮驱动电机的转矩常数，kb为左、右轮驱动电机的反电动势常数，n为 机械齿轮减速比；r为移动机器人驱动轮半径，b为两驱动轮之 间的二分之一距离；z为移动机器人的实际速度；d为对应预设集总扰动； ms＝s
t
ms，bs＝s
t
b，s为系统运动学模型中的变换矩阵，m为 系统动力学模型中的正定对称惯性矩阵，b为系统动力学模型中的输入力矩变 换矩阵。
[0024]
在其中一个实施例中，所述运动学模型为通过以下函数得到：
[0025][0026]
其中，z为移动机器人的实际速度；η＝[ηvη
ω
]
t
，ηv＝r(ξr+ξ
l
)/2为纵滑速 度，η
ω
＝r(ξ
r-ξ
l
)/(2b)为纵滑引起的横摆率扰动，r为移动机器人驱动轮半径， 2b为两驱动轮之间的距离，ξ
l
、ξr分别为移动机器人左、右驱动轮纵滑引起的干 扰角速度矢量；为移动机器人发生侧滑时引起的非 匹配扰动向量，μ为移动机器人侧滑速度；θ为移动机器人 的方向角。
[0027]
在其中一个实施例中，所述预先建立的动力学控制模块包括自抗扰控制模 块；
[0028]
所述基于预先建立的动力学控制模块，对所述移动机器人辅助速度数据和 所述当前速度数据进行处理，得到速度误差数据；
[0029]
基于自抗扰控制模块，结合线性扩张状态观测器(leso)对所述实际速度和 集总扰动的估计值，得到驱动电机电压控制输入，驱动移动机器人使其速度误 差数据收敛至第二阈值。
[0030]
在其中一个实施例中，所述自抗扰控制模块为通过以下函数得到：
[0031][0032]
其中，k
p
为自抗扰控制模块的增益；k1＝nk
t
/ra，k2＝n2k
t
kb/ra，ra为 左、右轮驱动电机的电枢电阻，k
t
为左、右轮驱动电机的转矩常数，kb为左、右 轮驱动电机的反电动势常数，n为机械齿轮减速比；r为移动机 器人驱动轮半径，b为两驱动轮之间的二分之一距离；zc为移动机器人的辅助速 度；ms＝s
t
ms，bs＝s
t
b，s为系统运动学模型中的变换矩阵， m为系统动力学模型中的正定对称惯性矩阵，b为系统动力学模型中的输入力 矩变换矩阵；分别为状态向量x1、x2的估计值，x1＝z,x2＝d，z为移动机 器人的实际速度，d为对应预设集总扰动；ua＝[u
ar u
al
]
t
，u
ar
为右轮驱动电机控 制电压，u
al
为左轮驱动电机控制电压。
[0033]
在其中一个实施例中，所述预先建立的动力学控制模块还包括线性扩张状 态观测器；
[0034]
对所述移动机器人实际速度信息及驱动电机电压输入信息进行处理，得到 实际速度及集总扰动的估计值，用于自抗扰控制器的设计。
[0035]
在其中一个实施例中，所述线性扩张状态观测器为通过以下函数得到：
[0036][0037]
其中，分别为状态向量x1、x2的估计值，x1＝z,x2＝d，z为移动机器 人的实际速度，d为对应预设集总扰动；β1、β2分别为线性扩张状态观测器的第 一增益、第二增益；k1＝nk
t
/ra，k2＝n2k
t
kb/ra，ra为左、右轮驱动电机的 电枢电阻，k
t
为左、右轮驱动电机的转矩常数，kb为左、右轮驱动电机的反电动 势常数，n为机械齿轮减速比；r为移动机器人驱动轮半径，b 为两驱动轮之间的二分之一距离；ms＝s
t
ms，bs＝s
t
b，s为 系统运动学模型中的变换矩阵，m为系统动力学模型中的正定对称惯性矩阵，b 为系统动力学模型中的输入力矩变换矩阵；ua＝[u
ar u
al
]
t
，u
ar
为右轮驱动电机控 制电压，u
al
为左轮驱动电机控制电压。
[0038]
另一方面，本发明实施例还提供了一种轨迹跟踪控制装置，包括：
[0039]
数据获取单元，用于获取移动机器人的当前速度数据和当前位姿数据；所 述当前
速度数据为基于预设的动力学模型处理得到；所述当前位姿数据为基于 预设的运动学模型处理得到；
[0040]
位姿处理单元，用于基于预先建立的运动学控制模块，对所述移动机器人 的参考位姿数据和所述当前位姿数据进行处理，得到位姿误差数据，结合参考 速度数据得到所述移动机器人的辅助速度用于控制所述移动机器人，使其位姿 误差数据收敛至第一阈值；
[0041]
速度处理单元，用于基于预先建立的动力学控制模块，对所述移动机器人 的所述辅助速度数据和所述当前速度数据进行处理，得到速度误差数据，并结 合实际速度及集总扰动的估计值，得到驱动电机控制电压用于驱动所述移动机 器人，使其速度误差数据收敛至第二阈值。
[0042]
另一方面，本发明实施例还提供一种移动机器人，包括移动机器人主体及 设置在移动机器人主体上的控制器；控制器用于执行上述任意一项的轨迹跟踪 控制方法。
[0043]
上述技术方案具有如下优点和有益效果：
[0044]
上述轨迹跟踪控制方法各实施例中，获取移动机器人当前速度数据和当前 位姿数据；当前速度数据为基于预设的动力学模型处理得到；当前位姿数据为 基于预设的运动学模型处理得到；基于预先建立的运动学控制模块，对移动机 器人的参考位姿数据和当前位姿数据进行处理，得到位姿误差数据和辅助速度 控制输入，驱动移动机器人使其位姿误差数据收敛至第一阈值；基于预先建立 的动力学控制模块，对辅助速度数据和当前速度数据进行处理，得到速度误差 数据；根据线性扩张状态观测器对集总扰动进行估计，设计自抗扰控制器驱动 移动机器人，使其速度误差数据收敛至第二阈值，实现对移动机器人的精准轨 迹跟踪，克服了未知纵滑和侧滑扰动、建模不确定性、未知输入扰动及执行器 动力学影响导致的移动机器人失控问题，增强了移动机器人轨迹跟踪控制的鲁 棒性。
附图说明
[0045]
图1为一个实施例中的轨迹跟踪控制方法的应用环境示意图；
[0046]
图2为一个实施例中轨迹跟踪控制方法的第一流程示意图；
[0047]
图3为一个实施例中轨迹跟踪控制方法的第二流程示意图；
[0048]
图4为一个实施例中轨迹跟踪控制方法的第三流程示意图；
[0049]
图5为一个实施例中轨迹跟踪控制方法的控制结构示意图；
[0050]
图6为一个实施例中直行运动轨迹的轨迹跟踪效果曲线示意图；
[0051]
图7为一个实施例中直行运动轨迹的各方向轨迹跟踪效果曲线示意图；
[0052]
图8为一个实施例中直行运动轨迹的位姿跟踪误差曲线示意图；
[0053]
图9为一个实施例中直行运动轨迹的机器人实际速度曲线示意图；
[0054]
图10为一个实施例中直行运动轨迹的各扰动及其估计值曲线示意图；
[0055]
图11为一个实施例中直行运动轨迹的控制电压输入曲线示意图；
[0056]
图12为一个实施例中圆弧运动轨迹的轨迹跟踪效果曲线示意图；
[0057]
图13为一个实施例中圆弧运动轨迹的各方向轨迹跟踪效果曲线示意图；
[0058]
图14为一个实施例中圆弧运动轨迹的位姿跟踪误差曲线示意图；
[0059]
图15为一个实施例中圆弧运动轨迹的机器人实际速度曲线示意图；
[0060]
图16为一个实施例中圆弧运动轨迹的各扰动及其估计值曲线示意图；
[0061]
图17为一个实施例中圆弧运动轨迹的控制电压输入曲线示意图；
[0062]
图18为一个实施例中原地旋转运动轨迹的各方向轨迹跟踪特性曲线示意图；
[0063]
图19为一个实施例中原地旋转运动轨迹的位姿跟踪误差曲线示意图；
[0064]
图20为一个实施例中原地旋转运动轨迹的机器人实际速度曲线示意图；
[0065]
图21为一个实施例中原地旋转运动轨迹的各扰动及其估计值曲线示意图；
[0066]
图22为一个实施例中原地旋转运动轨迹的控制电压输入曲线示意图；
[0067]
图23为一个实施例中轨迹跟踪控制装置的方框示意图。
具体实施方式
[0068]
为了使本技术领域的人员更好地理解本技术方案，下面将结合本技术实施 例中的附图，对本技术实施例中的技术方案进行清楚、完整地描述，显然，所 描述的实施例仅仅是本技术一部分的实施例，而不是全部的实施例。基于本申 请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所 有其它实施例，都应当属于本技术保护的范围。
[0069]
需要说明的是，本技术的说明书和权利要求书及上述附图中的术语“第一”、
ꢀ“
第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。 应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本技术的实 施例。此外，术语“包括”和“具有”及其任何变形，意图在于覆盖不排他的 包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必 限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些 过程、方法、产品或设备固有的其它步骤或单元。
[0070]
另外，术语“多个”的含义应为两个及两个以上。
[0071]
本技术提供的轨迹跟踪控制方法，可应用于如图1所示移动机器人上。其 中，移动机器人包括控制器104和移动机器人主体102，控制器104连接移动机 器人主体102，控制器104可用于获取移动机器人当前速度数据和当前位姿数据； 当前速度数据为基于预设的动力学模型处理得到；当前位姿数据为基于预设的 运动学模型处理得到；基于预先建立的运动学控制模块，对移动机器人参考速 度数据、参考位姿数据和当前位姿数据进行处理，得到位姿误差数据和辅助速 度控制输入，驱动移动机器人使其位姿误差数据收敛至第一阈值；基于预先建 立的动力学控制模块，利用线性扩张状态观测器观测系统集总扰动，设计自抗 扰控制器对系统集总扰动进行补偿，驱动移动机器人使其速度误差数据收敛至 第二阈值。
[0072]
为解决传统技术中对移动机器人等移动机器人运动控制方案大多建立在
ꢀ“
纯滚动无滑动”理想约束的基础上，然而在实际运行时，由于轮胎变形、路 面潮湿或结冰、快速转弯等因素的影响，容易出现轮子纵向打滑(纵滑)或侧 向打滑(侧滑)的情况，此时会破坏系统的非完整约束，使机器人的运动控制 设计变得更为复杂，导致移动机器人失控的问题，在一个实施例中，如图2所 示，提供了一种轨迹跟踪控制方法，以该方法应用于图1中的控制器104为例 进行说明，包括：
[0073]
步骤s210，获取移动机器人当前速度数据和当前位姿数据；当前速度数据 为基于预设的动力学模型处理得到；当前位姿数据为基于预设的运动学模型处 理得到。
[0074]
其中，当前速度数据可包括移动机器人当前线速度数据和当前角速度数据。 当前
位姿数据是指移动机器人的位置数据及姿态数据，例如移动机器人的位置 数据可以是移动机器人几何中心在全局坐标系中的坐标位置数据，移动机器人 的姿态数据可以是移动机器人的方向角。
[0075]
具体地，控制器预先设置相应的动力学模型和运动学模型，并基于动力学 模型处理得到当前速度数据，基于运动学模型处理得到当前位姿数据，进而控 制器可获取到移动机器人的当前速度数据和当前位姿数据。
[0076]
步骤s220，基于预先建立的运动学控制模块，对移动机器人参考位姿数据 和当前位姿数据进行处理，得到位姿误差数据，利用反步法(backstepping)结 合参考速度数据得到辅助速度控制输入，驱动移动机器人使其位姿误差数据收 敛至第一阈值。
[0077]
其中，运动学控制模块可根据系统预设得到，控制器可基于双闭环控制方 式，其中运动学控制模块为外环控制。参考速度数据可包括移动机器人的参考 线速度数据和参考角速度数据，可根据系统预设得到。参考位姿数据是指系统 预设的移动机器人的位置数据及姿态数据。位姿误差数据是指移动机器人的参 考位姿数据与当前位姿数据之间的差值。
[0078]
具体地，控制器可根据基于预先建立的运动学控制模块，将移动机器人的 参考速度数据、参考位姿数据和当前位姿数据输入运动学控制模块进行处理， 进而输出辅助速度数据，驱动移动机器人使其位姿误差数据收敛至第一阈值。 示例性，第一阈值可设置为0，使得移动机器人位姿跟踪误差趋于零。
[0079]
步骤s230，基于预先建立的动力学控制模块，对移动机器人的辅助速度数 据和当前速度数据进行处理，得到速度误差数据；结合线性扩张状态观测器对 集总扰动的估计值，利用自抗扰控制器对集总扰动进行补偿，使移动机器人速 度误差数据收敛至第二阈值。
[0080]
其中，动力学控制模块可根据系统预设得到，控制器可基于双闭环控制方 式，其中动力学控制模块为内环控制。速度误差数据指对应移动机器人的速度 数据的误差值。例如速度误差数据可以是移动机器人的辅助速度数据与实际速 度数据之间的差值。需要说明的是，速度误差数据可划分为移动机器人的线速 度误差数据和角速度误差数据。预设集总扰动可包括以下信息的任意一种或任 意组合：移动机器人未知纵滑和侧滑扰动信息、建模不确定及未知输入扰动信 息。
[0081]
具体地，控制器可根据预先建立的动力学控制模块，将移动机器人的辅助 速度数据和当前速度数据输入动力学控制模块进行处理，进而输出速度误差数 据；结合线性扩张状态观测器对集总扰动的估计值设计自抗扰控制器，实现移 动机器人的速度跟踪，使其速度误差数据收敛至第二阈值。示例性，第二阈值 可设置为0，使得移动机器人速度跟踪误差趋于零，移动机器人实际速度渐进收 敛到辅助速度，实现其轨迹跟踪控制。
[0082]
上述实施例中，通过获取移动机器人的当前速度数据和当前位姿数据；当 前速度数据为基于预设的动力学模型处理得到；当前位姿数据为基于预设的运 动学模型处理得到；基于预先建立的运动学控制模块，对移动机器人的参考速 度数据、参考位姿数据和当前位姿数据进行处理，得到辅助速度控制输入，驱 动移动机器人实现位姿跟踪，使其位姿误差数据收敛至第一阈值；基于预先建 立的动力学控制模块，利用线性扩张状态观测器对移动机器人集总扰动进行估 计，对辅助速度数据和当前速度数据进行处理，得到速度误差
数据，利用自抗 扰控制器得到驱动电机电压输入，驱动移动机器人直至所述速度误差数据收敛 至第二阈值，实现对移动机器人的精准轨迹跟踪，克服了在未知纵滑和侧滑扰 动、建模不确定性、未知输入扰动及执行器动力学影响下，导致移动机器人失 控的问题，增强了移动机器人轨迹跟踪控制的鲁棒性。
[0083]
在一个实施例中，如图3所示，基于预先建立的运动学控制模块，对移动 机器人参考位姿数据和当前位姿数据进行处理，得到位姿误差数据和辅助速度 数据的步骤包括：
[0084]
步骤s310，对移动机器人参考位姿数据和当前位姿数据进行处理，得到位 姿误差数据。
[0085]
具体地，控制器可将移动机器人参考位姿数据和当前位姿数据输入运动学 控制模块，通过运动学控制模块进行位姿误差处理，进而得到位姿误差数据。
[0086]
在一个示例中，定义移动机器人期望位姿为qr＝[x
r y
r θr]
t
∈r3×1，满足如下 运动学方程：
[0087]
上式中zr＝[vrωr]
t
，vr为移动机器人前进的参考线速度，ωr为移动机器人 本体绕几何中心的参考角速度。定义移动机器人位姿跟踪误差为：
[0088][0089]
求导可得位姿跟踪误差微分方程为：
[0090]
步骤s320，对移动机器人的参考速度数据和位姿跟踪误差数据进行处理， 得到辅助速度数据。
[0091]
其中，辅助速度控制输入可根据系统预设得到，例如，可基于反步法设计 得到辅助运动学控制模块。控制器可将移动机器人的参考速度数据输入辅助运 动学控制模块，通过辅助运动学控制模块进行处理，进而得到辅助速度数据。
[0092]
在一个示例中辅助运动学控制模块为通过以下函数得到：
[0093][0094]
其中，e
x
为移动机器人纵向误差，ey为移动机器人侧向误差，e
θ
为移动机 器人方向误差。
[0095]
示例性地，利用反步法设计移动机器人辅助运动学控制模块，可选取如下lyapunov函数：
[0096]
上式中k2为正常数，对v1求导，并将位姿跟踪误差微分方程代入，整理可得 (第一公式)：
[0097]
设计移动机器人辅助运动学控制输入zc为(第二公式)： [0098]
上式中vc、ωc分别为设计的移动机器人辅助线速度和角速度，k1、k2、k3为 辅助运动学控制律控制参数，且均为正常数。
[0099]
将第二公式代入第一公式，整理可得：
[0100]
由此可知在辅助运动学控制输入zc作用下，移动机器人位姿跟踪误差eq趋 于零。
[0101]
对于辅助运动学控制模块中的控制参数k1、k2、k3采用极点配置法进行在线 整定。设ξ为系统阻尼比，ωn为系统自然角频率，选择移动机器人受控系统的 期望极点为s1＝-2ξωn,其期望闭环特征多项式(第三 公式)可表示为：
[0102][0103]
上式中ξ∈(0,1)为系统阻尼比，ωn为系统自然角频率，均为正常数。
[0104]
将辅助运动学控制输入式(第二公式)代入系统位姿跟踪误差微分方程式 并在平衡点的邻域内用一阶taylor公式展开，可得：
[0105][0106]
系统矩阵aq的特征多项式(第四公式)为：
[0107][0108]
比较第四公式和第三公式，可得：
[0109]
k1+k3＝4ξωn,
[0110]
解得：k1＝2ξωn,k3＝2ξωn。
[0111]
当vr→
0时，k2→
∞，为避免该情况发生，选择为正 常数，则辅助运动学控制律中控制参数变为：k2＝γ。
[0112]
在一个实施例中，当前速度数据为基于预设的动力学模型处理得到的步骤 包括：
[0113]
获取驱动电机控制电压；
[0114]
根据电机模型，对驱动电机控制电压进行处理，得到驱动轮输入力矩；
[0115]
根据动力学模型，对驱动轮输入力矩和预设集总扰动进行处理，得到当前 速度数据。
[0116]
具体而言，驱动电机控制电压为驱动电机的工作电压。驱动电机用于驱动 移动机器人的轮子工作，移动机器人的轮子可包括左驱动轮和右驱动轮。电机 模型可根据系统预设得到。动力学控制器可根据反馈的实际速度数据及辅助速 度数据得到驱动电机控制电压，根据电机模型，将驱动电机控制电压输入电机 模型进行处理，进而得到驱动轮输入力矩；根据动力学模型，将驱动轮输入力 矩输入动力学模型进行处理，进而得到当前速度数据。
[0117]
在一个示例中，电机模型为通过以下函数得到：
[0118]
τ＝k1u
a-k2xz
[0119]
其中，τ＝[τ
r τ
l
]
t
，τr为移动机器人右轮输入力矩，τ
l
为移动机器人左轮输 入力矩；ua＝[u
ar u
al
]
t
，u
ar
为右轮驱动电机控制电压，u
al
为左轮驱动电机控制 电压；k1＝nk
t
/ra，k2＝n2k
t
kb/ra，ra为左、右轮驱动电机的电枢电阻，k
t
为 左、右轮驱动电机的转矩常数，kb为左、右轮驱动电机的反电动势常数，n为机 械齿轮减速比；r为移动机器人驱动轮半径，b为两驱动轮之间 的二分之一距离；z为移动机器人的实际速度。
[0120]
在一个示例中，动力学模型为通过以下函数得到：
[0121][0122]
其中，ua＝[u
ar u
al
]
t
，u
ar
为右轮驱动电机控制电压，u
al
为左轮驱动电机控 制电压；k1＝nk
t
/ra，k2＝n2k
t
kb/ra，ra为左、右轮驱动电机的电枢电阻，k
t
为左、右轮驱动电机的转矩常数，kb为左、右轮驱动电机的反电动势常数，n为 机械齿轮减速比；r为移动机器人驱动轮半径，b为两驱动轮之 间的二分之一距离；z为移动机器人的实际速度；ms＝s
t
ms， bs＝s
t
b，s为系统运动学模型中变换矩阵，m为系统动力学模型中正定对称 惯性矩阵，b为系统动力学模型中输入力矩变换矩阵。
[0123]
在一个实施例中，运动学模型为通过以下函数得到：
[0124][0125]
其中，z为移动机器人的实际速度；η＝[ηvη
ω
]
t
，ηv＝r(ξr+ξ
l
)/2为纵滑速 度，η
ω
＝r(ξ
r-ξ
l
)/(2b)为纵滑引起的横摆率扰动，r为移动机器人驱动轮半径， 2b为两驱动轮之间的距离，ξ
l
、ξr分别为移动机器人左、右驱动轮纵滑引起的干 扰角速度矢量；为移动机器人发生侧滑时引起的非 匹配扰动向量，μ为移动机器人侧滑速度，θ为移动机器人 的方向角。
[0126]
示例性地，定义移动机器人位姿为q＝[x y θ]
t
∈r3×1，(x,y)表示移动机器人 几
何中心在全局坐标系中的位置，θ表示移动机器人的方向角。在存在纵滑和侧 滑扰动情况下，移动机器人运动学模型(第五公式)可表示为： [0127]
其中z＝[vω]
t
，v和ω分别为移动机器人的线速度和角速度；η＝[ηvη
ω
]
t
， ηv＝r(ξr+ξ
l
)/2为纵滑速度，η
ω
＝r(ξ
r-ξ
l
)/(2b)为纵滑引起的横摆率扰动，r为 移动机器人驱动轮半径，2b为两驱动轮之间的距离，ξ
l
、ξr分别为移动机器人左、 右驱动轮纵滑引起的干扰角速度矢量；为移动机器 人发生侧滑时引起的非匹配扰动向量，μ为移动机器人侧滑速度；s(q)具有如 下形式：
[0128][0129]
在纯滚动无滑动理想情况下，移动机器人的动力学模型(第六公式)可描 述为：
[0130]
其中为系统正定对称惯性矩阵，m、j分别为移动 机器人的质量和转动惯量；为系统离心力和哥氏力矩阵，由于移动 机器人质心与几何中心重合，该项为全零矩阵；g(q)∈r3×1为系统的重力向量， 对于在平面运动的移动机器人该项为零向量；为系 统输入力矩变换矩阵；τ＝[τ
r τ
l
]
t
∈r2×1为右、左轮输入力矩； a(q)＝[-sinθ cosθ0]∈r1×3为系统非完整约束矩阵，且s
t
(q)a
t
(q)＝0∈r2×1； 为系统约束力项。
[0131]
第五公式与第六公式联立，可得(第七公式)：
[0132][0133]
第七公式中ms＝s
t
ms∈r2×2，f1＝s
t
m∈r2×3， f2＝s
t
v∈r2×3，bs＝s
t
b∈r2×2。
[0134]
考虑动力学模型参数不确定和未知动力学输入干扰，第七公式可重写为：
[0135][0136]
其中δms、δvs分别为ms、vs的变化量，τd∈r2×1为未知有界可导输入扰动。
[0137]
定义则第七公 式可进一步表示为：
[0138]
整理可得：式中为包含系统未知纵滑和侧滑扰动、 建
实际速度，d为对应预设集总扰动，ua＝[u
ar u
al
]
t
，u
ar
为右轮驱动电机控制电压， u
al
为左轮驱动电机控制电压。
[0153]
示例性地，自抗扰控制模块可分为集总扰动补偿部分和虚拟速度跟踪部分。 对集总扰动补偿部分设计如下：对虚拟速度跟踪部分设计如 下：
[0154]
其中为控制模块增益，ωc＞0为控制模块带宽。由此可得自抗 扰控制模块为：
[0155]
在一个实施例中，如图4所示，预先建立的动力学控制模块还包括线性扩 张状态观测器；根据系统反馈实际速度信息，得到系统状态及集总扰动估计值 的步骤包括：
[0156]
步骤s410，基于线性扩张状态观测器，对系统状态信息及集总扰动进行估 计。
[0157]
步骤s420，设计观测器参数，使线性扩张状态观测器的估计值趋于系统实 际值。
[0158]
具体而言，控制器可基于线性扩张状态观测器，精确估计纵滑和侧滑、动 力学不确定性、未知外部扰动及驱动电机动力学对机器人的干扰。
[0159]
在一个示例中，线性扩张状态观测器为通过以下函数得到：
[0160][0161]
其中，分别为状态向量x1、x2的估计值，x1＝z,x2＝d，z为移动机器 人的实际速度，d为对应预设集总扰动，β1、β2分别为线性扩张状态观测器的第 一增益、第二增益；k1＝nk
t
/ra，k2＝n2k
t
kb/ra，ra为左、右轮驱动电机的 电枢电阻，k
t
为左、右轮驱动电机的转矩常数，kb为左、右轮驱动电机的反电动 势常数，n为机械齿轮减速比；r为移动机器人驱动轮半径，b 为两驱动轮之间的二分之一距离；ms＝s
t
ms，bs＝s
t
b，s为 系统运动学模型中变换矩阵，m为系统动力学模型的正定对称惯性矩阵，b为 系统动力学模型的输入力矩变换矩阵；ua＝[u
ar u
al
]
t
，u
ar
为右轮驱动电机控制电 压，u
al
为左轮驱动电机控制电压。
[0162]
示例性地，线性扩张状态观测器设计过程为：定义状态向量x1＝z,x2＝d， 则动力学模型可描述为状态空间形式(第八公式)：
[0163]
针对第八公式设计线性扩张状态观测器：
[0164]
[0165]
其中分别为状态向量x1、x2的估计值，β1、β2为扩张状态观测器的增 益，取值为：
[0166]
其中，ωo＞0为观测器带宽。定义扩张状态观测器观测误差 通过设计合适的观测器带宽，可使扩张状态观测器观测 误差收敛到有界范围。
[0167]
在一个示例中，针对考虑电机动力学的移动机器人在未知纵滑和侧滑扰动、 建模不确定及未知输入扰动下的轨迹跟踪控制，采用双闭环控制策略，控制结 构如图5所示。外环为运动学控制模块，以移动机器人参考位姿与实际位姿的 误差作为输入，利用反步(backstepping)方法设计辅助运动学控制模块，产 生辅助线速度和角速度，保证位姿误差渐进收敛到零。内环为动力学控制模块， 以辅助速度与实际速度的误差作为自抗扰控制模块的输入并利用线性扩张状态 观测器对动力学集总扰动进行实时估计和补偿，使移动机器人实际运动速度渐 进收敛到辅助运动速度，实现其轨迹跟踪控制。
[0168]
应该理解的是，虽然图2～图4的流程图中的各个步骤按照箭头的指示依次 显示，但是这些步骤并不是必然按照箭头指示的顺序依次执行。除非本文中有 明确说明，这些步骤的执行并没有严格的顺序限制，可以其它的顺序执行。而 且，图2～图4中的至少一部分步骤可以包括多个子步骤或多个阶段，这些子步 骤或阶段并不必然是在同一时刻执行完成，而是可在不同时刻执行，这些子步 骤或阶段的执行顺序也不必然是依次进行，而是可与其它步骤或其它步骤的子 步骤或阶段的至少一部分轮流或交替地执行。
[0169]
移动机器人例如为扫地机器人、拖地机器人和服务机器人等具备自行走功 能的机器人。本实施例以扫地机器人为例进行说明，验证所设计控制算法的有 效性，进行了扫地机器人在未知纵滑和侧滑扰动、未知动力学输入干扰及模型 参数不确定情况下的轨迹跟踪控制仿真试验。由于拖地机器人和服务机器人与 扫地机器人的移动方式具有一定相通性，所以在拖地机器人或服务机器人等移 动机器人中也同样适用本方法。
[0170]
设定物理参数和驱动电机的相关参数如下：
[0171]
m＝2.7kg,j＝0.033kg
·
m2,r＝0.034m,b＝0.11m,
[0172]
ra＝13.5ω,kb＝0.0239v/rad
·
s-1
,k
t
＝0.0239(n
·
m)/a,n＝54.5。
[0173]
扫地机器人参考轨迹初始值设为qr(0)＝[0 0 0]
t
，初始位姿设为 q(0)＝[0 0 0]
t
，运动学控制模块参数设为γ＝50,ξ＝0.707，线性扩张状态观测 器参数设为ωo＝100，自抗扰控制模块参数设为ωc＝20。
[0174]
为了对所提出控制模块的鲁棒性做进一步研究，假设扫地机器人的未知动 力学输入扰动服从正态分布函数：
[0175]
系统不确定部分设为δms＝0.15ms。为了测试所设计控制模块对包含不同频 率及幅值信息扰动的控制效果，本试验以衰减正弦形式给出纵滑和侧滑扰动， 其中左、右驱动轮纵滑角速度扰动：
[0176][0177]
侧滑线速度扰动：其中ts为扰动起始时刻。
[0178]
为验证所设计控制算法的鲁棒性，将其分别与传统双闭环 pid(double-closed-loop pid,dclpid)、外环反步法设计运动学控制模块和内 环pid控制模块相结合的算法(backstepping control pid,bcpid)进行仿 真对比。dclpid控制模块参数为：
[0179][0180]
bcpid动力学控制模块参数为： k
p
＝diag([10,10]),ki＝diag([1,1]),kd＝diag([8,8])，运动学控制模块参数与本专 利提出的bcadrc运动学控制模块参数相同。
[0181]
在实际工况中，扫地机器人的运行轨迹可分为三类：直行、圆弧和原地旋 转。下面分别就三种工况下不同控制模块的控制效果进行对比分析。
[0182]
对于直行运动轨迹，根据实际工况，扫地机器人直行可分为两种情况：无 障碍物和有障碍物。无障碍物时，扫地机器人以vr＝0.28m/s恒速运行，此时 ωr＝0rad/s。检测到障碍物后，线速度每秒对半递减，且线速度递减到0.02m/s 以内，即可认为扫地机器人停止运动。经计算，检测到障碍物后，扫地机器人 在4s内停止运动，减速过程总位移为0.525m。
[0183]
仿真设置如下：仿真时间30s，采样频率100hz，扫地机器人在0～20s以 vr＝0.28m/s恒速运行，其中5～8s发生纵滑，10～13s发生侧滑，15～18s加 入集总扰动，即同时发生纵滑和侧滑，并考虑建模不确定和未知输入扰动。20s 时扫地机器人检测到障碍物，速度递减，在速度递减过程中(21～23s)加入集 总扰动。仿真结果如图6～11所示。
[0184]
具体而言，如图6中，展示了集总扰动下扫地机器人在辅助运动学控制模 块和自抗扰控制模块的作用下完成的轨迹跟踪任务效果图，可以看出纵滑、侧 滑及集总扰动下，扫地机器人仍能稳定地跟踪直线参考轨迹，且自抗扰控制模 块相较其它两种控制模块跟踪效果更好，跟踪误差更小。图7和图8为扫地机 器人各方向跟踪效果和位姿误差图，在无扰动时三种控制模块均可精确跟踪参 考轨迹。在发生纵滑和侧滑时，对x方向的位姿跟踪误差影响较小，主要影响y 方向和方向角的位姿跟踪。加入集总扰动后会对机器人x方向跟踪误差造成一定 影响，但总体影响较小，且扰动消失后机器人可迅速调整并收敛到期望轨迹。 图9中为扫地机器人实际速度，自抗扰控制模块在扰动下的速度波动较小，更 稳定。图10为线性扩张状态观测器对集总扰动的观测效果，分析可知线性扩张 状态观测器对不同频率和幅值的集总扰动均有较好的观测效果，收敛误差小， 观测精度较高。图11为控制电压随时间的变化曲线，当存在扰动时，控制模块 进行实时调整以抑制扰动对系统的影响。
[0185]
对于圆弧运动轨迹，假定扫地机器人参考轨迹为圆形轨迹，参考速度设为 vr＝0.28m/s,ωr＝1.22rad/s，机器人初始速度设为v＝0m/s,ω＝0rad/s，其 余参数同直行运动轨迹。扫地机器人在t＝5s时，负载发生了缓慢变化，变化量 δms＝0.15ms，10～13s机器人发生纵滑，15～18s机器人发生侧滑，在20～23s 加入集总扰动，仿真结果如图12～17所示。
[0186]
由图12～14可知，在集总扰动下，扫地机器人在三种控制模块的作用下均 能精确跟踪参考轨迹，但dclpid会有一定稳态误差，不能精确实现位姿跟踪， 而bcpid的初始误差较大，且在集总扰动下有较大的跟踪误差。基于bcadrc的 控制算法能实现精确轨迹跟踪，跟踪误差较小，有更好的鲁棒性。图15为三种 控制算法下扫地机器人的实际速度和角速度变化情况，自抗扰控制模块快速克 服了扰动干扰，且超调量小、振荡小，使得线速度和角速度快速收敛到辅助线 速度和角速度，控制性能远优于传统pid控制。图16为线性扩张状态观测器对 集总扰动的估计，误差较小，可实现对集总扰动实时、精确估计。图17给出了 控制电压随时间变化曲线。
[0187]
对于原地旋转运动轨迹，在实际工况中会存在扫地机器人原地旋转的情况， 针对该情况进行了仿真验证。仿真中扫地机器人参考速度设为 vr＝0m/s,ωr＝1.r2ad2/s，其余参数参照上述实施例。在t＝5s时，扫地机器 人负载发生缓慢变化，即δms＝0.15ms，10～13s机器人发生纵滑，15～18s机 器人发生侧滑，在20～23s加入集总扰动，仿真结果如图18～22所示。
[0188]
由图18和图19可知，集总扰动下，扫地机器人在三种控制模块作用下均 能实现精确轨迹跟踪，相较其它两种算法，自抗扰控制模块位姿跟踪误差更小， 且无稳态误差；图20为三种控制算法下，扫地机器人实际速度变化情况，可以 看出自抗扰控制超调小、震荡小。图21表明线性扩张状态观测器可实时准确地 对集总扰动进行估计。由图22可知左、右轮输入电压大小相等方向相反，对应 扫地机器人原地旋转，在存在扰动时对控制电压进行调整以抑制扰动影响。
[0189]
上述实施例中，考虑扫地机器人在运动过程中受未知纵滑和侧滑干扰、模 型不确定、未知输入扰动及驱动电机动态特性影响的问题，依据双闭环控制策 略设计辅助运动学控制模块和自抗扰控制模块，通过线性扩张状态观测器实时 估计和补偿扰动，使得机器人实现精确的轨迹跟踪控制。由仿真可知，自抗扰 控制可实时估计和补偿扰动，且超调小、振荡小，具有更好的鲁棒性。
[0190]
在一个实施例中，如图23所示，还提供了一种轨迹跟踪控制装置，包括：
[0191]
数据获取单元210，用于获取移动机器人当前速度数据和当前位姿数据；当 前速度数据为基于预设的动力学模型处理得到；当前位姿数据为基于预设的运 动学模型处理得到。
[0192]
位姿处理单元220，用于基于预先建立的运动学控制模块，对所述移动机器 人的参考位姿数据和所述当前位姿数据进行处理，得到位姿误差数据，结合参 考速度数据利用反步法得到辅助速度控制输入，驱动移动机器人使其位姿误差 数据收敛至第一阈值。
[0193]
速度处理单元230，用于基于预先建立的动力学控制模块，利用线性扩张状 态观测器对移动机器人集总扰动进行估计，对所述移动机器人的所述辅助速度 数据和所述当前速度数据进行处理，得到速度误差数据，利用自抗扰控制器得 到驱动电机电压输入，驱动移动机器人直至所述速度误差数据收敛至第二阈值。
[0194]
关于轨迹跟踪控制装置的具体限定可参见上文中对轨迹跟踪控制方法的限 定，在此不再赘述。上述轨迹跟踪控制装置中各个模块可全部或部分通过软件、 硬件及其组合来实现。上述各模块可以硬件形式内嵌于或独立于移动机器人中 的控制器中，也可以软件形式存储于移动机器人中的存储器中，以便控制器调 用执行以上各个模块对应的操作。
[0195]
在一个实施例中，还提供了一种移动机器人，包括移动机器人主体及设置 在移动机器人上的控制器；控制器用于执行上述任意一项的轨迹跟踪控制方法。
[0196]
其中，控制器用于执行以下的轨迹跟踪控制方法的步骤：
[0197]
获取移动机器人当前速度数据和当前位姿数据；当前速度数据为基于预设 的动力学模型处理得到；当前位姿数据为基于预设的运动学模型处理得到。基 于预先建立的运动学控制模块，对所述移动机器人的参考位姿数据和所述当前 位姿数据进行处理，得到位姿误差数据；结合参考速度数据利用反步法得到辅 助速度控制输入，驱动移动机器人使其位姿误差数据收敛至第一阈值。基于预 先建立的动力学控制模块，利用线性扩张状态观测器对移动机器人集总扰动进 行估计；对所述移动机器人的所述辅助速度数据和所述当前速度数据进行处理， 得到速度误差数据；利用自抗扰控制器得到驱动电机电压输入，驱动移动机器 人直至所述速度误差数据收敛至第二阈值。
[0198]
上述实施例中，通过获取移动机器人当前速度数据和当前位姿数据；当前 速度数据为基于预设的动力学模型处理得到；当前位姿数据为基于预设的运动 学模型处理得到；基于预先建立的运动学控制模块，对移动机器人的参考位姿 数据和当前位姿数据进行处理，得到位姿误差数据，结合参考速度数据利用反 步法得到辅助速度控制输入，驱动移动机器人使其位姿误差数据收敛至第一阈 值；基于预先建立的动力学控制模块，利用线性扩张状态观测器对移动机器人 集总扰动进行估计，对移动机器人的辅助速度数据和当前速度数据进行处理， 得到速度误差数据，利用自抗扰控制器得到驱动电机电压输入，驱动移动机器 人直至速度误差数据收敛至第二阈值，实现对移动机器人的精准轨迹跟踪，克 服了在未知纵滑和侧滑扰动、建模不确定性、未知输入扰动及执行器动力学影 响下，导致移动机器人失控的问题，增强了移动机器人轨迹跟踪控制的鲁棒性。
[0199]
在一个实施例中，还提供了一种计算机可读存储介质，其上存储有计算机 程序，计算机程序被处理器执行时实现上述任一项的轨迹跟踪控制方法的步骤。
[0200]
在一个示例中，计算机程序被处理器执行时实现以下步骤：
[0201]
获取移动机器人的当前速度数据和当前位姿数据；当前速度数据为基于预 设的动力学模型处理得到；当前位姿数据为基于预设的运动学模型处理得到。 基于预先建立的运动学控制模块，对移动机器人的参考位姿数据和当前位姿数 据进行处理，得到位姿误差数据，结合参考速度数据利用反步法得到辅助速度 控制输入，驱动移动机器人使其位姿误差数据收敛至第一阈值。基于预先建立 的动力学控制模块，利用线性扩张状态观测器对移动机器人集总扰动进行估计， 对移动机器人的辅助速度数据和当前速度数据进行处理，得到速度误差数据， 利用自抗扰控制器得到驱动电机电压输入，驱动移动机器人直至速度误差数据 收敛至第二阈值。
[0202]
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程， 是可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于 一非易失性计算机可读取存储介质中，该计算机程序在执行时，可包括如上述 各方法的实施例的流程。其中，本技术所提供的各实施例中所使用的对存储器、 存储、数据库或其它介质的任何引用，均可包括非易失性和/或易失性存储器。 非易失性存储器可包括只读存储器(rom)、可编程rom(prom)、电可编 程rom(eprom)、电可擦除可编程rom(eeprom)或闪存。易失性存储 器可包括随机存取存储器(ram)或外部高速缓冲存储器。作为说明而非局限， ram以多种形
式可得，诸如静态ram(sram)、动态ram(dram)、同 步dram(sdram)、双数据率sdram(ddrsdram)、增强型sdram (esdram)、同步链路(synchlink)dram(sldram)、存储器总线(rambus) 直接ram(rdram)、直接存储器总线动态ram(drdram)、以及存储 器总线动态ram(rdram)等。
[0203]
以上所述实施例的各技术特征可以进行任意组合，为使描述简洁，未对上 述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术 特征的组合不存在矛盾，都应当认为是本说明书记载的范围。
[0204]
以上所述实施例仅表达了本技术的几种实施方式，其描述较为具体和详细， 但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的 普通技术人员而言，在不脱离本技术构思的前提下，还可做出若干变形和改进， 这些都属于本技术的保护范围。因此，本技术专利的保护范围应以所附权利要 求为准。