CN104635734A - 履带式机器人的轨迹跟踪方法 - Google Patents

Abstract

本发明涉及机器人智能控制方法技术领域，是一种履带式机器人的轨迹跟踪方法，按照以下步骤进行：建立履带式机器人的初始运动学方程；设定切换函数，计算出基于常规趋近律的控制函数；使用基于切换函数的指数增益替换常量增益，计算出基于指数趋近律的滑模控制律；对每一组输入数据和每一组输出数据使用基于指数趋近律的滑模控制律，建立履带式机器人的初始控制函数矩阵；调整履带式机器人的方向为沿运动轨迹的切线方向；使用基于指数趋近律的滑模控制律，计算出履带式机器人的控制函数矩阵。本发明能根据路径规划，实现对机器人轨迹的智能控制，减少驾驶员的操作难度，提高履带式机器人在复杂地形的越野能力。

Description

履带式机器人的轨迹跟踪方法

技术领域

本发明涉及机器人智能控制方法技术领域，是一种履带式机器人的轨迹跟踪方法。

背景技术

履带式机器人由于其良好的越野机动性能，牵引附着性能，卓越的自复位和越障能力，而受到越来越广泛的重视。特别是在复杂地形，履带式机器人较其他机器人有明显地优势，然而要控制履带式装置，驾驶员必须完成一系列复杂的机械操作，这样既增加了驾驶员的工作量，又降低了履带式装置在复杂地形机动的能力，因此，根据路径规划，对履带式机器人进行自动控制非常重要。

目前，国内外有很多学者对履带式机器人的轨迹跟踪问题进行了研究。一种常用的方法是设计了“基于时间”的跟踪控制器，然后利用STR映射将此控制器转换成非时间参考的控制器，这种方法的实现较为复杂，不易找到合适的STR映射；另一种方法为通过重新设计切换面，提出了一种全局非奇异终端滑模控制器，可用于带参数不确定和外部扰动的二阶非线性动态系统，然而这种方法易产生较大的抖振，且实现起来很复杂。

发明内容

本发明提供了一种履带式机器人的轨迹跟踪方法，克服了上述现有技术之不足，其能有效解决现有存在的费时费力、施工效率较低、存在安全隐患的问题。

本发明的技术方案是通过以下措施来实现的：一种履带式机器人的轨迹跟踪方法，按照以下步骤进行：步骤101，在平面二维坐标系中，建立履带式机器人的初始运动学方程；步骤102，设定切换函数，计算出基于常规趋近律的控制函数；步骤103，使用基于切换函数的指数增益替换常量增益，计算出基于指数趋近律的滑模控制律；步骤104，对每一组输入数据和每一组输出数据使用基于指数趋近律的滑模控制律，建立履带式机器人的初始控制函数矩阵；步骤105，调整履带式机器人的方向为沿运动轨迹的切线方向；步骤106，使用基于指数趋近律的滑模控制律，计算出履带式机器人的控制函数矩阵。

下面是对上述发明技术方案的进一步优化或/和改进：

上述在步骤101中，用x表示履带式机器人在平面二维坐标系中的横坐标，y表示履带式机器人在平面二维坐标系中的纵坐标，用x′表示履带式机器人沿横坐标的速度，y′表示履带式机器人沿纵坐标的速度，在平面二维坐标系中，建立履带式机器人的初始运动学方程：

$\left\{\begin{array}{c}{x}^{\′}(v_1+v_2)/2\cos \left(\mathrm{\θ}\right)\\ y^{\′}=(v_1+v_2)/2\sin \left(\mathrm{\θ}\right)\\ \mathrm{\ω}\·\mathrm{\Δt}\·H=v_1\·\mathrm{\Δt}-v_2\·\mathrm{\Δt}\\ {\mathrm{\θ}}^{\′}=\mathrm{\ω}=(v_1-v_2)/H\\ v_1=\frac{x^{\′}}{\cos \left(0\right)}+\frac{H\·{\mathrm{\θ}}^{\′}}{2}\\ v_2=\frac{x^{\′}}{\cos \left(\mathrm{\θ}\right)}-\frac{H\·{\mathrm{\θ}}^{\′}}{2}\end{array}\right.$

其中，H表示履带式机器人的宽度，v₁和v₂分别表示左右履带轮的速度，θ表示履带式机器人与横轴的夹角，w或θ′表示角速度，Δt为一短时间，系统的输入变量为x_d和y_d，输出变量为x，y，θ，v₁，v₂；

在步骤102中，设定切换函数为s＝(x″-x_d″)+λ(x′-x_d′)，采用滑模常规趋近律s＝-kgsat(s)，则基于常规趋近律的滑模控制函数为：

(x″-x_d″)+λ(x′-x_d′)＝-k·sat((x′-x_d′)+λ(x-x_d))

其中，x和x_d分别表示实际轨迹变量和参考轨迹变量，sat()为饱和函数 $(\mathrm{sat}\left(x\right)=\left\{\begin{array}{cc}1& x\&GreaterEqual;1\\ x& -1<x<1\\ -1& x\&le;-1\end{array}\right.),$ λ为切换函数系数，k为控制律的增益；

在步骤103中，以基于切换函数的指数增益替换常规趋近律的常量增益k，建立基于指数趋近律的滑模控制函数：

$(x^{\&prime;\&prime;}-{x_d}^{\&prime;\&prime;})+\mathrm{\&lambda;}(x^{\&prime;}-{x_d}^{\&prime;})=-\frac{k}{\mathrm{\&alpha;}+(1-\mathrm{\&alpha;})\&CenterDot;e^{-\left|s\right|}}\&CenterDot;\mathrm{sat}((x^{\&prime;}-{x_d}^{\&prime;})+\mathrm{\&lambda;}(x-x_d))$

其中，s为切换函数，α为指数趋近律的控制系数；

在步骤104中，对履带式机器人的每一个运动状态都使用基于指数趋近律的滑模控制函数，可列出初始的控制函数矩阵：

$\left[\begin{array}{c}{x}^{\&prime;\&prime;}\\ y^{\&prime;\&prime;}\\ {\mathrm{\&theta;}}^{\&prime;\&prime;}\\ {v_1}^{\&prime;\&prime;}\\ {v_2}^{\&prime;\&prime;}\end{array}\right]=\left[\begin{array}{c}{x_d}^{\&prime;\&prime;}\\ {y_d}^{\&prime;\&prime;}\\ {{\mathrm{\&theta;}}_d}^{\&prime;\&prime;}\\ {v_{d1}}^{\&prime;\&prime;}\\ {v_{d2}}^{\&prime;\&prime;}\end{array}\right]-\left[\begin{array}{c}{\mathrm{\&lambda;}}_1\&CenterDot;(x^{\&prime;}-{x_d}^{\&prime;})\\ {\mathrm{\&lambda;}}_2\&CenterDot;(y^{\&prime;}-{y_d}^{\&prime;})\\ {\mathrm{\&lambda;}}_3\&CenterDot;({\mathrm{\&theta;}}^{\&prime;}-{{\mathrm{\&theta;}}_d}^{\&prime;})\\ {\mathrm{\&lambda;}}_4\&CenterDot;({v_1}^{\&prime;}-{v_{d1}}^{\&prime;})\\ {\mathrm{\&lambda;}}_5\&CenterDot;({v_2}^{\&prime;}-{v_{d2}}^{\&prime;})\end{array}\right]+\left[\begin{array}{c}\frac{k_1}{{\mathrm{\&alpha;}}_1+(1-{\mathrm{\&alpha;}}_1)\&CenterDot;e^{-\left|s_1\right|}}\&CenterDot;\mathrm{sat}({\mathrm{\&lambda;}}_1\&CenterDot;(x-x_d)+(x^{\&prime;}-{x_d}^{\&prime;}))\\ \frac{k_2}{{\mathrm{\&alpha;}}_2+(1-{\mathrm{\&alpha;}}_2)\&CenterDot;e^{-\left|s_2\right|}}\&CenterDot;\mathrm{sat}({\mathrm{\&lambda;}}_2\&CenterDot;(y-y_d)+(y^{\&prime;}-{y_d}^{\&prime;}))\\ \frac{k_3}{{\mathrm{\&alpha;}}_3+(1-{\mathrm{\&alpha;}}_3)\&CenterDot;e^{-\left|s_3\right|}}\&CenterDot;\mathrm{sat}({\mathrm{\&lambda;}}_3\&CenterDot;(\mathrm{\&theta;}-{\mathrm{\&theta;}}_d)+({\mathrm{\&theta;}}^{\&prime;}-{{\mathrm{\&theta;}}_d}^{\&prime;}))\\ \frac{k_4}{{\mathrm{\&alpha;}}_4+(1-{\mathrm{\&alpha;}}_4)\&CenterDot;e^{-\left|s_4\right|}}\&CenterDot;\mathrm{sat}({\mathrm{\&lambda;}}_4\&CenterDot;(v_1-v_{d1})+({v_1}^{\&prime;}-{y_{d1}}^{\&prime;}))\\ \frac{k_5}{{\mathrm{\&alpha;}}_5+(1-{\mathrm{\&alpha;}}_5)\&CenterDot;e^{-\left|s_5\right|}}\&CenterDot;\mathrm{sat}({\mathrm{\&lambda;}}_5\&CenterDot;(v_2-v_{d2})+({v_2}^{\&prime;}-{y_{d2}}^{\&prime;}))\end{array}\right]$

其中，x_d,y_d,θ_d,v_d1,v_d2分别表示横纵坐标、转角、左右履带轮速度的参考轨迹变量，x,y,θ,v₁,v₂则表示它们的实际轨迹变量；x′,y′,θ′,v₁′,v₂′和x_d′,y_d′,θ_d′,v_d1′,v_d2′表示他们的1阶导数；x″,y″,θ″,v₁″,v₂″和x_d″,y_d″,θ_d″,v_d1″,v_d2″表示他们的2阶导数；λ₁，λ₂，λ₃，λ₄，λ₅为对应控制律的切换函数系数；s₁，s₂，s₃，s₄，s₅为对应控制律的切换函数；k₁，k₂，k₃，k₄，k₅为对应控制律的增益系数；α₁，α₂，α₃，α₄，α₅为对应指数控制律的控制系数；

在步骤105中，设定履带式机器人的方向为沿运动轨迹的切线方向，则调整履带式机器人的运动学方程为

$\left\{\begin{array}{c}{v}_1=\sqrt{x^{\&prime;2}+y^{\&prime;2}}+\frac{H\&CenterDot;x^{\&prime;2}}{2(x^{\&prime;2}+y^{\&prime;2})}\\ v_2=\sqrt{x^{\&prime;2}+y^{\&prime;2}}-\frac{H\&CenterDot;x^{\&prime;2}}{2(x^{\&prime;2}+y^{\&prime;2})}\\ \mathrm{\&theta;}=\arctan \left(\frac{y^{\&prime;}}{x^{\&prime;}}\right)\end{array}\right.;$

在步骤106中，履带式机器人的控制函数矩阵为

$\left[\begin{array}{c}{x}^{\&prime;\&prime;}\\ y^{\&prime;\&prime;}\\ \mathrm{\&theta;}\\ v_1\\ v_2\end{array}\right]=\left[\begin{array}{c}{x_d}^{\&prime;\&prime;}-{\mathrm{\&lambda;}}_1\&CenterDot;(x^{\&prime;}-{x_d}^{\&prime;})+\frac{k_1}{{\mathrm{\&alpha;}}_1+(1-{\mathrm{\&alpha;}}_1)\&CenterDot;e^{-\left|s_1\right|}}\&CenterDot;\mathrm{sat}({\mathrm{\&lambda;}}_1\&CenterDot;(x-x_d)+(x^{\&prime;}-{x_d}^{\&prime;}))\\ {y_d}^{\&prime;\&prime;}-{\mathrm{\&lambda;}}_2\&CenterDot;(y^{\&prime;}-{y_d}^{\&prime;})+\frac{k_2}{{\mathrm{\&alpha;}}_2+(1-{\mathrm{\&alpha;}}_2)\&CenterDot;e^{-\left|s_2\right|}}\&CenterDot;\mathrm{sat}({\mathrm{\&lambda;}}_2\&CenterDot;(y-y_d)+(y^{\&prime;}-{y_d}^{\&prime;}))\\ \arctan \left(\frac{y^{\&prime;}}{x^{\&prime;}}\right)\\ \sqrt{x^{\&prime;2}+y^{\&prime;2}}+\frac{H\&CenterDot;x^{\&prime;2}}{2(x^{\&prime;2}+y^{\&prime;2})}\\ \sqrt{x^{\&prime;2}+y^{\&prime;2}}+\frac{H\&CenterDot;x^{\&prime;2}}{2(x^{\&prime;2}+y^{\&prime;2})}\end{array}\right].$

上述在步骤104中，输入的数据为采集得到的数据，即 $\begin{array}{c}{x}_d\\ y_d\\ {\mathrm{\&theta;}}_d\\ v_{1d}\\ v_{2d}\end{array};$ 输出的数据为 $\begin{array}{c}x\\ y\\ \mathrm{\&theta;}\\ v_1\\ v_2\end{array}.$

上述在步骤104中，输入的数据为采集得到的数据，即 $\begin{array}{c}{x}_d\\ y_d\end{array}.$

本发明提供的履带式机器人的轨迹跟踪方法，首先建立基于轨迹跟踪的初始运动学模型，再针对建立的模型，设计滑模切换函数和初始输出控制律，然后结合实际应用对运动学方程进行调整，同时对滑模控制律进行改进，以指数趋近律替代常规趋近律，这样能以减小抖振，加快系统到达理想状态的时间，提高系统的稳定性，最后将基于指数趋近律的滑模控制律应用于履带式机器人的轨迹控制中，计算出最后的履带式机器人的控制函数矩阵。其能根据路径规划，实现对机器人轨迹的智能控制，减少驾驶员的操作难度，提高履带式机器人在复杂地形的越野能力。

附图说明

附图1为本发明所述履带式机器人的轨迹跟踪方法的流程图。

具体实施方式

本发明不受下述实施例的限制，可根据本发明的技术方案与实际情况来确定具体的实施方式。

下面结合实施例及附图对本发明作进一步描述：

如附图1所示，该履带式机器人的轨迹跟踪方法，按照以下步骤进行：步骤101，在平面二维坐标系中，建立履带式机器人的初始运动学方程；步骤102，设定切换函数，计算出基于常规趋近律的控制函数；步骤103，使用基于切换函数的指数增益替换常量增益，计算出基于指数趋近律的滑模控制律；步骤104，对每一组输入数据和每一组输出数据使用基于指数趋近律的滑模控制律，建立履带式机器人的初始控制函数矩阵；步骤105，调整履带式机器人的方向为沿运动轨迹的切线方向；步骤106，使用基于指数趋近律的滑模控制律，计算出履带式机器人的控制函数矩阵，则可实现对履带式机器人的轨迹跟踪。履带式机器人的初始运动学方程是指履带式机器人的速度、角速度以及左右履带轮速与运动轨迹之间的关系式。切换函数是指系统在滑模面附近运动时，调节系统，使系统能趋近于理想状态的函数。滑模控制律是指使用滑模控制的方法，针对系统设计的控制函数。

可根据实际需要，对上述履带式机器人的轨迹跟踪方法作进一步优化

或/和改进：

实施例：步骤101，用x表示履带式机器人在平面二维坐标系中的横坐标，y表示履带式机器人在平面二维坐标系中的纵坐标，用x′表示履带式机器人沿横坐标的速度，y′表示履带式机器人沿纵坐标的速度，在平面二维坐标系中，建立履带式机器人的初始运动学方程：

$\left\{\begin{array}{c}{x}^{\&prime;}(v_1+v_2)/2\cos \left(\mathrm{\&theta;}\right)\\ y^{\&prime;}=(v_1+v_2)/2\sin \left(\mathrm{\&theta;}\right)\\ \mathrm{\&omega;}\&CenterDot;\mathrm{\&Delta;t}\&CenterDot;H=v_1\&CenterDot;\mathrm{\&Delta;t}-v_2\&CenterDot;\mathrm{\&Delta;t}\\ {\mathrm{\&theta;}}^{\&prime;}=\mathrm{\&omega;}=(v_1-v_2)/H\\ v_1=\frac{x^{\&prime;}}{\cos \left(0\right)}+\frac{H\&CenterDot;{\mathrm{\&theta;}}^{\&prime;}}{2}\\ v_2=\frac{x^{\&prime;}}{\cos \left(\mathrm{\&theta;}\right)}-\frac{H\&CenterDot;{\mathrm{\&theta;}}^{\&prime;}}{2}\end{array}\right.$

其中，H表示履带式机器人的宽度，v₁和v₂分别表示左右履带轮的速度，θ表示履带式机器人与横轴的夹角，w或θ′表示角速度，Δt为一短时间，系统的输入变量为x_d和y_d，输出变量为x，_y，θ，v₁，v₂。

步骤102，设定切换函数为s＝(x″-x_d″)+λ(x′-x_d′)，采用滑模常规趋近律s＝-kgsat(s)，则基于常规趋近律的滑模控制函数为：

(x″-x_d″)+λ(x′-x_d′)＝-k·sat((x′-x_d′)+λ(x-x_d))

其中，x和x_d分别表示实际轨迹变量和参考轨迹变量，sat()为饱和函数 $(\mathrm{sat}\left(x\right)=\left\{\begin{array}{cc}1& x\&GreaterEqual;1\\ x& -1<x<1\\ -1& x\&le;-1\end{array}\right.),$ λ为切换函数系数，k为控制律的增益。

步骤103，以基于切换函数的指数增益替换常规趋近律的常量增益k，建立基于指数趋近律的滑模控制函数：

$(x^{\&prime;\&prime;}-{x_d}^{\&prime;\&prime;})+\mathrm{\&lambda;}(x^{\&prime;}-{x_d}^{\&prime;})=-\frac{k}{\mathrm{\&alpha;}+(1-\mathrm{\&alpha;})\&CenterDot;e^{-\left|s\right|}}\&CenterDot;\mathrm{sat}((x^{\&prime;}-{x_d}^{\&prime;})+\mathrm{\&lambda;}(x-x_d))$

其中，s为切换函数，α为指数趋近律的控制系数。

步骤104，对履带式机器人的每一个运动状态都使用基于指数趋近律的滑模控制函数，可列出初始的控制函数矩阵：

$\left[\begin{array}{c}{x}^{\&prime;\&prime;}\\ y^{\&prime;\&prime;}\\ {\mathrm{\&theta;}}^{\&prime;\&prime;}\\ {v_1}^{\&prime;\&prime;}\\ {v_2}^{\&prime;\&prime;}\end{array}\right]=\left[\begin{array}{c}{x_d}^{\&prime;\&prime;}\\ {y_d}^{\&prime;\&prime;}\\ {{\mathrm{\&theta;}}_d}^{\&prime;\&prime;}\\ {v_{d1}}^{\&prime;\&prime;}\\ {v_{d2}}^{\&prime;\&prime;}\end{array}\right]-\left[\begin{array}{c}{\mathrm{\&lambda;}}_1\&CenterDot;(x^{\&prime;}-{x_d}^{\&prime;})\\ {\mathrm{\&lambda;}}_2\&CenterDot;(y^{\&prime;}-{y_d}^{\&prime;})\\ {\mathrm{\&lambda;}}_3\&CenterDot;({\mathrm{\&theta;}}^{\&prime;}-{{\mathrm{\&theta;}}_d}^{\&prime;})\\ {\mathrm{\&lambda;}}_4\&CenterDot;({v_1}^{\&prime;}-{v_{d1}}^{\&prime;})\\ {\mathrm{\&lambda;}}_5\&CenterDot;({v_2}^{\&prime;}-{v_{d2}}^{\&prime;})\end{array}\right]+\left[\begin{array}{c}\frac{k_1}{{\mathrm{\&alpha;}}_1+(1-{\mathrm{\&alpha;}}_1)\&CenterDot;e^{-\left|s_1\right|}}\&CenterDot;\mathrm{sat}({\mathrm{\&lambda;}}_1\&CenterDot;(x-x_d)+(x^{\&prime;}-{x_d}^{\&prime;}))\\ \frac{k_2}{{\mathrm{\&alpha;}}_2+(1-{\mathrm{\&alpha;}}_2)\&CenterDot;e^{-\left|s_2\right|}}\&CenterDot;\mathrm{sat}({\mathrm{\&lambda;}}_2\&CenterDot;(y-y_d)+(y^{\&prime;}-{y_d}^{\&prime;}))\\ \frac{k_3}{{\mathrm{\&alpha;}}_3+(1-{\mathrm{\&alpha;}}_3)\&CenterDot;e^{-\left|s_3\right|}}\&CenterDot;\mathrm{sat}({\mathrm{\&lambda;}}_3\&CenterDot;(\mathrm{\&theta;}-{\mathrm{\&theta;}}_d)+({\mathrm{\&theta;}}^{\&prime;}-{{\mathrm{\&theta;}}_d}^{\&prime;}))\\ \frac{k_4}{{\mathrm{\&alpha;}}_4+(1-{\mathrm{\&alpha;}}_4)\&CenterDot;e^{-\left|s_4\right|}}\&CenterDot;\mathrm{sat}({\mathrm{\&lambda;}}_4\&CenterDot;(v_1-v_{d1})+({v_1}^{\&prime;}-{y_{d1}}^{\&prime;}))\\ \frac{k_5}{{\mathrm{\&alpha;}}_5+(1-{\mathrm{\&alpha;}}_5)\&CenterDot;e^{-\left|s_5\right|}}\&CenterDot;\mathrm{sat}({\mathrm{\&lambda;}}_5\&CenterDot;(v_2-v_{d2})+({v_2}^{\&prime;}-{y_{d2}}^{\&prime;}))\end{array}\right]$

其中，x_d,y_d,θ_d,v_d1,v_d2分别表示横纵坐标、转角、左右履带轮速度的参考轨迹变量，x,y,θ,v₁,v₂则表示它们的实际轨迹变量；x′,y′,θ′,v₁′,v₂′和x_d′,y_d′,θ_d′,v_d1′,v_d2′表示他们的1阶导数；x″,y″,θ″,v₁″,v₂″和x_d″,y_d″,θ_d″,v_d1″,v_d2″表示他们的2阶导数；λ₁，λ₂，λ₃，λ₄，λ₅为对应控制律的切换函数系数；s₁，s₂，s₃，s₄，s₅为对应控制律的切换函数；k₁，k₂，k₃，k₄，k₅为对应控制律的增益系数；α₁，α₂，α₃，α₄，α₅为对应指数控制律的控制系数。步骤4中采集的数据只有输入，即 $\begin{array}{c}{x}_d\\ y_d\\ {\mathrm{\&theta;}}_d\\ v_{1d}\\ v_{2d}\end{array};$ 而输出，即 $\begin{array}{c}x\\ y\\ \mathrm{\&theta;}\\ v_1\\ v_2\end{array}.$ 是通过控制矩阵最后求得的数据。通过后续几个步骤，可以把输入数据减少为两个，即 $\begin{array}{c}{x}_d\\ y_d\end{array},$ 因此只需对这两个数据进行采集。

数据采集的过程如下：系统包含数据发送端和数据接收端，数据接收端即为履带式机器人。数据发送端将路径的数据，即 $\begin{array}{c}{x}_d\\ y_d\end{array},$ 通过无线方式传送到数据接收端的传感器，数据接收端的控制器检测到有数据传入传感器后，将数据从传感器取出，并存入存储器；然后再将数据送入数据接收端的控制器的计算单元，计算单元即可通过采集到的数据计算履带式机器人的输出。

在步骤105中，设定履带式机器人的方向为沿运动轨迹的切线方向，则调整履带式机器人的运动学方程为

$\left\{\begin{array}{c}{v}_1=\sqrt{x^{\&prime;2}+y^{\&prime;2}}+\frac{H\&CenterDot;x^{\&prime;2}}{2(x^{\&prime;2}+y^{\&prime;2})}\\ v_2=\sqrt{x^{\&prime;2}+y^{\&prime;2}}-\frac{H\&CenterDot;x^{\&prime;2}}{2(x^{\&prime;2}+y^{\&prime;2})}\\ \mathrm{\&theta;}=\arctan \left(\frac{y^{\&prime;}}{x^{\&prime;}}\right)\end{array}\right.;$

在步骤106中，履带式机器人的控制函数矩阵为

$\left[\begin{array}{c}{x}^{\&prime;\&prime;}\\ y^{\&prime;\&prime;}\\ \mathrm{\&theta;}\\ v_1\\ v_2\end{array}\right]=\left[\begin{array}{c}{x_d}^{\&prime;\&prime;}-{\mathrm{\&lambda;}}_1\&CenterDot;(x^{\&prime;}-{x_d}^{\&prime;})+\frac{k_1}{{\mathrm{\&alpha;}}_1+(1-{\mathrm{\&alpha;}}_1)\&CenterDot;e^{-\left|s_1\right|}}\&CenterDot;\mathrm{sat}({\mathrm{\&lambda;}}_1\&CenterDot;(x-x_d)+(x^{\&prime;}-{x_d}^{\&prime;}))\\ {y_d}^{\&prime;\&prime;}-{\mathrm{\&lambda;}}_2\&CenterDot;(y^{\&prime;}-{y_d}^{\&prime;})+\frac{k_2}{{\mathrm{\&alpha;}}_2+(1-{\mathrm{\&alpha;}}_2)\&CenterDot;e^{-\left|s_2\right|}}\&CenterDot;\mathrm{sat}({\mathrm{\&lambda;}}_2\&CenterDot;(y-y_d)+(y^{\&prime;}-{y_d}^{\&prime;}))\\ \arctan \left(\frac{y^{\&prime;}}{x^{\&prime;}}\right)\\ \sqrt{x^{\&prime;2}+y^{\&prime;2}}+\frac{H\&CenterDot;x^{\&prime;2}}{2(x^{\&prime;2}+y^{\&prime;2})}\\ \sqrt{x^{\&prime;2}+y^{\&prime;2}}+\frac{H\&CenterDot;x^{\&prime;2}}{2(x^{\&prime;2}+y^{\&prime;2})}\end{array}\right].$

本实施例提供的履带式机器人的轨迹跟踪方法，首先建立基于轨迹跟踪的初始运动学模型，再针对建立的模型，设计滑模切换函数和初始输出控制律，然后结合实际应用对运动学方程进行调整，同时对滑模控制律进行改进，以指数趋近律替代常规趋近律，这样能以减小抖振，加快系统到达理想状态的时间，提高系统的稳定性，最后将基于指数趋近律的滑模控制律应用于履带式机器人的轨迹控制中，计算出最后的履带式机器人的控制函数矩阵。其能根据路径规划，实现对机器人轨迹的智能控制，减少驾驶员的操作难度，提高履带式机器人在复杂地形的越野能力。

以上技术特征构成了本发明的最佳实施例，其具有较强的适应性和最佳实施效果，可根据实际需要增减非必要的技术特征，来满足不同情况的需求。

Claims (4)

1.一种履带式机器人的轨迹跟踪方法，其特征在于按照以下步骤进行：

步骤101，在平面二维坐标系中，建立履带式机器人的初始运动学方程；

步骤102，设定切换函数，计算出基于常规趋近律的控制函数；

步骤103，使用基于切换函数的指数增益替换常量增益，计算出基于指数趋近律的滑模控制律；

步骤104，对每一组输入数据和每一组输出数据使用基于指数趋近律的滑模控制律，建立履带式机器人的初始控制函数矩阵；

步骤105，调整履带式机器人的方向为沿运动轨迹的切线方向；

步骤106，使用基于指数趋近律的滑模控制律，计算出履带式机器人的控制函数矩阵。

2.根据权利要求1所述的履带式机器人的轨迹跟踪方法，其特征在于在步骤101中，用x表示履带式机器人在平面二维坐标系中的横坐标，y表示履带式机器人在平面二维坐标系中的纵坐标，用x′表示履带式机器人沿横坐标的速度，y′表示履带式机器人沿纵坐标的速度，在平面二维坐标系中，建立履带式机器人的初始运动学方程：

$\left\{\begin{array}{c}{x}^{\&prime;}=(v_1+v_2)/2\cos \left(\mathrm{\&theta;}\right)\\ y^{\&prime;}=(v_1+v_2)/2\sin \left(\mathrm{\&theta;}\right)\\ \mathrm{\&omega;}\&CenterDot;\mathrm{\&Delta;t}\&CenterDot;H=v_1\&CenterDot;\mathrm{\&Delta;t}-v_2\&CenterDot;\mathrm{\&Delta;t}\\ {\mathrm{\&theta;}}^{\&prime;}=\mathrm{\&omega;}=(v_1+v_2)/H\\ v_1=\frac{x^{\&prime;}}{\cos \left(\mathrm{\&theta;}\right)}+\frac{H\&CenterDot;{\mathrm{\&theta;}}^{\&prime;}}{2}\\ v_2=\frac{x^{\&prime;}}{\cos \left(\mathrm{\&theta;}\right)}-\frac{H\&CenterDot;{\mathrm{\&theta;}}^{\&prime;}}{2}\end{array}\right.$

其中，H表示履带式机器人的宽度，v₁和v₂分别表示左右履带轮的速度，θ表示履带式机器人与横轴的夹角，w或θ′表示角速度，Δt为一短时间，系统的输入变量为x_d和y_d，输出变量为x，y，θ，v₁，v₂；

在步骤102中，设定切换函数为s＝(x″-x_d″)+λ(x′-x_d′)，采用滑模常规趋近律s＝-kgsat(s)，则基于常规趋近律的滑模控制函数为：

(x″-x_d″)+λ(x′-x_d′)＝-k·sat((x′-x_d′)+λ(x-x_d))

其中，x和x_d分别表示实际轨迹变量和参考轨迹变量，sat()为饱和函数 $\left(\mathrm{sat}\left(x\right)\left\{\begin{array}{cc}1& x\&GreaterEqual;1\\ x& -1<x<1\\ -1& x\&le;-1\end{array}\right.\right),$ λ为切换函数系数，k为控制律的增益；

在步骤103中，以基于切换函数的指数增益替换常规趋近律的常量增益k，建立基于指数趋近律的滑模控制函数：

$(x^{\&prime;\&prime;}-{x_d}^{\&prime;\&prime;})+\mathrm{\&lambda;}(x^{\&prime;}-{x_d}^{\&prime;})=-\frac{k}{\mathrm{\&alpha;}+(1-\mathrm{\&alpha;})\&CenterDot;e^{-\left|s\right|}}\&CenterDot;\mathrm{sat}((x^{\&prime;}-{x_d}^{\&prime;})+\mathrm{\&lambda;}(x-x_d))$

其中，s为切换函数，α为指数趋近律的控制系数；

在步骤104中，对履带式机器人的每一个运动状态都使用基于指数趋近律的滑模控制函数，可列出初始的控制函数矩阵：

$\left[\begin{array}{c}{x}^{\&prime;\&prime;}\\ y^{\&prime;\&prime;}\\ {\mathrm{\&theta;}}^{\&prime;\&prime;}\\ {v_1}^{\&prime;\&prime;}\\ {v_2}^{\&prime;\&prime;}\end{array}\right]=\left[\begin{array}{c}{x_d}^{\&prime;\&prime;}\\ {y_d}^{\&prime;\&prime;}\\ {{\mathrm{\&theta;}}_d}^{\&prime;\&prime;}\\ {v_{d1}}^{\&prime;\&prime;}\\ {v_{d2}}^{\&prime;\&prime;}\end{array}\right]-\left[\begin{array}{c}{\mathrm{\&lambda;}}_1\&CenterDot;(x^{\&prime;}-{x_d}^{\&prime;})\\ {\mathrm{\&lambda;}}_2\&CenterDot;(y^{\&prime;}-{y_d}^{\&prime;})\\ {\mathrm{\&lambda;}}_3\&CenterDot;({\mathrm{\&theta;}}^{\&prime;}-{{\mathrm{\&theta;}}_d}^{\&prime;})\\ {\mathrm{\&lambda;}}_4\&CenterDot;({v_1}^{\&prime;}-{v_{d1}}^{\&prime;})\\ {\mathrm{\&lambda;}}_5\&CenterDot;({v_2}^{\&prime;}-{v_{d2}}^{\&prime;})\end{array}\right]+\left[\begin{array}{c}\frac{k_1}{{\mathrm{\&alpha;}}_1+(1-{\mathrm{\&alpha;}}_1)\&CenterDot;e^{-\left|s_1\right|}}\&CenterDot;\mathrm{sat}({\mathrm{\&lambda;}}_1\&CenterDot;(x-x_d)+(x^{\&prime;}-{x_d}^{\&prime;}))\\ \frac{k_2}{{\mathrm{\&alpha;}}_2+(1-{\mathrm{\&alpha;}}_2)\&CenterDot;e^{-\left|s_2\right|}}\&CenterDot;\mathrm{sat}({\mathrm{\&lambda;}}_2\&CenterDot;(y-y_d)+(y^{\&prime;}-{y_d}^{\&prime;}))\\ \frac{k_3}{{\mathrm{\&alpha;}}_3+(1-{\mathrm{\&alpha;}}_3)\&CenterDot;e^{-\left|s_3\right|}}\&CenterDot;\mathrm{sat}({\mathrm{\&lambda;}}_3\&CenterDot;(\mathrm{\&theta;}-{\mathrm{\&theta;}}_d)+(\mathrm{\&theta;}-{{\mathrm{\&theta;}}_d}^{\&prime;}))\\ \frac{k_4}{{\mathrm{\&alpha;}}_4+(1-{\mathrm{\&alpha;}}_4)\&CenterDot;e^{\left|s_4\right|}}\&CenterDot;\mathrm{sat}({\mathrm{\&lambda;}}_4\&CenterDot;(v_1-v_{d1})+{v_1}^{\&prime;}-{v_{d1}}^{\&prime;})\\ \frac{k_5}{{\mathrm{\&alpha;}}_5+(1-{\mathrm{\&alpha;}}_5)\&CenterDot;e^{\left|s_5\right|}}\&CenterDot;\mathrm{sat}({\mathrm{\&lambda;}}_5\&CenterDot;(v_2-v_{d2})+({v_2}^{\&prime;}-{v_{d2}}^{\&prime;}))\end{array}\right]$

其中，x_d,y_d,θ_d,v_d1,v_d2分别表示横纵坐标、转角、左右履带轮速度的参考轨迹变量，x,y,θ,v₁,v₂则表示它们的实际轨迹变量；x′,y′,θ′,v₁′,v₂′和x_d′,y_d′,θ_d′,v_d1′,v_d2′表示他们的1阶导数；x″,y″,θ″,v₁″,v₂″和x_d″,y_d″,θ_d″,v_d1″,v_d2″表示他们的2阶导数；λ₁，λ₂，λ₃，λ₄，λ₅为对应控制律的切换函数系数；s₁，s₂，s₃，s₄，s₅为对应控制律的切换函数；k₁，k₂，k₃，k₄，k₅为对应控制律的增益系数；α₁，α₂，α₃，α₄，α₅为对应指数控制律的控制系数；

在步骤105中，设定履带式机器人的方向为沿运动轨迹的切线方向，则调整履带式机器人的运动学方程为

$\left\{\begin{array}{c}{v}_1=\sqrt{x^{\&prime;2}+y^{\&prime;2}}+\frac{H\&CenterDot;x^{\&prime;2}}{2(x^{\&prime;2}+y^{\&prime;2})}\\ v_2=\sqrt{x^{\&prime;2}+y^{\&prime;2}}-\frac{H\&CenterDot;x^{\&prime;2}}{2(x^{\&prime;2}+y^{\&prime;2})}\\ \mathrm{\&theta;}=\arctan \left(\frac{y^{\&prime;}}{x^{\&prime;}}\right)\end{array}\right.;$

在步骤106中，履带式机器人的控制函数矩阵为

$\left[\begin{array}{c}{x}^{\&prime;\&prime;}\\ y^{\&prime;\&prime;}\\ \mathrm{\&theta;}\\ v_1\\ v_2\end{array}\right]=\left[\begin{array}{c}{x_d}^{\&prime;\&prime;}-{\mathrm{\&lambda;}}_1\&CenterDot;(x^{\&prime;}-{x_d}^{\&prime;})+\frac{k_1}{{\mathrm{\&alpha;}}_1+(1-{\mathrm{\&alpha;}}_1)\&CenterDot;e^{-\left|s_1\right|}}\&CenterDot;\mathrm{sat}({\mathrm{\&lambda;}}_1\&CenterDot;(x-x_d)+(x^{\&prime;}-{x_d}^{\&prime;}))\\ {y_d}^{\&prime;\&prime;}-{\mathrm{\&lambda;}}_2\&CenterDot;(y^{\&prime;}-{y_d}^{\&prime;})+\frac{k_2}{{\mathrm{\&alpha;}}_2+(1-{\mathrm{\&alpha;}}_2)\&CenterDot;e^{-\left|s_2\right|}}\&CenterDot;\mathrm{sat}({\mathrm{\&lambda;}}_2\&CenterDot;(y-y_d)+(y^{\&prime;}-{y_d}^{\&prime;}))\\ \arctan \left(\frac{y^{\&prime;}}{x^{\&prime;}}\right)\\ \sqrt{x^{\&prime;2}+y^{\&prime;2}}+\frac{H\&CenterDot;x^{\&prime;2}}{2(x^{\&prime;2}+y^{\&prime;2})}\\ \sqrt{x^{\&prime;2}+y^{\&prime;2}}-\frac{H\&CenterDot;x^{\&prime;2}}{2(x^{\&prime;2}+y^{\&prime;2})}\end{array}\right].$

3.根据权利要求2所述的履带式机器人的轨迹跟踪方法，其特征在于在步骤104中，输入的数据为采集得到的数据，即 $\begin{array}{c}{x}_d\\ y_d\\ {\mathrm{\&theta;}}_d\\ v_{1d}\\ v_{2d}\end{array};$ 输出的数据为 $\begin{array}{c}x\\ y\\ \mathrm{\&theta;}\\ v_1\\ v_2\end{array}.$

4.根据权利要求3所述的履带式机器人的轨迹跟踪方法，其特征在于在步骤104中，输入的数据为采集得到的数据，即 $\begin{array}{c}{x}_d\\ y_d\end{array}.$