一种基于轮式移动机器人轨迹跟踪的混合控制方法

摘要

本发明公开一种基于轮式移动机器人轨迹跟踪的混合控制方法，由运动学的虚拟速度控制器，基于动力学的滑模力矩控制器和扰动观测器三个部分。虚拟速度控制器包括机器人线速度和角速度的设计；滑模控制器包括滑模面的设计和滑模控制律的设计；扰动观测器用来观测系统的外部扰动，来降低滑模控制器的控制量，作为前馈项引入。本发明所涉及的混合控制方法能够使得系统在参数有界变化和外界扰动的情况下实现对机器人轨迹跟踪控制。仿真实验表明该发明的混合控制方法能有效地减小滑模控制输出的抖振，有效降低控制量的输出，并且具有良好的鲁棒性。

说明书

技术领域

本发明属于移动机器人的轨迹跟踪控制领域，尤其涉及到一种基于轮式移动 机器人轨迹跟踪的混合控制方法。

背景技术

轮式移动机器人是一种将环境感知、动态决策与规划、行为控制与执行等多 种功能综合于一体的移动平台，具备高度自规划、自组织和自适应能力，可在无 人干预和复杂环境下有目的地自主运动，并完成特定的作业功能。由于轮式移动 机器人在物料自动搬运、特殊人群服务、抢险救灾、未知和危险地域探索等方面 应用具有不可比拟的优势，已广泛地应用于工农业、服务业、国防、宇宙探索等 领域，对人类社会的生产和生活产生了积极而深远的影响。

非完整轮式移动机器人(wheeled mobile robot，WMR)是一种典型的多输入多 输出耦合欠驱动非线性系统，其运动控制问题极具挑战性。一方面，应当考虑实 际系统一些被忽略的固有非线性特性，如摩擦、间隙、执行器饱和等；另一方面， 系统还会受到外界扰动以及参数不确定性的影响，这些因素造成实际系统与理想 数学模型出现较大的偏差。基于理想数学模型所设计的控制律往往难以达到所需 的控制指标，甚至会引起系统不稳定。需设法来消除系统不确定性的不利影响， 这给运动控制带来了更大的挑战。因此，解决复杂情况下非完整轮式移动机器人 的运动控制问题具有重要的理论意义和实际应用价值。

滑模控制作为一种变结构控制方法，当系统运动状态到达滑模面上时，对系 统参数的不确定性以及外界干扰有着很强的鲁棒性。(张鑫,刘凤娟,闫茂德.基于 动力学模型的轮式移动机器人自适应滑模轨迹跟踪控制[J].机械科学与技 术.2012(01))采用自适应滑模控制算法，实现机器人轨迹跟踪。但对于较大扰 动滑模控制会引起系统的抖振，本发明结合扰动观测器，以降低外部扰动带来的 抖振问题。

发明内容

发明目的：本发明所要解决的技术问题是给出一种能够在轮式移动机器人控 制系统存在参数扰动和外界干扰的情况下进行轨迹跟踪控制。

技术方案：本发明采用如下技术方案解决上述技术问题：设计了一种基于轮 式移动机器人轨迹跟踪的混合控制方法，包括如下具体步骤：

步骤(1)：对轨迹模型和机器人执行机构进行分析，建立具有非完整性约束 的移动机器人运动学模型和动力学模型；

步骤(2)：利用单目摄像头获取轨迹，结合步骤(1)所推导的运动学模型，确 定机器人要实现轨迹跟踪虚拟线速度和角速度控制器v_c,w_c；

步骤(3)：利用光电编码器获取机器人两轮的角速度根据转换公式计 算实际的机器人线速度和角速度v,w；计算步骤(2)中获得的虚拟速度和角速度与 实际速度和角速度的偏差

步骤(4)：根据步骤(3)中的选择合适的滑模面S；

步骤(5)：根据步骤(4)中的滑模面S确定移动机器人左右轮驱动电机的力矩 控制量(τ₁,τ₂)^T；

步骤(6)：根据步骤(5)的(τ₁,τ₂)^T和当前的机器人实际速度v,w设计干扰观测 器并进行相应的前馈补偿，减小系统抖振。

作为本发明的第一个改进，步骤(4)中所选择的滑膜面为：

$\left(\begin{array}{c}{S}_1=\tilde{v}+c_1{\int }_0^t\tilde{v}\mathrm{dt}\\ S_2=\tilde{w}+c_2{\int }_0^t\tilde{w}\mathrm{dt}\end{array}\right)$

式中，S₁,S₂为滑模面，c₁,c₂＞0为滑模系数，t定义为实际变量。

根据上述的滑模面，步骤(5)中所选择的左右轮的力矩控制量为：

$\left(\begin{array}{c}{\tau }_1=\frac{1}{2}[-{\mathrm{mrc}}_1\tilde{v}+k_3\mathrm{sgn}\left(S_1\right)-\frac{c_2\mathrm{rI}}{b}\tilde{w}+k_4\mathrm{sgn}\left(S_2\right)+r({\stackrel{\widehat{‾}}{F}}_1+{\stackrel{\widehat{‾}}{F}}_2)]\\ {\tau }_2=\frac{1}{2}[-{\mathrm{mrc}}_1\tilde{v}+k_3\mathrm{sgn}\left(S_1\right)+\frac{c_2\mathrm{rI}}{b}\tilde{w}-k_4\mathrm{sgn}\left(S_2\right)+r({\stackrel{\widehat{‾}}{F}}_1+{\stackrel{\widehat{‾}}{F}}_2)]\end{array}\right)$

其中，τ₁,τ₂为左右轮的控制力矩；m是机器人质量，I机器人转动惯量，k₃,k₄是 可设定的增益量，sgn(S_i)是关于滑模面S_i的符号函数，分别是左右轮的外 加扰动的观测器估计值。

本发明的第二个改进，由于外界扰动很难确定上界，当扰动量很大时滑模控 制容易带来抖振，因而结合干扰观测器来解决外部扰动的影响，以减小系统的抖 振，且易于实际工程的实现。步骤(6)中所选择的针对通用模型的 干扰观测器为：

$\left(\begin{array}{c}\stackrel{.}{z}=[E-{\lg }_{2}H]z+\mathrm{Ep}\left(\alpha \right)-[g_2\mathrm{Hp}\left(\alpha \right)+f+g_{1}u]\\ \hat{\xi }=z+p\left(\alpha \right)\\ \hat{\delta }=H\hat{\xi }\end{array}\right)$

其中，外加扰动源满足$\left(\begin{array}{c}\stackrel{.}{\xi }=\mathrm{E\xi }\\ \delta =\mathrm{H\xi }\end{array};\right)$其中E是一个频率矩阵是扰动 频率；p(α)是需要设计的非线性函数，是非线性干扰观测器的增益。 通过调节l可以决定观测器的收敛速度。

有益效果：本发明由运动学的虚拟速度控制器，基于动力学的滑模力矩控制 器和扰动观测器三个部分。虚拟速度控制器包括机器人线速度和角速度的设计， 目的是保证机器人的运动轨迹能跟踪期望的轨迹；滑模控制器包括滑模面的设计 和滑模控制律的设计，目的是保证力矩控制器能够提供与虚拟速度同样大小和方 向的实际速度，从而保证闭环系统的稳定性。扰动观测器用来观测系统的外部扰 动，来降低滑模控制器的控制量，作为前馈项引入。本发明所涉及的混合控制方 法能够使得系统在参数有界变化和外界扰动的情况下实现对机器人轨迹跟踪控 制。仿真实验表明该发明的混合控制方法能有效地减小滑模控制输出的抖振，有 效降低控制量的输出，并且具有良好的鲁棒性。

附图说明

图1是本发明中两轮驱动移动机器人模型示意图；

图2是本发明中轮式移动机器人控制的原理简图；

图3是本发明中轮式移动机器人控制的原理实现框图；

图4是本发明在没有干扰观测器(DOB)的情况下的左轮力矩控制量；

图5是本发明在有干扰观测器(DOB)的情况下的左轮力矩控制量；

图6是本发明在没有干扰观测器(DOB)的情况下的右轮力矩控制量；

图7是本发明在有干扰观测器(DOB)的情况下的右轮力矩控制量；

图8是本发明中干扰观测器(DOB)对外界扰动的观测；

图9是本发明中轮式移动机器人跟踪圆轨迹的轨迹曲线图；

图10是本发明中轮式移动机器人跟踪圆轨迹时的误差曲线图；

图11是本发明中轮式移动机器人跟踪直线轨迹的轨迹曲线图；

图12是本发明中轮式移动机器人跟踪直线轨迹时的误差曲线图。

具体实施方式

下面结合附图对本发明作进一步说明：

如图1-图12所示，本发明设计了一种基于轮式移动机器人轨迹跟踪的混合 控制方法，包括如下具体步骤：

步骤(1)：对轨迹模型和机器人执行机构进行分析，建立具有非完整性约束 的移动机器人运动学模型和动力学模型；

步骤(2)：利用单目摄像头获取轨迹，结合步骤(1)所推导的运动学模型，确 定机器人要实现轨迹跟踪虚拟线速度和角速度控制器v_c,w_c；

步骤(3)：利用光电编码器获取机器人两轮的角速度根据

转换公式计算实际的机器人线速度和角速度v,w；计算步骤(2)中获得的虚拟速度 和角速度与实际速度和角速度的偏差

步骤(4)：根据步骤(3)中的选择合适的滑模面S；

步骤(5)：根据步骤(4)中的滑模面S确定移动机器人左右轮驱动电机的力矩 控制量(τ₁,τ₂)^T；

步骤(6)：根据步骤(5)的(τ₁,τ₂)^T和当前的机器人实际速度v,w设计干扰观测 器并进行相应的前馈补偿，减小系统抖振。

作为本发明的第一个改进，步骤(4)中所选择的滑膜面为：

$\left(\begin{array}{c}{S}_1=\tilde{v}+c_1{\int }_0^t\tilde{v}\mathrm{dt}\\ S_2=\tilde{w}+c_2{\int }_0^t\tilde{w}\mathrm{dt}\end{array}\right)$

式中，S₁,S₂为滑模面，c₁,c₂＞0为滑模系数，t定义为实际变量。

根据上述的滑模面，步骤(5)中所选择的左右轮的力矩控制量为：

$\left(\begin{array}{c}{\tau }_1=\frac{1}{2}[-{\mathrm{mrc}}_1\tilde{v}+k_3\mathrm{sgn}\left(S_1\right)-\frac{c_2\mathrm{rI}}{b}\tilde{w}+k_4\mathrm{sgn}\left(S_2\right)+r({\stackrel{\widehat{‾}}{F}}_1+{\stackrel{\widehat{‾}}{F}}_2)]\\ {\tau }_2=\frac{1}{2}[-{\mathrm{mrc}}_1\tilde{v}+k_3\mathrm{sgn}\left(S_1\right)+\frac{c_2\mathrm{rI}}{b}\tilde{w}-k_4\mathrm{sgn}\left(S_2\right)+r({\stackrel{\widehat{‾}}{F}}_1+{\stackrel{\widehat{‾}}{F}}_2)]\end{array}\right)$

其中，τ₁,τ₂为左右轮的控制力矩。m是机器人质量，I机器人转动惯量，k₃,k₄是 可设定的增益量，sgn(S_i)是关于滑模面S_i的符号函数，分别是左右轮的外 加扰动的观测器估计值。

本发明的第二个改进，由于外界扰动很难确定上界，当扰动量很大时滑模控 制容易带来抖振，因而结合干扰观测器来解决外部扰动的影响，以减小系统的抖 振，且易于实际工程的实现。步骤(6)中所选择的针对通用模型的 干扰观测器为：

$\left(\begin{array}{c}\stackrel{.}{z}=[E-{\lg }_{2}H]z+\mathrm{Ep}\left(\alpha \right)-[g_2\mathrm{Hp}\left(\alpha \right)+f+g_{1}u]\\ \hat{\xi }=z+p\left(\alpha \right)\\ \hat{\delta }=H\hat{\xi }\end{array}\right)$

其中，外加扰动源满足$\left(\begin{array}{c}\stackrel{·}{\xi }=\mathrm{E\xi }\\ \delta =\mathrm{H\xi }\end{array}\right).$其中E是一个频率矩阵是扰动 频率。p(α)是需要设计的非线性函数，是非线性干扰观测器的增益。 通过调节l可以决定观测器的收敛速度。

在具体实例中，我们设计的运动学虚拟线速度和角速度控制器，滑模力矩控 制器和干扰观测器的步骤如下：

步骤1：建立轮式移动机器人的运动学模型：$\left(\begin{array}{c}\stackrel{.}{x}\\ \stackrel{.}{y}\\ \stackrel{.}{\theta }\end{array}\right)=\left(\begin{array}{cc}\cos \theta & 0\\ \sin \theta & 0\\ 0& 1\end{array}\right)\left(\begin{array}{c}v\\ w\end{array}\right),$

约束条件是：其中q＝[x,y,θ]^T是系统的状态，即移动 机器人位姿，(x,y)为质心在世界坐标系中的坐标(单位：m)，θ为机器人姿态角 (单位：rad)，v,w分别是机器人的线速度和角速度。期望轨迹模型 $\left(\begin{array}{c}{\stackrel{.}{x}}_r\\ {\stackrel{.}{y}}_r\\ {\stackrel{.}{\theta }}_r\end{array}\right)=\left(\begin{array}{cc}\cos {\theta }_r& 0\\ \sin {\theta }_r& 0\\ 0& 1\end{array}\right)\left(\begin{array}{c}{v}_r\\ w_r\end{array}\right),$(x_r,y_r,θ_r)是期望轨迹状态，(v_r,w_r)是期望线速度和角 速度。针对期望轨迹和运动学模型建立误差模型

$\left(\begin{array}{c}{x}_e\\ y_e\\ {\theta }_e\end{array}\right)=\left(\begin{array}{ccc}\cos \theta & \sin \theta & 0\\ -\sin \theta & \cos \theta & 0\\ 0& 0& 1\end{array}\right)\left(\begin{array}{c}{x}_r-x\\ y_r-y\\ {\theta }_r-\theta \end{array}\right).$

步骤2：设计虚拟速度控制器使得误差最终趋于零：

$\left(\begin{array}{c}{v}_c=v_r\cos {\theta }_e+k_1{x}_e\\ w_c=w_r+v_r{y}_e+k_2\sin {\theta }_e\end{array}\right)$

其中，k₁,k₂是正数。取李雅普诺夫方程：

$V_1=\frac{1}{2}(x_e^2+y_e^2)+(1-\cos {\theta }_e)$

将虚拟速度控制器代入可得

${\stackrel{.}{V}}_1=-k_1{x}_e^2-k_2{\sin }^2{\theta }_e\le 0$

步骤3：根据执行机构以及周边的环境建立动力学模型：

$M\left(q\right)\stackrel{..}{q}+C(\stackrel{.}{q},q)+G\left(q\right)+F\left(\stackrel{.}{q}\right)+{\tau }_d=B\left(q\right)\tau -A^T\left(q\right)\lambda$

对上式进行一定的变换，得

$\bar{M}\stackrel{·}{\eta }+\bar{L}\eta +\bar{F}=\bar{B}\tau$

其中，$\eta =\left(\begin{array}{c}v\\ w\end{array}\right),\bar{M}=\left(\begin{array}{cc}m& 0\\ 0& I\end{array}\right),\bar{L}=\left(\begin{array}{cc}0& \mathrm{md\theta }\\ -\mathrm{md\theta }& 0\end{array}\right),\bar{F}=\left(\begin{array}{c}{\bar{F}}_1\\ {\bar{F}}_2\end{array}\right),\bar{B}=\left(\begin{array}{cc}\frac{1}{r}& \frac{1}{r}\\ \frac{b}{r}& \frac{-b}{r}\end{array}\right).$

步骤4：针对线速度和角速度的误差设计滑模面

$\left(\begin{array}{c}{s}_1=\tilde{v}+c_1{\int }_0^t\tilde{v}\mathrm{dt}\\ s_2=\tilde{w}+c_2{\int }_0^t\tilde{w}\mathrm{dt}\end{array}\right)$

步骤5：设计左、右轮的力矩控制器来控制机器人的速度

$\left(\begin{array}{c}{\tau }_1=\frac{1}{2}[-{\mathrm{mrc}}_1\tilde{v}+k_3\mathrm{sgn}\left(S_1\right)-\frac{c_2\mathrm{rI}}{b}\tilde{w}+k_4\mathrm{sgn}\left(S_2\right)+r({\stackrel{\widehat{‾}}{F}}_1+{\stackrel{\widehat{‾}}{F}}_2)]\\ {\tau }_2=\frac{1}{2}[-{\mathrm{mrc}}_1\tilde{v}+k_3\mathrm{sgn}\left(S_1\right)+\frac{c_2\mathrm{rI}}{b}\tilde{w}-k_4\mathrm{sgn}\left(S_2\right)+r({\stackrel{\widehat{‾}}{F}}_1+{\stackrel{\widehat{‾}}{F}}_2)]\end{array}\right)$

其中，τ₁,τ₂分别是左右轮的驱动力矩。

令$f_v=(m{\stackrel{.}{v}}_c+\mathrm{md}{w}^2-\mathrm{mdw}\tilde{w})$和$f_w=(I{\stackrel{.}{w}}_c-\mathrm{mdvw}+\mathrm{mdw}\tilde{v}),$将其视为内部的扰 动，利用滑模来处理。取李雅普诺夫方程如下：

$V_2=\frac{1}{2}{s}_1^2$

求导可得：

${\stackrel{.}{V}}_2=s_1{\stackrel{.}{s}}_1=s_1\frac{k_3\left|s_1\right|-({\stackrel{\widehat{‾}}{F}}_1-{\bar{F}}_1+f_v){\mathrm{rs}}_1}{\mathrm{mr}}\le \frac{\left|s_1\right|(k_3+|{\stackrel{\widehat{‾}}{F}}_1-{\bar{F}}_1+f_v\left|r\right)}{\mathrm{mr}}$

只要满足$k_3<-|{\stackrel{\widehat{‾}}{F}}_1-{\bar{F}}_1+f_v|r$就有$s_1{\stackrel{.}{s}}_1<0.$

s₂采用上述同样的方法也可以得到相应的稳定性结果。

步骤6：针对通用模型的干扰观测器为：

$\left(\begin{array}{c}\stackrel{.}{z}=[E-{\lg }_{2}H]z+\mathrm{Ep}\left(\alpha \right)-[g_2\mathrm{Hp}\left(\alpha \right)+f+g_{1}u]\\ \hat{\xi }=z+p\left(\alpha \right)\\ \hat{\delta }=H\hat{\xi }\end{array}\right)$

其中，外加扰动源满足$\left(\begin{array}{c}\stackrel{.}{\xi }=\mathrm{E\xi }\\ \delta =\mathrm{H\xi }\end{array}\right).$E是一个频率矩阵是扰动频率。 是非线性干扰观测器的增益；p(α)是需要设计的非线性函 数，使得观测器以一定的速度收敛。

对于干扰误差的证明如下：

$\stackrel{\stackrel{.}{~}}{\xi }=\stackrel{\stackrel{.}{^}}{\xi }-\stackrel{.}{\xi }=\stackrel{.}{z}-\frac{\partial p\left(\alpha \right)}{\partial \alpha }\stackrel{.}{\alpha }-\mathrm{E\xi }=[E-{\lg }_{2}H]\tilde{\xi }$

只要能满足[E-lg₂H]的所有特征值均具有负实部，该观测器就可以收敛。

在本例中要针对线速度和角速度模型分别设计观测器。线速度模型与通用模 型对应参数是$f=0,g_1=1/\left(\mathrm{mr}\right),g_2=1/m,\delta ={\bar{F}}_1.$角速度模型与通用模型对应的 参数是$f=0,g_1=b/\left(\mathrm{Ir}\right),g_2=1/I,\delta ={\bar{F}}_1.$

综合以上稳定性的分析，本发明所设计的混合控制算法可以使得整个闭环系 统是稳定的，跟踪效果好，具有较强的鲁棒性。