CN105629729A - 一种基于线性自抗扰的网络化移动机器人轨迹跟踪控制方法 - Google Patents

Abstract

一种基于线性自抗扰的网络化移动机器人轨迹跟踪控制方法，首先，建立含有通信时延的移动机器人轨迹跟踪控制系统模型，并将时延引起的不确定动态描述为系统模型的不确定性。其次，设计扩张状态观测器，用于估计由网络时延引起的轨迹跟踪控制系统的不确定性。进而，设计带有扩张状态观测器的移动机器人轨迹跟踪线性自抗扰控制器，对时延引起的不确定动态进行动态线性化补偿，从而消除网络时延对系统性能的影响，实现网络时延的实时补偿和高精度的轨迹跟踪。

Description

一种基于线性自抗扰的网络化移动机器人轨迹跟踪控制方法

技术领域

本发明涉及移动机器人控制领域，尤其是一种基于线性自抗扰的网络化移动机器人轨迹跟踪控制方法。

背景技术

机器人领域的变化趋势由在生产线固定的机械手向能做更复杂任务的自主移动机器人过渡，比如救援、军事行动以及日常生活服务等。近几年，移动机器人被广泛应用于不同的工业过程，其控制方法设计被视为一个难点，因为移动机器人是一类典型非完整系统。移动机器人控制问题重点之一就是轨迹跟踪控制，然而，描述轨迹跟踪的跟踪误差系统是一个耦合非线性系统，不满足Brockett的必要条件从而复杂化了该控制问题。此外，在移动机器人轨迹跟踪领航追随系统中，用通讯网络连接传感器、控制器和执行器，完成位置和速度信息传送，而将通信网络引入控制回路也带来一些新的问题。由于信息传递的分时复用原则，并限于网络的带宽和承载能力，必然会造成信息的冲撞、重传等现象，从而导致了信息在控制回路的传输过程中产生网络诱导时延。这种网络诱导时延使得系统控制输入不能及时更新，从而导致系统性能下降甚至失稳。

移动机器人轨迹跟踪控制问题已有许多研究结果。为了延伸轨迹跟踪控制问题到笛卡尔空间，Samson和Ait-Abderrahim在1991年就提出全局跟踪控制律。随后一系列方法引入进来，滑模变结构方法有快速的响应、良好的瞬态以及对较大的参数变动及外部扰动的鲁棒性，从而被很多学者采纳作为轨迹跟踪控制策略，但不可避免的出现抖振现象，直接影响到控制效果。反演法是较早的基于运动学考虑的轨迹跟踪控制决策，被广泛应用于跟踪问题，但其控制结构和设计过程十分复杂，此外，要求移动机器人运动时尽可能提供较大的加速度，这在实际情况中是较难实现的。自适应控制可以适时地调节控制系统参数来更新控制律，有利于适应结构的不确定性但具有复杂的参数选择、鲁棒性较差等缺点。模糊控制方法具有一定的鲁棒性，但是模糊控制规则会受到人的主观因素的影响而不能完全归总，且因缺乏“自我学习”的能力很难消除稳态误差。神经网络控制方法固有的并行性和学习能力，使其在机器人运动控制系统中十分受欢迎，但学习速度慢、算法不完备仍是其主要缺点，此外还有预测控制、粒子群算法、遗传算法、H_∞算法以及其他智能控制(包括混合控制)方法。

发明内容

为了克服已有移动机器人轨迹跟踪控制方法的各自不适性、灵活性差、计算量大、设计过程复杂且均没有对轨迹跟踪时存在的网络诱导延时现象加以考虑的不足，本发明提供一种线性自抗扰的方法来处理移动机器人轨迹跟踪问题，降低了算法复杂度且对控制系统中的通信延时予以补偿，它具有算法简单、控制精度高、抗干扰能力强等优点。

本发明解决其技术问题所采用的技术方案是：

一种基于线性自抗扰的网络化移动机器人轨迹跟踪控制方法，所述方法包括以下步骤：

步骤1)建立移动机器人的运动学模型：

$\left[\begin{array}{c}\stackrel{\·}{x}\left(t\right)\\ \stackrel{\·}{y}\left(t\right)\\ \stackrel{\·}{\mathrm{\θ}}\left(t\right)\end{array}\right]=\left[\begin{array}{cc}\cos \mathrm{\θ}\left(t\right)& -d\sin \mathrm{\θ}\left(t\right)\\ \sin \mathrm{\θ}\left(t\right)& d\cos \mathrm{\θ}\left(t\right)\\ 0& 1\end{array}\right]\left[\begin{array}{c}v\left(t\right)\\ w\left(t\right)\end{array}\right]---\left(1\right)$

其中，x(t)、y(t)表示移动机器人质心在全局坐标系中的坐标，θ(t)表示移动机器人的航向角，v(t)表示移动机器人移动线速度，w(t)表示移动机器人移动角速度，d表示移动机器人质心到驱动轮轴心的距离；

步骤2)建立主从移动机器人的追随误差模型，根据主从移动机器人的动态特性，对主从移动机器人追随系统的位置偏差和角度偏差进行分析，记e_φ(t)为主从移动机器人之间的航向角偏差，e_x(t)和e_y(t)为主从移动机器人之间的位置偏差，从移动机器人追随主移动机器人的追随偏差表示为：

$\left\{\begin{array}{c}{e}_x\left(t\right)=x_m\left(t\right)-x_s\left(t\right)\\ e_y\left(t\right)=y_m\left(t\right)-y_s\left(t\right)\\ e_{\mathrm{\φ}}\left(t\right)={\mathrm{\φ}}_m\left(t\right)-{\mathrm{\φ}}_s\left(t\right)\end{array}\right.---\left(2\right)$

其中，x_m(t)、y_m(t)表示主移动机器人质心在全局坐标系中的坐标，x_s(t)、y_s(t)表示从移动机器人质心在全局坐标系中的坐标，φ_m(t)表示主移动机器人航向角，φ_s(t)表示从移动机器人航向角；

主移动机器人的姿态信息z_m(t)＝[x_m(t),y_m(t),φ_m(t)]在轨迹跟踪过程中通过无线发送至从移动机器人，假定延时为τ，则t时刻从移动机器人接收到的主移动机器人的姿态信息为t-τ时刻的姿态信息，表示为z_m(t-τ)＝[x_m(t-τ),y_m(t-τ),φ_m(t-τ)]，偏差表示为：

$\left\{\begin{array}{c}{e}_x\left(t\right)=x_m(t-\mathrm{\τ})-x_s\left(t\right)\\ e_y\left(t\right)=y_m(t-\mathrm{\τ})-y_s\left(t\right)\\ e_{\mathrm{\φ}}\left(t\right)={\mathrm{\φ}}_m(t-\mathrm{\τ})-{\mathrm{\φ}}_s\left(t\right)\end{array}\right.---\left(3\right)$

对式(3)两边关于时间t求导，得：

$\left\{\begin{array}{c}\stackrel{\·}{e_x}\left(t\right)=\stackrel{\·}{x_m}(t-\mathrm{\τ})-\stackrel{\·}{x_s}\left(t\right)\\ \stackrel{\·}{e_y}\left(t\right)=\stackrel{\·}{y_m}(t-\mathrm{\τ})-\stackrel{\·}{y_s}\left(t\right)\\ \stackrel{\·}{e_{\mathrm{\φ}}}\left(t\right)=\stackrel{\·}{{\mathrm{\φ}}_m}(t-\mathrm{\τ})-\stackrel{\·}{{\mathrm{\φ}}_s}\left(t\right)\end{array}\right.---\left(4\right)$

由式(1)和(4)，得：

$\left\{\begin{array}{c}\stackrel{\·}{e_x}\left(t\right)=-v_s\left(t\right){\mathrm{cos\φ}}_s\left(t\right)+{\mathrm{dw}}_s\left(t\right){\mathrm{sin\φ}}_s\left(t\right)\\ +v_m(t-\mathrm{\τ}){\mathrm{cos\φ}}_m(t-\mathrm{\τ})-{\mathrm{dw}}_m(t-\mathrm{\τ}){\mathrm{sin\φ}}_m(t-\mathrm{\τ})\\ \stackrel{\·}{e_y}\left(t\right)=-v_s\left(t\right){\mathrm{sin\φ}}_s\left(t\right)-{\mathrm{dw}}_s\left(t\right){\mathrm{cos\φ}}_s\left(t\right)\\ +v_m(t-\mathrm{\τ}){\mathrm{sin\φ}}_m(t-\mathrm{\τ})+{\mathrm{dw}}_m(t-\mathrm{\τ}){\mathrm{cos\φ}}_m(t-\mathrm{\τ})\\ \stackrel{\·}{e_{\mathrm{\φ}}}\left(t\right)=w_m(t-\mathrm{\τ})-w_s\left(t\right)\end{array}\right.---\left(5\right)$

其中，v_m(t)和w_m(t)分别表示主移动机器人的移动线速度和角速度，v_s(t)和w_s(t)分别为从移动机器人的移动线速度和角速度，由式(3)知：

φ_s(t)＝φ_m(t-τ)-e_φ(t)(6)

将式(6)代入式(5)得：

$\left\{\begin{array}{c}\stackrel{\·}{d_1}=-v_s\left(t\right)\cos \left({\mathrm{\φ}}_m\right(t-\mathrm{\τ})-e_{\mathrm{\φ}}(t\left)\right)+{\mathrm{dw}}_s\left(t\right)\sin \left({\mathrm{\φ}}_m\right(t-\mathrm{\τ})-e_{\mathrm{\φ}}(t\left)\right)\\ +v_m(t-\mathrm{\τ}){\mathrm{cos\φ}}_m(t-\mathrm{\τ})-{\mathrm{dw}}_m(t-\mathrm{\τ}){\mathrm{sin\φ}}_m(t-\mathrm{\τ})\\ \stackrel{\·}{d_2}=-v_s\left(t\right)\sin \left({\mathrm{\φ}}_m\right(t-\mathrm{\τ})-e_{\mathrm{\φ}}(t\left)\right)+{\mathrm{dw}}_s\left(t\right)\cos \left({\mathrm{\φ}}_m\right(t-\mathrm{\τ})-e_{\mathrm{\φ}}(t\left)\right)\\ +v_m(t-\mathrm{\τ}){\mathrm{sin\φ}}_m(t-\mathrm{\τ})+{\mathrm{dw}}_m(t-\mathrm{\τ}){\mathrm{cos\φ}}_m(t-\mathrm{\τ})\\ \stackrel{\·}{e_{\mathrm{\φ}}}\left(t\right)=w_m(t-\mathrm{\τ})-w_s\left(t\right)\end{array}\right.---\left(7\right)$

式(7)为主从移动机器人的追随误差模型，d₁和d₂分别为主从移动机器人的横向距离和纵向距离；当主移动机器人的线速度v_m(t)和角速度w_m(t)给定后，控制从移动机器人的线速度v_s(t)和角速度w_s(t)使得主从移动机器人之间的横向距离d₁和纵向距离d₂为设定的常值；

步骤3)针对步骤2)中的追随误差模型(7)，以v_s(t)、w_s(t)为间接控制量，明确动态耦合和静态耦合部分，从而设计解耦律；

步骤4)设计线性扩张状态观测器对含时延的动态耦合部分进行估计和补偿；

步骤5)设计控制律控制从机器人的线速度v_s(t)和角速度w_s(t)，从而当主机的线速度v_m(t)和角速度w_m(t)给定后使得主从机器人之间的横向距离d₁和纵向距离d₂为设定的常值。

进一步，所述步骤3)中，所述的动态耦合部分为：

$\left[\begin{array}{c}{f}_1(v_m,w_m,{\mathrm{\φ}}_m,t)\\ f_2(v_m,w_m,{\mathrm{\φ}}_m,t)\end{array}\right]=\left[\begin{array}{c}{v}_m(t-\mathrm{\τ}){\mathrm{cos\φ}}_m(t-\mathrm{\τ})-{\mathrm{dw}}_m(t-\mathrm{\τ}){\mathrm{sin\φ}}_m(t-\mathrm{\τ})\\ v_m(t-\mathrm{\τ}){\mathrm{sin\φ}}_m(t-\mathrm{\τ})+{\mathrm{dw}}_m(t-\mathrm{\τ}){\mathrm{cos\φ}}_m(t-\mathrm{\τ})\end{array}\right]---\left(8\right)$

所述的静态耦合部分为：

$B=\left[\begin{array}{cc}-\cos \left({\mathrm{\φ}}_m\right(t-\mathrm{\τ})-e_{\mathrm{\φ}}(t\left)\right)& d\sin \left({\mathrm{\φ}}_m\right(t-\mathrm{\τ})-e_{\mathrm{\φ}}(t\left)\right)\\ -\sin \left({\mathrm{\φ}}_m\right(t-\mathrm{\τ})-e_{\mathrm{\φ}}(t\left)\right)& -d\cos \left({\mathrm{\φ}}_m\right(t-\mathrm{\τ})-e_{\mathrm{\φ}}(t\left)\right)\end{array}\right]---\left(9\right)$

由于d是移动机器人质心到驱动轮轴中心的距离，一定不为零，静态耦合矩阵B是可逆的，所以如式(7)所示的主从移动机器人的追随误差模型，当动态耦合部分被观测器观测补偿后，剩余的静态耦合部分乘以静态耦合矩阵的逆B^-1就可以得到式(7)所示追随误差系统的输入，即从移动机器人的线速度v_s(t)和角速度w_s(t)，从而做到解耦控制，其他类似解耦方法的处理为了避开解耦矩阵的复杂性而取特值，使得解耦矩阵为时不变矩阵然后利用自抗扰方法允许误差存在原理进行解耦，但自抗扰方法允许的误差是有限度的，本方法采用的是时变解耦矩阵，避免了误差存在；

再进一步，所述步骤4)中，对含时延的动态耦合部分进行估计和补偿，所述的线性扩张状态观测器，在横向间距跟踪控制中，线性扩张状态观测器方程为：

$\left\{\begin{array}{c}{\stackrel{\·}{\stackrel{\‾}{d}}}_1={\stackrel{\‾}{f}}_1-{\mathrm{\β}}_1({\stackrel{\·}{d}}_1-d_1)+b_0{u}_1\\ {\stackrel{\·}{\stackrel{\‾}{f}}}_1=-{\mathrm{\β}}_2({\stackrel{\·}{d}}_1-d_1)\end{array}\right.---\left(10\right)$

d₁为实际横向距离值，为实际横向距离的估计值，为含延时的动态耦合部分的估计值，b₀为补偿因子，β₁和β₂为观测器参数，u₁为式(7)所示追随误差系统的输入；在纵向间距跟踪控制中，线性扩张状态观测器方程为：

$\left\{\begin{array}{c}{\stackrel{\·}{\stackrel{\‾}{d}}}_2={\stackrel{\‾}{f}}_2-{\mathrm{\β}}_3({\stackrel{\·}{d}}_2-d_2)+b_1{u}_2\\ {\stackrel{\·}{\stackrel{\‾}{f}}}_2=-{\mathrm{\β}}_4({\stackrel{\·}{d}}_2-d_2)\end{array}\right.---\left(11\right)$

d₂为实际纵向距离值，为实际纵向距离的估计值，为含延时的动态耦合部分的估计值，b₁为补偿因子，β₃和β₄为观测器参数，u₂为式(7)所示追随误差系统的输入。关于(10)式的线性扩张状态观测器，可以求得其特征方程为：s²+β₁s+β₂把上式改写成为理想特征方程(s+w₁)²，于是有，[β₁β₂]^T＝[2ω₁ω₁ ²]^T，w₁称为观测器带宽。其中的w₁根据系统带宽的要求确定或在线整定，一般情况下w₁的适应范围很宽，因此比较容易调整出合适的w₁，关于(11)式的线性扩张状态观测器，也存在[β₃β₄]^T＝[2ω₂ω₂ ²]^T，也可以调整出合适的w₂；

更进一步，所述步骤5)中，所述的控制律，在横向间距跟踪控制中，表达式为：

$\{\begin{array}{c}{u}_{0x}=k_{p1}({d_1}^{*}-{\stackrel{\‾}{d}}_1)\\ u_1=(u_{0x}-{\stackrel{\‾}{f}}_1)/b_0\end{array}---\left(12\right)$

其中，d₁ ^*为期望的横向距离，k_p1为控制增益，u_0x为控制器输出，u₁为式(7)所示追随误差系统的输入；在纵向间距跟踪控制中，表达式为：

$\{\begin{array}{c}{u}_{0y}=k_{p2}({d_2}^{*}-{\stackrel{\‾}{d}}_2)\\ u_2=(u_{0y}-{\stackrel{\‾}{f}}_2)/b_1\end{array}---\left(13\right)$

其中，d₂ ^*为期望的纵向距离，k_p2为控制增益，u_0y为控制器输出，u₂为式(7)所示追随误差系统的输入。

本发明的有益效果主要表现在：由于在实际情况中，移动机器人运动速度变化时或者运动速度较大时都会不可避免地出现轮子打滑现象以及轨迹跟踪过程中主从移动机器人之间一定存在网络诱导时延现象，而本发明提出的线性自抗扰方法采用“基于误差消除误差”的控制理念的同时引入扩张状态观测器，对一切上述现象引发的模型不确定性进行估计和补偿，从而大大提高了系统的抗干扰能力；

附图说明

图1是移动机器人结构示意图，便于分析得到移动机器人运动学模型；

图2是主从移动机器人跟随系统示意图，便于得到主从移动机器人的追随误差模型；

图3是线性自抗扰解耦控制方法的结构图；

具体实施方式

下面结合附图对本发明作进一步描述。

参照图1到图3，一种基于线性自抗扰的网络化移动机器人轨迹跟踪控制方法，所述方法包括以下步骤：

步骤1)根据移动机器人的动态特性，在建立的全局坐标系中对其进行运动学分析，并假设质心到驱动轮轴心不重合，存在距离d，速度分解时引入了切向速度，从而得到更加符合实际情况的运动学模型；

建立移动机器人的运动学模型：

$\left[\begin{array}{c}\stackrel{\·}{x}\left(t\right)\\ \stackrel{\·}{y}\left(t\right)\\ \stackrel{\·}{\mathrm{\θ}}\left(t\right)\end{array}\right]=\left[\begin{array}{cc}\cos \mathrm{\θ}\left(t\right)& -d\sin \mathrm{\θ}\left(t\right)\\ \sin \mathrm{\θ}\left(t\right)& d\cos \mathrm{\θ}\left(t\right)\\ 0& 1\end{array}\right]\left[\begin{array}{c}v\left(t\right)\\ w\left(t\right)\end{array}\right]---\left(14\right)$

其中，x(t)、y(t)表示移动机器人质心在全局坐标系中的坐标，θ(t)表示移动机器人的航向角，v(t)表示移动机器人移动线速度，w(t)表示移动机器人移动角速度，d表示移动机器人质心到驱动轮轴心的距离；

步骤2)考虑本发明中移动机器人轨迹跟踪指的是从机器人以恒定的横向和纵向距离跟踪主机器人的运动，因此，刻画横、纵向距离差，并给出主从机器人的追随误差模型是十分必要的，在步骤1)的基础上考虑主从机器人通信传送过程中的延时为τ，根据主机器人和从机器人的动态模型，对主从机器人追随系统的位置偏差和角度偏差进行分析，从而得到主从移动机器人的追随误差模型；

建立主从移动机器人的追随误差模型，根据主从移动机器人的动态特性，对主从移动机器人的位置偏差和角度偏差进行分析，记e_φ(t)为主从移动机器人之间的航向角偏差，e_x(t)和e_y(t)为主从移动机器人之间的位置偏差，从移动机器人追随主移动机器人的追随偏差表示为：

$\left\{\begin{array}{c}{e}_x\left(t\right)=x_m\left(t\right)-x_s\left(t\right)\\ e_y\left(t\right)=y_m\left(t\right)-y_s\left(t\right)\\ e_{\mathrm{\φ}}\left(t\right)={\mathrm{\φ}}_m\left(t\right)-{\mathrm{\φ}}_s\left(t\right)\end{array}\right.---\left(15\right)$

其中，x_m(t)、y_m(t)表示主移动机器人质心在全局坐标系中的坐标，x_s(t)、y_s(t)表示从移动机器人质心在全局坐标系中的坐标，φ_m(t)表示主移动机器人航向角，φ_s(t)表示从移动机器人航向角；

主移动机器人的姿态信息z_m(t)＝[x_m(t),y_m(t),φ_m(t)]在轨迹跟踪过程中通过无线发送至从移动机器人，假定延时为τ，则t时刻从移动机器人接收到的主移动机器人的姿态信息为t-τ时刻的姿态信息，表示为z_m(t-τ)＝[x_m(t-τ),y_m(t-τ),φ_m(t-τ)]，偏差表示为：

$\left\{\begin{array}{c}{e}_x\left(t\right)=x_m(t-\mathrm{\τ})-x_s\left(t\right)\\ e_y\left(t\right)=y_m(t-\mathrm{\τ})-y_s\left(t\right)\\ e_{\mathrm{\φ}}\left(t\right)={\mathrm{\φ}}_m(t-\mathrm{\τ})-{\mathrm{\φ}}_s\left(t\right)\end{array}\right.---\left(16\right)$

对式(16)两边关于时间t求导，得：

$\left\{\begin{array}{c}\stackrel{\·}{e_x}\left(t\right)=\stackrel{\·}{x_m}(t-\mathrm{\τ})-\stackrel{\·}{x_s}\left(t\right)\\ \stackrel{\·}{e_y}\left(t\right)=\stackrel{\·}{y_m}(t-\mathrm{\τ})-\stackrel{\·}{y_s}\left(t\right)\\ \stackrel{\·}{e_{\mathrm{\φ}}}\left(t\right)=\stackrel{\·}{{\mathrm{\φ}}_m}(t-\mathrm{\τ})-\stackrel{\·}{{\mathrm{\φ}}_s}\left(t\right)\end{array}\right.---\left(17\right)$

由式(14)和(17)，得：

$\left\{\begin{array}{c}\stackrel{\·}{e_x}\left(t\right)=-v_s\left(t\right){\mathrm{cos\φ}}_s\left(t\right)+{\mathrm{dw}}_s\left(t\right){\mathrm{sin\φ}}_s\left(t\right)\\ +v_m(t-\mathrm{\τ}){\mathrm{cos\φ}}_m(t-\mathrm{\τ})-{\mathrm{dw}}_m(t-\mathrm{\τ}){\mathrm{sin\φ}}_m(t-\mathrm{\τ})\\ \stackrel{\·}{e_y}\left(t\right)=-v_s\left(t\right){\mathrm{sin\φ}}_s\left(t\right)-{\mathrm{dw}}_s\left(t\right){\mathrm{cos\φ}}_s\left(t\right)\\ +v_m(t-\mathrm{\τ}){\mathrm{sin\φ}}_m(t-\mathrm{\τ})+{\mathrm{dw}}_m(t-\mathrm{\τ}){\mathrm{cos\φ}}_m(t-\mathrm{\τ})\\ \stackrel{\·}{e_{\mathrm{\φ}}}\left(t\right)=w_m(t-\mathrm{\τ})-w_s\left(t\right)\end{array}\right.---\left(18\right)$

其中，v_m(t)和w_m(t)分别表示主移动机器人的移动线速度和角速度，v_s(t)和w_s(t)分别为从移动机器人的移动线速度和角速度，由式(16)知：

φ_s(t)＝φ_m(t-τ)-e_φ(t)(19)

将式(19)代入式(18)得：

$\left\{\begin{array}{c}\stackrel{\·}{d_1}=-v_s\left(t\right)\cos \left({\mathrm{\φ}}_m\right(t-\mathrm{\τ})-e_{\mathrm{\φ}}(t\left)\right)+{\mathrm{dw}}_s\left(t\right)\sin \left({\mathrm{\φ}}_m\right(t-\mathrm{\τ})-e_{\mathrm{\φ}}(t\left)\right)\\ +v_m(t-\mathrm{\τ}){\mathrm{cos\φ}}_m(t-\mathrm{\τ})-{\mathrm{dw}}_m(t-\mathrm{\τ}){\mathrm{sin\φ}}_m(t-\mathrm{\τ})\\ \stackrel{\·}{d_2}=-v_s\left(t\right)\sin \left({\mathrm{\φ}}_m\right(t-\mathrm{\τ})-e_{\mathrm{\φ}}(t\left)\right)+{\mathrm{dw}}_s\left(t\right)\cos \left({\mathrm{\φ}}_m\right(t-\mathrm{\τ})-e_{\mathrm{\φ}}(t\left)\right)\\ +v_m(t-\mathrm{\τ}){\mathrm{sin\φ}}_m(t-\mathrm{\τ})+{\mathrm{dw}}_m(t-\mathrm{\τ}){\mathrm{cos\φ}}_m(t-\mathrm{\τ})\\ \stackrel{\·}{e_{\mathrm{\φ}}}\left(t\right)=w_m(t-\mathrm{\τ})-w_s\left(t\right)\end{array}\right.---\left(20\right)$

式(20)为主从移动机器人的追随误差模型，d₁和d₂分别为主从移动机器人的横向距离和纵向距离；当主移动机器人的线速度v_m(t)和角速度w_m(t)给定后，控制从移动机器人的线速度v_s(t)和角速度w_s(t)使得主从移动机器人之间的横向距离d₁和纵向距离d₂为设定的常值；

步骤3)本发明所要求的轨迹跟踪控制的关键是对主从移动机器人之间横纵向距离的控制，系统的输入是从移动机器人的线速度和角速度，这样该系统可以看成两输入两输出系统，以v_s(t)，w_s(t)为间接控制量，将主从移动机器人的追随误差模型进行分解，得到动态耦合部分和静态耦合部分；

针对步骤2)中的追随误差模型(20)，以v_s(t)、w_s(t)为间接控制量，明确动态耦合和静态耦合部分，从而设计解耦律；

步骤4)设计线性扩张状态观测器并调节补偿因子系数和观测器带宽对动态耦合部分进行估计和补偿，使得解耦过程不需要对动态耦合部分进行考虑，只需考虑静态耦合部分，当移动机器人质心到驱动轮轴中心的距离d不等于零时，解耦矩阵B是可逆的，而移动机器人质心到驱动轮轴中心的距离一定不为零，这样关于本发明的两输入两输出系统一定可以做到解耦控制；

步骤5)设计控制律并调节控制增益保证良好的主从移动机器人横纵向间距跟踪效果。

设计控制律控制从机器人的线速度v_s(t)和角速度w_s(t)，从而当主机的线速度v_m(t)和角速度w_m(t)给定后使得主从机器人之间的横向距离d₁和纵向距离d₂为设定的常值。

本实例中，所述步骤2)中，所述的主从移动机器人的追随误差模型充分考虑了主从移动机器人之间的通信延时现象。

进一步，所述步骤3)中，所述的动态耦合部分为：

$\left[\begin{array}{c}{f}_1(v_m,w_m,{\mathrm{\φ}}_m,t)\\ f_2(v_m,w_m,{\mathrm{\φ}}_m,t)\end{array}\right]=\left[\begin{array}{c}{v}_m(t-\mathrm{\τ}){\mathrm{cos\φ}}_m(t-\mathrm{\τ})-{\mathrm{dw}}_m(t-\mathrm{\τ}){\mathrm{sin\φ}}_m(t-\mathrm{\τ})\\ v_m(t-\mathrm{\τ}){\mathrm{sin\φ}}_m(t-\mathrm{\τ})+{\mathrm{dw}}_m(t-\mathrm{\τ}){\mathrm{cos\φ}}_m(t-\mathrm{\τ})\end{array}\right]---\left(21\right)$

所述的静态耦合部分为：

$B=\left[\begin{array}{cc}-\cos \left({\mathrm{\φ}}_m\right(t-\mathrm{\τ})-e_{\mathrm{\φ}}(t\left)\right)& d\sin \left({\mathrm{\φ}}_m\right(t-\mathrm{\τ})-e_{\mathrm{\φ}}(t\left)\right)\\ -\sin \left({\mathrm{\φ}}_m\right(t-\mathrm{\τ})-e_{\mathrm{\φ}}(t\left)\right)& -d\cos \left({\mathrm{\φ}}_m\right(t-\mathrm{\τ})-e_{\mathrm{\φ}}(t\left)\right)\end{array}\right]---\left(22\right)$

由于d是移动机器人质心到驱动轮轴中心的距离，一定不为零，静态耦合矩阵B是可逆的，所以如式(20)所示的主从移动机器人的追随误差模型，当动态耦合部分被观测器观测补偿后，剩余的静态耦合部分乘以静态耦合矩阵的逆B^-1就可以得到系统的输入，即从移动机器人的线速度v_s(t)和角速度w_s(t)，从而做到解耦控制，其他类似解耦方法的处理为了避开解耦矩阵的复杂性而取特值，使得解耦矩阵为时不变矩阵然后利用自抗扰方法允许误差存在原理进行解耦，但自抗扰方法允许的误差是有限度的，本方法采用的是时变解耦矩阵，避免了误差存在；

再进一步，所述步骤4)中，对含时延的动态耦合部分进行估计和补偿，所述的线性扩张状态观测器，在横向间距跟踪控制中，线性扩张状态观测器方程为：

$\left\{\begin{array}{c}{\stackrel{\·}{\stackrel{\‾}{d}}}_1={\stackrel{\‾}{f}}_1-{\mathrm{\β}}_1({\stackrel{\·}{d}}_1-d_1)+b_0{u}_1\\ {\stackrel{\·}{\stackrel{\‾}{f}}}_1=-{\mathrm{\β}}_2({\stackrel{\·}{d}}_1-d_1)\end{array}\right.---\left(23\right)$

d₁为实际横向距离值，为实际横向距离的估计值，为含延时的动态耦合部分的估计值，b₀为补偿因子，β₁和β₂为观测器参数，u₁为式(20)所示追随误差系统的输入；在纵向间距跟踪控制中，线性扩张状态观测器方程为：

$\left\{\begin{array}{c}{\stackrel{\·}{\stackrel{\‾}{d}}}_2={\stackrel{\‾}{f}}_2-{\mathrm{\β}}_3({\stackrel{\·}{d}}_2-d_2)+b_1{u}_2\\ {\stackrel{\·}{\stackrel{\‾}{f}}}_2=-{\mathrm{\β}}_4({\stackrel{\·}{d}}_2-d_2)\end{array}\right.---\left(24\right)$

d₂为实际纵向距离值，为实际纵向距离的估计值，为含延时的动态耦合部分的估计值，b₁为补偿因子，β₃和β₄为观测器参数，u₂为式(20)所示追随误差系统的输入。关于(23)式的线性扩张状态观测器，可以求得其特征方程为：s²+β₁s+β₂把上式改写成为理想特征方程(s+w₁)²，于是有，[β₁β₂]^T＝[2ω₁ω₁ ²]^T，w₁称为观测器带宽。其中的w₁根据系统带宽的要求确定或在线整定，一般情况下w₁的适应范围很宽，因此比较容易调整出合适的w₁，关于(24)式的线性扩张状态观测器，也存在[β₃β₄]^T＝[2ω₂ω₂ ²]^T，也可以调整出合适的w₂；

更进一步，所述步骤5)中，所述的控制律，在横向间距跟踪控制中，表达式为：

$\{\begin{array}{c}{u}_{0x}=k_{p1}({d_1}^{*}-{\stackrel{\‾}{d}}_1)\\ u_1=(u_{0x}-{\stackrel{\‾}{f}}_1)/b_0\end{array}---\left(25\right)$

其中，d₁ ^*为期望的横向距离，k_p1为控制增益，u_0x为控制器输出，u₁为式(20)所示追随误差系统的输入；在纵向间距跟踪控制中，表达式为：

$\{\begin{array}{c}{u}_{0y}=k_{p2}({d_2}^{*}-{\stackrel{\‾}{d}}_2)\\ u_2=(u_{0y}-{\stackrel{\‾}{f}}_2)/b_1\end{array}---\left(26\right)$

其中，d₂ ^*为期望的纵向距离，k_p2为控制增益，u_0y为控制器输出，u₂为式(20)所示追随误差系统的输入。

如图1所示，移动机器人的各参数表示明晰，并且在全局坐标系中给定移动机器人的最一般姿态，对其进行动态分析，速度分解得到移动机器人的运动学模型，图中各符号表示为：

o——移动机器人两驱动轮中心点；

o'——移动机器人的旋转瞬心；

H——移动机器人两驱动轮中心之间的距离；

R——移动机器人绕瞬心的转弯半径；

r——轮子半径；

G(x(t),y(t))——移动机器人的质心位置；

θ(t)——移动机器人的航向角；

v(t)——移动机器人移动线速度；

w(t)——移动机器人移动角速度；

d——移动机器人质心到驱动轮轴心的距离；

如图2所示，为主从移动机器人在全局坐标系中的最一般姿态和相对位置关系表示，根据主从移动机器人的动态特性，对主从移动机器人的位置偏差和角度偏差进行分析，并基于已得到的移动机器人运动学模型上考虑主从移动机器人之间的网络诱导时延，从而得出主从移动机器人的追随误差模型；

如图3所示，为主从移动机器人横纵向间距的控制策略表示，包括线性扩张状态观测器(LESO)、控制律(LSEF)以及解耦律，由于主从移动机器人的追随误差模型可知本系统是一阶系统，故设计的线性自抗扰控制器包括两个部分，省略了跟踪微分器，从本文的解耦控制方法还可以看出，本文大胆创新的将模型中的动态耦合部分抽离出来，作为扰动项的一部分，被线性观测器观测得到并予以补偿，使得模型简化后更易于处理，有定理给出将模型部分量作为干扰项来处理的前提条件是，该部分存在一阶导并且一阶导是有界的，对于本文中的动态耦合部分，显然满足此条件。

Claims (3)

1.一种基于线性自抗扰的网络化移动机器人轨迹跟踪控制方法，其特征在于：所述方法包括以下步骤：

步骤1)建立移动机器人的运动学模型：

$\left[\begin{array}{c}\stackrel{\·}{x}\left(t\right)\\ \stackrel{\·}{y}\left(t\right)\\ \stackrel{\·}{\mathrm{\θ}}\left(t\right)\end{array}\right]=\left[\begin{array}{cc}cos\mathrm{\θ}\left(t\right)& -dsin\mathrm{\θ}\left(t\right)\\ sin\mathrm{\θ}\left(t\right)& d\cos \mathrm{\θ}\left(t\right)\\ 0& 1\end{array}\right]\left[\begin{array}{c}v\left(t\right)\\ w\left(t\right)\end{array}\right]---\left(1\right)$

其中，x(t)、y(t)表示移动机器人质心在全局坐标系中的坐标，θ(t)表示移动机器人的航向角，v(t)表示移动机器人移动线速度，w(t)表示移动机器人移动角速度，d表示移动机器人质心到驱动轮轴心的距离；

步骤2)建立主从移动机器人的追随误差模型，根据主从移动机器人的动态特性，对主从移动机器人的位置偏差和角度偏差进行分析，记e_φ(t)为主从移动机器人之间的航向角偏差，e_x(t)和e_y(t)为主从移动机器人之间的位置偏差，从移动机器人追随主移动机器人的追随偏差表示为：

$\left\{\begin{array}{c}{e}_x\left(t\right)=x_m\left(t\right)-x_s\left(t\right)\\ e_y\left(t\right)=y_m\left(t\right)-y_s\left(t\right)\\ e_{\mathrm{\φ}}\left(t\right)={\mathrm{\φ}}_m\left(t\right)-{\mathrm{\φ}}_s\left(t\right)\end{array}\right.---\left(2\right)$

其中，x_m(t)、y_m(t)表示主移动机器人质心在全局坐标系中的坐标，x_s(t)、y_s(t)表示从移动机器人质心在全局坐标系中的坐标，φ_m(t)表示主移动机器人的航向角，φ_s(t)表示从移动机器人的航向角；

主移动机器人的姿态信息z_m(t)＝[x_m(t),y_m(t),φ_m(t)]在轨迹跟踪过程中通过无线发送至从移动机器人，假定延时为τ，则t时刻从移动机器人接收到的主移动机器人的姿态信息为t-τ时刻的姿态信息，表示为z_m(t-τ)＝[x_m(t-τ),y_m(t-τ),φ_m(t-τ)]，偏差表示为：

$\left\{\begin{array}{c}{e}_x\left(t\right)=x_m(t-\mathrm{\τ})-x_s\left(t\right)\\ e_y\left(t\right)=y_m(t-\mathrm{\τ})-y_s\left(t\right)\\ e_{\mathrm{\φ}}\left(t\right)={\mathrm{\φ}}_m(t-\mathrm{\τ})-{\mathrm{\φ}}_s\left(t\right)\end{array}\right.---\left(3\right)$

对式(3)两边关于时间t求导，得：

$\left\{\begin{array}{c}\stackrel{\·}{e_x}\left(t\right)=\stackrel{\·}{x_m}(t-\mathrm{\τ})-\stackrel{\·}{x_s}\left(t\right)\\ \stackrel{\·}{e_y}\left(t\right)=\stackrel{\·}{y_m}(t-\mathrm{\τ})-\stackrel{\·}{y_s}\left(t\right)\\ \stackrel{\·}{e_{\mathrm{\φ}}}\left(t\right)=\stackrel{\·}{{\mathrm{\φ}}_m}(t-\mathrm{\τ})-\stackrel{\·}{{\mathrm{\φ}}_s}\left(t\right)\end{array}\right.---\left(4\right)$

由式(1)和(4)，得：

$\left\{\begin{array}{c}\stackrel{\·}{e_x}\left(t\right)=-v_s\left(t\right){\mathrm{cos\φ}}_s\left(t\right)+{\mathrm{dw}}_s\left(t\right){\mathrm{sin\φ}}_s\left(t\right)\\ +v_m(t-\mathrm{\τ}){\mathrm{cos\φ}}_m(t-\mathrm{\τ})-{\mathrm{dw}}_m(t-\mathrm{\τ}){\mathrm{sin\φ}}_m(t-\mathrm{\τ})\\ \stackrel{\·}{e_y}\left(t\right)=-v_s\left(t\right){\mathrm{sin\φ}}_s\left(t\right)-{\mathrm{dw}}_s\left(t\right){\mathrm{cos\φ}}_s\left(t\right)\\ +v_m(t-\mathrm{\τ}){\mathrm{sin\φ}}_m(t-\mathrm{\τ})+{\mathrm{dw}}_m(t-\mathrm{\τ}){\mathrm{cos\φ}}_m(t-\mathrm{\τ})\\ \stackrel{\·}{e_{\mathrm{\φ}}}\left(t\right)=w_m(t-\mathrm{\τ})-w_s\left(t\right)\end{array}\right.---\left(5\right)$

其中，v_m(t)和w_m(t)分别表示主移动机器人的移动线速度和角速度，v_s(t)和w_s(t)分别为从移动机器人的移动线速度和角速度，由式(3)知：

φ_s(t)＝φ_m(t-τ)-e_φ(t)(6)

将式(6)代入式(5)得：

$\left\{\begin{array}{c}{\stackrel{\·}{d}}_1=-v_s\left(t\right)cos\left({\mathrm{\φ}}_m\right(t-\mathrm{\τ})-e_{\mathrm{\φ}}\left(t\right))+{\mathrm{dw}}_s\left(t\right)sin\left({\mathrm{\φ}}_m\right(t-\mathrm{\τ})-e_{\mathrm{\φ}}\left(t\right))\\ +v_m(t-\mathrm{\τ}){\mathrm{cos\φ}}_m(t-\mathrm{\τ})-{\mathrm{dw}}_m(t-\mathrm{\τ}){\mathrm{sin\φ}}_m(t-\mathrm{\τ})\\ {\stackrel{\·}{d}}_2=-v_s\left(t\right)\sin \left({\mathrm{\φ}}_m\right(t-\mathrm{\τ})-e_{\mathrm{\φ}}\left(t\right))-{\mathrm{dw}}_s\left(t\right)\cos \left({\mathrm{\φ}}_m\right(t-\mathrm{\τ})-e_{\mathrm{\φ}}\left(t\right))\\ +v_m(t-\mathrm{\τ}){\mathrm{sin\φ}}_m(t-\mathrm{\τ})+{\mathrm{dw}}_m(t-\mathrm{\τ}){\mathrm{cos\φ}}_m(t-\mathrm{\τ})\\ \stackrel{\·}{e_{\mathrm{\φ}}}\left(t\right)=w_m(t-\mathrm{\τ})-w_s\left(t\right)\end{array}\right.---\left(7\right)$

式(7)为主从移动机器人的追随误差模型，d₁和d₂分别为主从移动机器人的横向距离和纵向距离；当主移动机器人的线速度v_m(t)和角速度w_m(t)给定后，控制从移动机器人的移动线速度v_s(t)和角速度w_s(t)使得主从移动机器人之间的横向距离d₁和纵向距离d₂为设定的常值；

步骤3)针对步骤2)中的追随误差模型(7)，以v_s(t)、w_s(t)为间接控制量，明确动态耦合和静态耦合部分，从而设计解耦律；

步骤4)设计线性扩张状态观测器对含时延的动态耦合部分进行估计和补偿；

步骤5)设计控制律控制从移动机器人的线速度v_s(t)和角速度w_s(t)，从而当主机的线速度v_m(t)和角速度w_m(t)给定后使得主从机器人之间的横向距离d₁和纵向距离d₂为设定的常值。

2.如权利要求1所述的一种基于线性自抗扰的网络化移动机器人轨迹跟踪控制方法，其特征在于：所述步骤3)中，所述的动态耦合部分为：

$\left[\begin{array}{c}{f}_1(v_m,w_m,{\mathrm{\φ}}_m,t)\\ f_2(v_m,w_m,{\mathrm{\φ}}_m,t)\end{array}\right]=\left[\begin{array}{c}{v}_m(t-\mathrm{\τ})cos{\mathrm{\φ}}_m(t-\mathrm{\τ})-d{w}_m(t-\mathrm{\τ})sin{\mathrm{\φ}}_m(t-\mathrm{\τ})\\ v_m(t-\mathrm{\τ})sin{\mathrm{\φ}}_m(t-\mathrm{\τ})+d{w}_m(t-\mathrm{\τ})cos{\mathrm{\φ}}_m(t-\mathrm{\τ})\end{array}\right]---\left(8\right)$

所述的静态耦合部分为：

$B=\left[\begin{array}{cc}-\cos \left({\mathrm{\φ}}_m\right(t-\mathrm{\τ})-e_{\mathrm{\φ}}\left(t\right))& d\sin \left({\mathrm{\φ}}_m\right(t-\mathrm{\τ})-e_{\mathrm{\φ}}\left(t\right))\\ -\sin \left({\mathrm{\φ}}_m\right(t-\mathrm{\τ})-e_{\mathrm{\φ}}\left(t\right))& -d\cos \left({\mathrm{\φ}}_m\right(t-\mathrm{\τ})-e_{\mathrm{\φ}}\left(t\right))\end{array}\right]---\left(9\right)$

由于d是移动机器人质心到驱动轮轴中心的距离，静态耦合矩阵B是可逆的，所以如式(7)所示的主从移动机器人的追随误差模型，当动态耦合部分被观测器观测补偿后，剩余的静态耦合部分乘以静态耦合矩阵的逆B^-1就可以得到式(7)所示追随误差系统的输入，即从移动机器人的线速度v_s(t)和角速度w_s(t)，从而做到解耦控制。

3.如权利要求1或2所述的一种基于线性自抗扰的网络化移动机器人轨迹跟踪控制方法，其特征在于：所述步骤4)中，对含时延的动态耦合部分进行估计和补偿，所述的线性扩张状态观测器，在横向间距跟踪控制中，线性扩张状态观测器方程为：

$\left\{\begin{array}{c}{\stackrel{\·}{\stackrel{\‾}{d}}}_1={\stackrel{\‾}{f}}_1-{\mathrm{\β}}_1({\stackrel{\·}{d}}_1-d_1)+b_0{u}_1\\ {\stackrel{\·}{\stackrel{\‾}{f}}}_1=-{\mathrm{\β}}_2({\stackrel{\·}{d}}_1-d_1)\end{array}\right.---\left(10\right)$

d₁为实际横向距离值，为实际横向距离的估计值，为含延时的动态耦合部分的估计值，b₀为补偿因子，β₁和β₂为观测器参数，u₁为式(7)所示追随误差系统的输入；在纵向间距跟踪控制中，线性扩张状态观测器方程为：

$\left\{\begin{array}{c}{\stackrel{\·}{\stackrel{\‾}{d}}}_2={\stackrel{\‾}{f}}_2-{\mathrm{\β}}_3({\stackrel{\·}{d}}_2-d_2)+b_1{u}_2\\ {\stackrel{\·}{\stackrel{\‾}{f}}}_2=-{\mathrm{\β}}_4({\stackrel{\·}{d}}_2-d_2)\end{array}\right.---\left(11\right)$

d₂为实际纵向距离值，为实际纵向距离的估计值，为含延时的动态耦合部分的估计值，b₁为补偿因子，β₃和β₄为观测器参数，u₂为式(7)所示追随误差系统的输入；关于(10)式的线性扩张状态观测器，求得其特征方程为：s²+β₁s+β₂把上式改写成为理想特征方程(s+w₁)²，于是有，[β₁β₂]^T＝[2ω₁ω₁ ²]^T，w₁称为观测器带宽；其中的w₁根据系统带宽的要求确定或在线整定。