CN106354160A - 一种n节拖挂式移动机器人反向运动时的方向角控制方法 - Google Patents

Abstract

本发明的n节拖挂式移动机器人反向运动时的方向角控制方法，拖挂式移动机器人由一牵引车和后端的n节拖车组成，其特征在于，由最后一节拖车开始，根据后面拖车的参考夹角和跟踪误差满足运动稳定性的条件，推导出前面拖车的参考夹角的大小，进而最终计算出牵引车的与n个拖车当前夹角相关的转向角，牵引车按照转向角运动，即可驱使拖挂式移动机器人平稳地反向运动。解决了带有n节拖车的机器人的方向跟踪控制问题，为n节拖挂式移动机器人反向运动控制提供了理论指导，有益效果显著，适于应用推广。

Description

一种n节拖挂式移动机器人反向运动时的方向角控制方法

技术领域

本发明涉及一种n节拖挂式移动机器人反向运动时的方向角控制方法，更具体的说，尤其涉及一种基于递归控制设计过程将方向角的跟踪控制问题转化为车体夹角的稳定控制问题的n节拖挂式移动机器人反向运动时的方向角控制方法。

背景技术

由于系统的特性，如高非线性运动学，非完整约束，结构奇异和反向运动时的不稳定性，带有n节拖车的移动机器人的反向运动控制问题是一个非常具有挑战性的任务。同时，该系统的输入和状态具有限制约束，控制输入需满足车体夹角这些特性使得控制机器人稳定地反向运动非常困难。

在过去的几十年里，带拖车的移动机器人的研究引起了广泛的关注，主要集中在三个领域：反馈镇定，运动规划，路径和轨迹跟踪。

由于非完整约束的存在，带拖车移动机器人的系统无法满足Brockett条件，这意味着类似系统的反馈镇定问题不存在任何连续时不变的渐近稳定的控制方法。因此，时变或不连续的控制方法。提出了将拖车系统转化为链式系统的方法。基于链式系统，提出了一种具有指数稳定性的反馈控制方法来解决带n节拖车的机器人的稳定性问题。链式系统可进一步转化为一个幂式系统，利用一个时变光滑的反馈控制方法，能够将幂式系统稳定在任何给定状态。σ过程是一个非连续的方法，它可以通过一个指数稳定的控制方法来解决反馈镇定问题。短时间可控性(STC)被引入到这样的非完整系统的运动规划问题中。由非完整系统转换生成的链式系统是幂零的，每个李氏括号的指数可以被精确地看作是控制向量场的有限组合。非完整系统可以计算出一个容许和分段的恒定控制输入来引导完整系统达到期望的状态。如果这两个位形位于一个连通流形上，并且系统在每个流形的位形上是短时间可控的，则运动规划问题就可以解决了。

根据链式幂零系统，利用正弦时变方法，可使系统从初始状态运动到任意所需的状态。Rouchon等人提出的驱动方法引入了平面度和线性输出的概念，以此来解决带有两节拖车的移动机器人的停车问题。对于未转换的系统，Michalek提出了一种用于带有n节拖车的机器人矢量场定向控制器，它可以引导机器人稳定地运动到给定点。

带有拖车的移动机器人的路径和轨迹跟踪问题也得到了很多专家的研究。Altafini专为卡车和拖车设计了混合控制器，它可以稳定的跟踪轨迹从而避免了jack-knife的影响。Astolfi利用Lyapunov技术为单轴拖车提出了一个渐近稳定的路径跟踪控制器。Yoo采用了一种新的机械结构，将拖车与牵引车的前保险杠连接在一起，并提出了一种控制算法来跟踪直线和圆弧参考路径。带有离轴拖车的试验车配备了结合边坡补偿器的双层控制器来跟踪由直线和曲线弧构成的路径]。Petrov采用高增益设计技术，针对反向驱动式移动机器人推离轴式拖车的问题，提出了一种具有非线性的路径控制方法。Khalaji提出了一种鲁棒自适应反馈线性化动态控制器来跟踪根据移动机器人运动学模型规划出的参考轨迹。

方向跟踪是另一个重要的控制问题。从实际应用的角度来看，无论是路径跟踪还是运动规划问题，都可以通过对拖车方向角采用适当的控制规律来解决。现有文献已提出了一种渐近稳定的控制器，带有两个轴拖车的移动机器人在反向运动中跟踪给定的方向角。此外，针对具有两个离轴拖车的机器人沿曲线轨迹的反向运动问题，也已有文献提出了具有渐近稳定性的反馈控制法。路径跟踪和轨迹跟踪的大部分工作都致力于有限拖车数量的机器人。

发明内容

本发明为了克服上述技术问题的缺点，提供了一种n节拖挂式移动机器人反向运动时的方向角控制方法。

本发明的n节拖挂式移动机器人反向运动时的方向角控制方法，拖挂式移动机器人由一牵引车和后端的n节拖车组成，牵引车为前轮转向后轮驱动，设牵引车的前轮转向角为线速度为v，定义前轮转向左边时， 前轮转向右边时，n节拖车通过车轴与前面牵引车或拖车连接，设牵引车的轴距为L₁，n节拖车的连杆长度分别为L₂、L₃、…、L_n+1，其特征在于，移动机器人反向运动时的方向角控制方法为：由最后一节拖车开始，根据后面拖车的参考夹角和跟踪误差满足运动稳定性的条件，推导出前面拖车的参考夹角的大小，进而最终计算出牵引车的与n个拖车当前夹角相关的转向角牵引车按照转向角运动，即可驱使拖挂式移动机器人平稳地反向运动。

本发明的n节拖挂式移动机器人反向运动时的方向角控制方法，移动机器人反向运动时的方向角控制方法通过以下步骤来实现：

a).建立坐标系，在牵引车和拖车所在的平面内建立直角坐标系，设牵引车和拖车的纵轴线相对于X轴的方向角为分别为θ₁、θ₂、…、θ_n+1；

b).建立微分方程，平面内牵引车和拖车的车轮与地面之间为非滑动运动，则推导出具有n节拖车的移动机器人的运动学微分方程为：

其中，x₁、y₁所组成的点(x₁、y₁)为以牵引车的后轴为中心建立的笛卡尔坐标系的原点；γ_i是车体之间的夹角，γ_i＝θ_i-θ_i+1，i＝1,2,...,n；

线速度v和转向角作为控制输入，在控制过程中v设定为常值，反向运动时为负值，通过控制的大小来保证反向运动的稳定；

c).条件设定，为了避免牵引车反向运动时的动态特性不可控，转向角应满足：反向运动过程中为了避免相邻车体之间弯成V字形而无法运动，应满足：i＝1,2,...,n；反向运动，v＜0；

d).设定期望方向角，通过设计控制输入的反馈控制律，设系统能够渐近稳定的跟踪恒定的期望方向角为θ_d，

即：

同时车体之间的夹角满足：

$\underset{t\→\mathrm{\∞}}{lim}{\mathrm{\γ}}_i=0,i=1,2,...,n;$

为了保证在初始条件下牵引车可正常反向运动，应满足θ_n+1(0)为牵引车的反向运动的初始夹角；

e).第n、n-1和n-2节拖车状态的求取，对于给定的期望方向角θ_d，定义第n节拖车的参考夹角为：

${\widetilde{\mathrm{\γ}}}_n={\mathrm{\θ}}_{n+1}-{\mathrm{\θ}}_d---\left(2\right)$

定义γ_n与之间的跟踪误差s₁为：

$s_1=sin{\widetilde{\mathrm{\γ}}}_n-{\mathrm{sin\γ}}_n---\left(3\right)$

由微分方程(1)中的关系式，可以推导出s₁的导数为：

${\stackrel{\·}{s}}_1=v\left(\frac{1}{L_{n+1}}{\mathrm{tan\γ}}_{n}cos\right({\mathrm{\θ}}_{n+1}-{\mathrm{\θ}}_d)+\frac{1}{L_{n+1}}{\mathrm{sin\γ}}_n-\frac{1}{L_n}{\mathrm{tan\γ}}_{n-1})\underset{i=1}{\overset{n}{\Π}}{\mathrm{cos\γ}}_i---\left(4\right)$

如果令：

$\frac{1}{L_{n+1}}{\mathrm{tan\γ}}_{n}cos({\mathrm{\θ}}_{n+1}-{\mathrm{\θ}}_d)+\frac{1}{L_{n+1}}{\mathrm{sin\γ}}_n-\frac{1}{L_n}{\mathrm{tan\γ}}_{n-1}=k_1{s}_1---\left(5\right)$

其中，k₁＞0；由于v＜0、则可以得到：

${\stackrel{\·}{s}}_1=-\left|k_{1}v\underset{i=1}{\overset{n}{\Π}}{\mathrm{cos\γ}}_i\right|s_1$

根据Lyapunov稳定性定理，可知当t→∞，s₁将会渐近收敛到零；为满足公式(5)，第n-1节拖车的参考夹角定义为：

${\widetilde{\mathrm{\γ}}}_{n-1}=arctan\left(\frac{L_n}{L_{n+1}}{\mathrm{tan\γ}}_{n}cos\right({\mathrm{\θ}}_{n+1}-{\mathrm{\θ}}_d)+\frac{L_n}{L_{n+1}}{\mathrm{sin\γ}}_n-L_n{k}_1{s}_1)---\left(6\right)$

定义γ_n-1与之间的跟踪误差s₂为：

$s_2=sin{\widetilde{\mathrm{\γ}}}_{-1}-{\mathrm{sin\γ}}_{n-1}---\left(7\right)$

由微分方程(1)中的关系式，可以推导出s₂的导数为：

${\stackrel{\·}{s}}_2=v(\frac{1}{v}\cos {\widetilde{\mathrm{\γ}}}_{n-1}{\stackrel{\·}{\widetilde{\mathrm{\γ}}}}_{n-1}\underset{i=1}{\overset{n-1}{\Π}}{\mathrm{sec\γ}}_i+\frac{1}{L_n}{\mathrm{sin\γ}}_{n-1}-\frac{1}{L_{n-1}}{\mathrm{tan\γ}}_{n-2})\underset{i=1}{\overset{n-1}{\Π}}{\mathrm{cos\γ}}_i---\left(8\right)$

同样地，令：

$\frac{1}{v}cos{\widetilde{\mathrm{\γ}}}_{n-1}{\stackrel{\·}{\widetilde{\mathrm{\γ}}}}_{n-1}\underset{i=1}{\overset{n-1}{\Π}}{\mathrm{sec\γ}}_i+\frac{1}{L_n}{\mathrm{sin\γ}}_{n-1}-\frac{1}{L_{n-1}}{\mathrm{tan\γ}}_{n-2}=k_2{s}_2---\left(9\right)$

其中，k₂＞0；由于v＜0、则可以得到：

${\stackrel{\·}{s}}_2=-\left|k_{2}v\underset{i=1}{\overset{n-1}{\Π}}{\mathrm{cos\γ}}_i\right|s_2---\left(10\right)$

可知当t→∞，s₂将会渐近收敛到零；为满足公式(10)，第n-2节拖车的参考夹角定义为：

${\widetilde{\mathrm{\γ}}}_{n-2}=arctan(\frac{L_{n-1}}{v}cos{\widetilde{\mathrm{\γ}}}_{n-1}{\stackrel{\·}{\widetilde{\mathrm{\γ}}}}_{n-1}\underset{i=1}{\overset{n-1}{\Π}}{\mathrm{sec\γ}}_i+\frac{L_{n-1}}{L_n}{\mathrm{sin\γ}}_{n-1}-L_{n-1}{k}_2{s}_2)---\left(11\right);$

f).递推运算，由步骤e)中的运算过程，可递推出：

$s_i=sin{\widetilde{\mathrm{\γ}}}_{n+1-i}-{\mathrm{sin\γ}}_{n+1-i},i=2,3,...,n---\left(12\right)$

以及：

${\widetilde{\mathrm{\γ}}}_i=\arctan (\frac{L_{i+1}}{v}\cos {\widetilde{\mathrm{\γ}}}_{i+1}{\stackrel{\·}{\widetilde{\mathrm{\γ}}}}_{i+1}\underset{j=1}{\overset{i+1}{\Π}}{\mathrm{sec\γ}}_j+\frac{L_{n+1}}{L_{i+2}}{\mathrm{sin\γ}}_{i+1}-L_{i+1}{k}_{n-i}{s}_{n-i}),i=n-2,n-1,\mathrm{...},1---\left(13\right)$

由微分方程(1)中的关系式，可以推导出s_i的导数为：

${\stackrel{\·}{s}}_i=v(\frac{1}{v}\cos {\widetilde{\mathrm{\γ}}}_{n+1-i}{\stackrel{\·}{\widetilde{\mathrm{\γ}}}}_{n+1-i}\underset{j=1}{\overset{n-i+1}{\Π}}{\mathrm{sec\γ}}_j+\frac{1}{L_{n+2-i}}{\mathrm{sin\γ}}_{n+1-i}-\frac{1}{L_{n+1-i}}{\mathrm{tan\γ}}_{n-i})\underset{j=1}{\overset{n+1-i}{\Π}}{\mathrm{cos\γ}}_j---\left(14\right);$

g).求牵引车控制输入将i＝n代入公式(14)可得：

可知当t→∞，s_n将会渐近收敛到零，则推导出牵引车的控制输入为：

其中，k_n＞0，为控制参数；s_n由公式(12)求取，γ₁＝θ₁-θ₂，γ₁由公式(13)求取，为γ₁的导数；

牵引车利用公式(16)确定的控制输入作为反馈控制律，即可驱使拖挂式移动机器人反向运动。

本发明的n节拖挂式移动机器人反向运动时的方向角控制方法，所述牵引车上设置有陀螺仪，以获取当前的方向角θ₁；n节拖车与前面牵引车或拖车的连接处设置有角度传感器，以采集相对的转动角度，进而计算得到n节拖车的方向角θ_i，i＝2,3,...,n+1。

本发明的有益效果是：本发明的n节拖挂式移动机器人反向运动时的方向角控制方法，由最后一节拖车开始，根据后面拖车的参考夹角和跟踪误差满足运动稳定性的条件，推导出前面拖车的参考夹角的大小，进而最终计算出牵引车的与n个拖车当前夹角相关的转向角牵引车按照转向角运动，即可驱使拖挂式移动机器人平稳地反向运动。解决了带有n节拖车的机器人的方向跟踪控制问题，为n节拖挂式移动机器人反向运动控制提供了理论指导，有益效果显著，适于应用推广。

附图说明

图1为本发明的n节拖挂式移动机器人的结构示意图。

具体实施方式

下面结合附图与实施例对本发明作进一步说明。

如图1所示，给出了本发明的n节拖挂式移动机器人的结构示意图，拖挂式移动机器人由一牵引车和后端的n节拖车组成，牵引车为前轮转向后轮驱动，设牵引车的前轮转向角为线速度为v，定义前轮转向左边时，前轮转向右边时，n节拖车通过车轴与前面牵引车或拖车连接，设牵引车的轴距为L₁，n节拖车的连杆长度分别为L₂、L₃、…、L_n+1；P_i(x_i,y_i)(i＝1,2,…,n+1)是在牵引车和拖车的后轴中心建立的笛卡尔坐标系。

移动机器人反向运动时的方向角控制方法通过以下步骤来实现：

a).建立坐标系，在牵引车和拖车所在的平面内建立直角坐标系，设牵引车和拖车的纵轴线相对于X轴的方向角为分别为θ₁、θ₂、…、θ_n+1；

b).建立微分方程，平面内牵引车和拖车的车轮与地面之间为非滑动运动，则推导出具有n节拖车的移动机器人的运动学微分方程为：

其中，x₁、y₁所组成的点(x₁、y₁)为以牵引车的后轴为中心建立的笛卡尔坐标系的原点；γ_i是车体之间的夹角，γ_i＝θ_i-θ_i+1，i＝1,2,...,n；

线速度v和转向角作为控制输入，在控制过程中v设定为常值，反向运动时为负值，通过控制的大小来保证反向运动的稳定；

c).条件设定，为了避免牵引车反向运动时的动态特性不可控，转向角应满足：反向运动过程中为了避免相邻车体之间弯成V字形而无法运动，应满足：i＝1,2,...,n；反向运动，v＜0；

d).设定期望方向角，通过设计控制输入的反馈控制律，设系统能够渐近稳定的跟踪恒定的期望方向角为θ_d，

即：

同时车体之间的夹角满足：

$\underset{t\→\mathrm{\∞}}{lim}{\mathrm{\γ}}_i=0,i=1,2,...,n;$

为了保证在初始条件下牵引车可正常反向运动，应满足θ_n+1(0)为牵引车的反向运动的初始夹角；

e).第n、n-1和n-2节拖车状态的求取，对于给定的期望方向角θ_d，定义第n节拖车的参考夹角为：

${\widetilde{\mathrm{\γ}}}_n={\mathrm{\θ}}_{n+1}-{\mathrm{\θ}}_d---\left(2\right)$

定义γ_n与之间的跟踪误差s₁为：

$s_1=sin{\widetilde{\mathrm{\γ}}}_n-{\mathrm{sin\γ}}_n---\left(3\right)$

由微分方程(1)中的关系式，可以推导出s₁的导数为：

${\stackrel{\·}{s}}_1=v\left(\frac{1}{L_{n+1}}{\mathrm{tan\γ}}_{n}cos\right({\mathrm{\θ}}_{n+1}-{\mathrm{\θ}}_d)+\frac{1}{L_{n+1}}{\mathrm{sin\γ}}_n-\frac{1}{L_n}{\mathrm{tan\γ}}_{n-1})\underset{i=1}{\overset{n}{\Π}}{\mathrm{cos\γ}}_i---\left(4\right)$

如果令：

$\frac{1}{L_{n+1}}{\mathrm{tan\γ}}_{n}cos({\mathrm{\θ}}_{n+1}-{\mathrm{\θ}}_d)+\frac{1}{L_{n+1}}{\mathrm{sin\γ}}_n-\frac{1}{L_n}{\mathrm{tan\γ}}_{n-1}=k_1{s}_1---\left(5\right)$

其中，k₁＞0；由于v＜0、则可以得到：

${\stackrel{\·}{s}}_1=-\left|k_{1}v\underset{i=1}{\overset{n}{\Π}}{\mathrm{cos\γ}}_i\right|s_1$

根据Lyapunov稳定性定理，可知当t→∞，s₁将会渐近收敛到零；为满足公式(5)，第n-1节拖车的参考夹角定义为：

${\widetilde{\mathrm{\γ}}}_{n-1}=arctan\left(\frac{L_n}{L_{n+1}}{\mathrm{tan\γ}}_{n}cos\right({\mathrm{\θ}}_{n+1}-{\mathrm{\θ}}_d)+\frac{L_n}{L_{n+1}}{\mathrm{sin\γ}}_n-L_n{k}_1{s}_1)---\left(6\right)$

定义γ_n-1与之间的跟踪误差s₂为：

$s_2=sin{\widetilde{\mathrm{\γ}}}_{n-1}-{\mathrm{sin\γ}}_{n-1}---\left(7\right)$

由微分方程(1)中的关系式，可以推导出s₂的导数为：

${\stackrel{\·}{s}}_2=v(\frac{1}{v}\cos {\widetilde{\mathrm{\γ}}}_{n-1}{\stackrel{\·}{\widetilde{\mathrm{\γ}}}}_{n-1}\underset{i=1}{\overset{n-1}{\Π}}{\mathrm{sec\γ}}_i+\frac{1}{L_n}{\mathrm{sin\γ}}_{n-1}-\frac{1}{L_{n-1}}{\mathrm{tan\γ}}_{n-2})\underset{i=1}{\overset{n-1}{\Π}}{\mathrm{cos\γ}}_i---\left(8\right)$

同样地，令：

$\frac{1}{v}cos{\widetilde{\mathrm{\γ}}}_{n-1}{\stackrel{\·}{\widetilde{\mathrm{\γ}}}}_{n-1}\underset{i=1}{\overset{n-1}{\Π}}{\mathrm{sec\γ}}_i+\frac{1}{L_n}{\mathrm{sin\γ}}_{n-1}-\frac{1}{L_{n-1}}{\mathrm{tan\γ}}_{n-2}=k_2{s}_2---\left(9\right)$

其中，k₂＞0；由于v＜0、则可以得到：

${\stackrel{\·}{s}}_2=-\left|k_{2}v\underset{i=1}{\overset{n-1}{\Π}}{\mathrm{cos\γ}}_i\right|s_2---\left(10\right)$

可知当t→∞，s₂将会渐近收敛到零；为满足公式(10)，第n-2节拖车的参考夹角定义为：

${\widetilde{\mathrm{\γ}}}_{n-2}=arctan(\frac{L_{n-1}}{v}cos{\widetilde{\mathrm{\γ}}}_{n-1}{\stackrel{\·}{\widetilde{\mathrm{\γ}}}}_{n-1}\underset{i=1}{\overset{n-1}{\Π}}{\mathrm{sec\γ}}_i+\frac{L_{n-1}}{L_n}{\mathrm{sin\γ}}_{n-1}-L_{n-1}{k}_2{s}_2)---\left(11\right);$

f).递推运算，由步骤e)中的运算过程，可递推出：

$s_i=sin{\widetilde{\mathrm{\γ}}}_{n+1-i}-{\mathrm{sin\γ}}_{n+1-i},i=2,3,...,n---\left(12\right)$

以及：

${\widetilde{\mathrm{\γ}}}_i=\arctan (\frac{L_{i+1}}{v}\cos {\widetilde{\mathrm{\γ}}}_{i+1}{\stackrel{\·}{\widetilde{\mathrm{\γ}}}}_{i+1}\underset{j=1}{\overset{i+1}{\Π}}{\mathrm{sec\γ}}_j+\frac{L_{n+1}}{L_{i+2}}{\mathrm{sin\γ}}_{i+1}-L_{i+1}{k}_{n-i}{s}_{n-i}),i=n-2,n-1,\mathrm{...},1---\left(13\right)$

由微分方程(1)中的关系式，可以推导出s_i的导数为：

${\stackrel{\·}{s}}_i=v(\frac{1}{v}\cos {\widetilde{\mathrm{\γ}}}_{n+1-i}{\stackrel{\·}{\widetilde{\mathrm{\γ}}}}_{n+1-i}\underset{j=1}{\overset{n-i+1}{\Π}}{\mathrm{sec\γ}}_j+\frac{1}{L_{n+2-i}}{\mathrm{sin\γ}}_{n+1-i}-\frac{1}{L_{n+1-i}}{\mathrm{tan\γ}}_{n-i})\underset{j=1}{\overset{n+1-i}{\Π}}{\mathrm{cos\γ}}_j---\left(14\right);$

g).求牵引车控制输入将i＝n代入公式(14)可得：

可知当t→∞，s_n将会渐近收敛到零，则推导出牵引车的控制输入为：

其中，k_n＞0，为控制参数；s_n由公式(12)求取，γ₁＝θ₁-θ₂，γ₁由公式(13)求取，为γ₁的导数；

牵引车利用公式(16)确定的控制输入作为反馈控制律，即可驱使拖挂式移动机器人反向运动。

其中，所述牵引车上设置有陀螺仪，以获取当前的方向角θ₁；n节拖车与前面牵引车或拖车的连接处设置有角度传感器，以采集相对的转动角度，进而计算得到n节拖车的方向角θ_i，i＝2,3,...,n+1。

证明1：为了证明公式(1)确定的n节拖挂式移动机器人反向运动系统的稳定性，需要证明对于v＜0且公式(16)确定的控制律能够稳定的反向驱动机器人，使得γ_i渐近稳定收敛到证明过程如下：

将控制律(16)代入(15)，可得：

${\stackrel{\·}{s}}_n=-k_n\left|v\right|{\mathrm{cos\γ}}_n{s}_n---\left(17\right)$

定义Lyapunov函数可以得到所以s_n将会渐近收敛到原点。

由(12)可知：

$s_{n-1}=sin{\widetilde{\mathrm{\γ}}}_2-{\mathrm{sin\γ}}_2---\left(18\right)$

$s_n=sin{\widetilde{\mathrm{\γ}}}_1-{\mathrm{sin\γ}}_1---\left(19\right)$

由(13)可知：

${\widetilde{\mathrm{\γ}}}_1=arctan(\frac{L_2}{v}cos{\widetilde{\mathrm{\γ}}}_2{\stackrel{\·}{\widetilde{\mathrm{\γ}}}}_2\underset{j=1}{\overset{2}{\Π}}{\mathrm{sec\γ}}_j+\frac{L_2}{L_3}{\mathrm{sin\γ}}_2-L_2{k}_{n-1}{s}_{n-1})---\left(20\right)$

由公式(18)和公式(1)可得：

${\stackrel{\·}{s}}_{n-1}=v(\frac{1}{v}cos{\widetilde{\mathrm{\γ}}}_2{\stackrel{\·}{\widetilde{\mathrm{\γ}}}}_2{\mathrm{sec\γ}}_2-\frac{1}{L_2}{\mathrm{sin\γ}}_1+\frac{1}{L_3}{\mathrm{cos\γ}}_1{\mathrm{sin\γ}}_2){\mathrm{cos\γ}}_2---\left(21\right)$

由公式(19)可知代入(21)可得：

${\stackrel{\·}{s}}_{n-1}=v(\frac{1}{v}cos{\widetilde{\mathrm{\γ}}}_2{\stackrel{\·}{\widetilde{\mathrm{\γ}}}}_2{\mathrm{sec\γ}}_2-\frac{1}{L_2}(sin{\widetilde{\mathrm{\γ}}}_1-s_n)+\frac{1}{L_3}{\mathrm{cos\γ}}_1{\mathrm{sin\γ}}_2){\mathrm{cos\γ}}_2---\left(22\right)$

将s_n作为子系统(22)的控制输入，则(22)和(17)构成了一种级联系统。当s_n＝0，则有则(22)式变为：

$\begin{array}{c}{\stackrel{\·}{s}}_{n-1}=v{\mathrm{cos\γ}}_1{\mathrm{cos\γ}}_2{k}_{n-1}{s}_{n-1}\\ =-k_{n-1}\left|v\right|{\mathrm{cos\γ}}_1{\mathrm{cos\γ}}_2{s}_{n-1}\end{array}---\left(23\right)$

定义Lyapunov函数可以得到所以所以s_n-1将会渐近收敛到原点。

类似的，可以递推的证明s_i(i＝2,...,n)在原点是渐近收敛的。由(13)可知：

${\widetilde{\mathrm{\γ}}}_{n-i}=\arctan (\frac{L_{n+1-i}}{v}\cos {\widetilde{\mathrm{\γ}}}_{n+1-i}{\stackrel{\·}{\widetilde{\mathrm{\γ}}}}_{n+1-i}\underset{j=1}{\overset{n+1-i}{\Π}}{\mathrm{sec\γ}}_j+\frac{L_{n+1-i}}{L_{n+2-i}}{\mathrm{sin\γ}}_{n+1-i}-L_{n+1-i}{k}_i{s}_i)---\left(24\right)$

假设已证明s_i+1在原点是渐近稳定的，则当s_i+1＝0时有将s_i+1作为(14)的控制输入，并令则(14)变为：

${\stackrel{\·}{s}}_i=-k_{i-1}\left|v\right|s_i\underset{j=1}{\overset{n+1-i}{\Π}}{\mathrm{cos\γ}}_j$

定义Lyapunov函数则有由此可知系统(17)在s_i+1＝0时在原点是指数稳定的，即是输入状态稳定的。另外，已知s_i+1在原点是渐近稳定的，则和组成的级联系统是渐近稳定的。由此可得s_i＝0时在原点是指数稳定的。

根据s_i的定义可知当s_i＝0，有

证明2：考虑带有n节拖车的移动机器人系统(1)，假设v＜0且 控制律(16)能够稳定的反向驱动机器人，使得θ_i渐近稳定收敛到θ_d，

证明：对于第n节拖车的子系统，定义：

$s_0=sin{\widetilde{\mathrm{\γ}}}_n$

s₀的导数可以推导为：

$\begin{array}{c}{s}_0=\left(\frac{v}{L_{n+1}}\cos {\widetilde{\mathrm{\γ}}}_n\underset{i=1}{\overset{n-1}{\Π}}{\mathrm{cos\γ}}_i\right){\mathrm{sin\γ}}_n\\ =\left(\frac{v}{L_{n+1}}\cos {\widetilde{\mathrm{\γ}}}_n\underset{i=1}{\overset{n-1}{\Π}}{\mathrm{cos\γ}}_i\right)({\mathrm{sin\γ}}_i-s_1)\end{array}---\left(25\right)$

将s₁作为(25)的控制输入，令s₁＝0，则有：

${\stackrel{\·}{s}}_0=\left(\frac{v}{L_{n+1}}cos{\widetilde{\mathrm{\γ}}}_n\underset{i=1}{\overset{n-1}{\Π}}{\mathrm{cos\γ}}_i\right)sin{\widetilde{\mathrm{\γ}}}_n$

定义Lyapunov函数可得：

$\begin{array}{c}{\stackrel{\·}{V}}_0=\left(\frac{v}{L_{n+1}}\cos {\widetilde{\mathrm{\γ}}}_n\underset{i=1}{\overset{n-1}{\Π}}{\mathrm{cos\γ}}_i\right)\sin {\widetilde{\mathrm{\γ}}}_n^2\\ \≤-\left|v\right|\frac{1}{L_{n+1}}\cos {\widetilde{\mathrm{\γ}}}_n\underset{i=1}{\overset{n-1}{\Π}}{\mathrm{cos\γ}}_i\sin {\widetilde{\mathrm{\γ}}}_n^2\end{array}$

所以，当s₁＝0时，系统(24)在原点是指数收敛的，即系统(24)是输入-状态稳定的。由系统(24)和(4)构成的级联系统是渐近稳定的。当t→∞时，s₀将会渐近收敛到原点。

根据定理1，有s_i→0(i＝1,2,...,n)。当s₀→0时，有当γ_n→0且s₁→0时，由(6)可得类似，可以递推证明γ_i→0(i＝1,2,...,n)。由γ_i→0(i＝1,2,...,n)，可以得到θ_i→θ_d(i＝1,2,...,n+1)，即在公式(16)确定的控制输入的控制作用下，牵引车和n节拖车的纵轴线相对于X轴的方向角θ_i最终会达到一致，均等于θ_d。

Claims (3)

1.一种n节拖挂式移动机器人反向运动时的方向角控制方法，拖挂式移动机器人由一牵引车和后端的n节拖车组成，牵引车为前轮转向后轮驱动，设牵引车的前轮转向角为线速度为v，定义前轮转向左边时，前轮转向右边时，n节拖车通过车轴与前面牵引车或拖车连接，设牵引车的轴距为L₁，n节拖车的连杆长度分别为L₂、L₃、…、L_n+1，其特征在于，移动机器人反向运动时的方向角控制方法为：由最后一节拖车开始，根据后面拖车的参考夹角和跟踪误差满足运动稳定性的条件，推导出前面拖车的参考夹角的大小，进而最终计算出牵引车的与n个拖车当前夹角相关的转向角牵引车按照转向角运动，即可驱使拖挂式移动机器人平稳地反向运动。

2.根据权利要求1所述的n节拖挂式移动机器人反向运动时的方向角控制方法，其特征在于：移动机器人反向运动时的方向角控制方法通过以下步骤来实现：

a).建立坐标系，在牵引车和拖车所在的平面内建立直角坐标系，设牵引车和拖车的纵轴线相对于X轴的方向角为分别为θ₁、θ₂、…、θ_n+1；

b).建立微分方程，平面内牵引车和拖车的车轮与地面之间为非滑动运动，则推导出具有n节拖车的移动机器人的运动学微分方程为：

其中，x₁、y₁所组成的点(x₁、y₁)为以牵引车的后轴为中心建立的笛卡尔坐标系的原点；γ_i是车体之间的夹角，γ_i＝θ_i-θ_i+1，i＝1,2,...,n；

线速度v和转向角作为控制输入，在控制过程中v设定为常值，反向运动时为负值，通过控制的大小来保证反向运动的稳定；

c).条件设定，为了避免牵引车反向运动时的动态特性不可控，转向角应满足：反向运动过程中为了避免相邻车体之间弯成V字形而无法运动，应满足：i＝1,2,...,n；反向运动，v＜0；

d).设定期望方向角，通过设计控制输入的反馈控制律，设系统能够渐近稳定的跟踪恒定的期望方向角为θ_d，

即：

同时车体之间的夹角满足：

为了保证在初始条件下牵引车可正常反向运动，应满足θ_n+1(0)为牵引车的反向运动的初始夹角；

e).第n、n-1和n-2节拖车状态的求取，对于给定的期望方向角θ_d，定义第n节拖车的参考夹角为：

${\widetilde{\mathrm{\γ}}}_n={\mathrm{\θ}}_{n+1}-{\mathrm{\θ}}_d---\left(2\right)$

定义γ_n与之间的跟踪误差s₁为：

$s_1=sin{\widetilde{\mathrm{\γ}}}_n-{\mathrm{sin\γ}}_n---\left(3\right)$

由微分方程(1)中的关系式，可以推导出s₁的导数为：

${\stackrel{\·}{s}}_1=v\left(\frac{1}{L_{n+1}}{\mathrm{tan\γ}}_{n}cos\right({\mathrm{\θ}}_{n+1}-{\mathrm{\θ}}_d)+\frac{1}{L_{n+1}}{\mathrm{sin\γ}}_n-\frac{1}{L_n}{\mathrm{tan\γ}}_{n-1})\underset{i=1}{\overset{n}{\Π}}{\mathrm{cos\γ}}_i---\left(4\right)$

如果令：

$\frac{1}{L_{n+1}}{\mathrm{tan\γ}}_{n}cos({\mathrm{\θ}}_{n+1}-{\mathrm{\θ}}_d)+\frac{1}{L_{n+1}}{\mathrm{sin\γ}}_n-\frac{1}{L_n}{\mathrm{tan\γ}}_{n-1}=k_1{s}_1---\left(5\right)$

其中，k₁＞0；由于v＜0、则可以得到：

${\stackrel{\·}{s}}_1=-\left|k_{1}v\underset{i=1}{\overset{n}{\Π}}{\mathrm{cos\γ}}_i\right|s_1$

根据Lyapunov稳定性定理，可知当t→∞，s₁将会渐近收敛到零；为满足公式(5)，第n-1节拖车的参考夹角定义为：

${\widetilde{\mathrm{\γ}}}_{n-1}=arctan\left(\frac{L_n}{L_{n+1}}{\mathrm{tan\γ}}_{n}cos\right({\mathrm{\θ}}_{n+1}-{\mathrm{\θ}}_d)+\frac{L_n}{L_{n+1}}{\mathrm{sin\γ}}_n-L_n{k}_1{s}_1)---\left(6\right)$

定义γ_n-1与之间的跟踪误差s₂为：

$s_2=sin{\widetilde{\mathrm{\γ}}}_{n-1}-{\mathrm{sin\γ}}_{n-1}---\left(7\right)$

由微分方程(1)中的关系式，可以推导出s₂的导数为：

${\stackrel{\·}{s}}_2=v(\frac{1}{v}\cos {\widetilde{\mathrm{\γ}}}_{n-1}{\stackrel{\·}{\widetilde{\mathrm{\γ}}}}_{n-1}\underset{i=1}{\overset{n-1}{\mathrm{\Π}}}{\mathrm{sec\γ}}_i+\frac{1}{L_n}{\mathrm{sin\γ}}_{n-1}-\frac{1}{L_{n-1}}{\mathrm{tan\γ}}_{n-2})\underset{i=1}{\overset{n-1}{\mathrm{\Π}}}{\mathrm{cos\γ}}_i---\left(8\right)$

同样地，令：

$\frac{1}{v}cos{\widetilde{\mathrm{\γ}}}_{n-1}{\stackrel{\·}{\widetilde{\mathrm{\γ}}}}_{n-1}\underset{i=1}{\overset{n-1}{\Π}}{\mathrm{sec\γ}}_i+\frac{1}{L_n}{\mathrm{sin\γ}}_{n-1}-\frac{1}{L_{n-1}}{\mathrm{tan\γ}}_{n-2}=k_2{s}_2---\left(9\right)$

其中，k₂＞0；由于v＜0、则可以得到：

${\stackrel{\·}{s}}_2=-\left|k_{2}v\underset{i=1}{\overset{n-1}{\Π}}{\mathrm{cos\γ}}_i\right|s_2---\left(10\right)$

可知当t→∞，s₂将会渐近收敛到零；为满足公式(10)，第n-2节拖车的参考夹角定义为：

${\widetilde{\mathrm{\γ}}}_{n-2}=arctan(\frac{L_{n-1}}{v}cos{\widetilde{\mathrm{\γ}}}_{n-1}{\stackrel{\·}{\widetilde{\mathrm{\γ}}}}_{n-1}\underset{i=1}{\overset{n-1}{\Π}}{\mathrm{sec\γ}}_i+\frac{L_{n-1}}{L_n}{\mathrm{sin\γ}}_{n-1}-L_{n-1}{k}_2{s}_2)---\left(11\right);$

f).递推运算，由步骤e)中的运算过程，可递推出：

$s_i=sin{\widetilde{\mathrm{\γ}}}_{n+1-i}-{\mathrm{sin\γ}}_{n+1-i},i=2,3,...,n---\left(12\right)$

以及：

${\widetilde{\mathrm{\γ}}}_i=\arctan (\frac{L_{i+1}}{v}\cos {\widetilde{\mathrm{\γ}}}_{i+1}{\stackrel{\·}{\widetilde{\mathrm{\γ}}}}_{i+1}\underset{j=1}{\overset{i+1}{\mathrm{\Π}}}{\mathrm{sec\γ}}_j+\frac{L_{n+1}}{L_{i+2}}{\mathrm{sin\γ}}_{i+1}-L_{i+1}{k}_{n-i}{s}_{n-i}),i=n-2,n-1,...,1---\left(13\right)$

其中，k_n-i＞0,；

由微分方程(1)中的关系式，可以推导出s_i的导数为：

${\stackrel{\·}{s}}_i=v(\frac{1}{v}cos{\widetilde{\mathrm{\γ}}}_{n+1-i}{\stackrel{\·}{\widetilde{\mathrm{\γ}}}}_{n+1-i}\underset{j=1}{\overset{n-i+1}{\Π}}{\mathrm{sec\γ}}_j+\frac{1}{L_{n+2-i}}{\mathrm{sin\γ}}_{n+1-i}-\frac{1}{L_{n+1-i}}{\mathrm{tan\γ}}_{n-i})\underset{j=1}{\overset{n+1-i}{\Π}}{\mathrm{cos\γ}}_j---\left(14\right);$

g).求牵引车控制输入将i＝n代入公式(14)可得：

可知当t→∞，s_n将会渐近收敛到零，则推导出牵引车的控制输入为：

其中，k_n＞0，为控制参数；s_n由公式(12)求取，γ₁＝θ₁-θ₂，γ₁由公式(13)求取，为γ₁的导数；

牵引车利用公式(16)确定的控制输入作为反馈控制律，即可驱使拖挂式移动机器人反向运动。

3.根据权利要求1或2所述的n节拖挂式移动机器人反向运动时的方向角控制方法，其特征在于：所述牵引车上设置有陀螺仪，以获取当前的方向角θ₁；n节拖车与前面牵引车或拖车的连接处设置有角度传感器，以采集相对的转动角度，进而计算得到n节拖车的方向角θ_i，i＝2,3,...,n+1。