CN108153309B - 用于履带机器人的控制方法以及履带机器人 - Google Patents

Abstract

本发明涉及移动机器人运动控制领域，公开了一种用于履带机器人的控制方法以及履带机器人。本发明将履带机器人视为由电机驱动系统和车体运动系统组成的级联系统，构建变倾斜参数的自适应积分滑模切换函数，并根据自适应积分滑模切换函数提出基于等效控制和切换控制的自适应滑模跟踪控制，以机器人的速度，在线辨识所得的驱动电机时变不确定参数，以及在运动学模型中求取的与目标位姿的误差反馈至驱动系统的控制器中，然后根据运动学关系，分解各个电机的期望速度，进而实现机器人的稳定运动控制。

Description

用于履带机器人的控制方法以及履带机器人

技术领域

本发明涉及移动机器人运动控制，具体地，涉及一种用于履带机器人的控制方法以及履带机器人。

背景技术

随着农用履带机器人(Agricultural Tracked Robot,ATR)的广泛应用，对机器人系统的自适应性、控制的精确性和运动的平稳性都提出了更高的要求。然而农田地形复杂，环境多变，加上ATR系统本身存在的强耦合和模型不确定的特性，使得ATR控制系统的设计与使用难度增大。

为此，国内外学者对其进行了深入研究，有的学者提出了各种轨迹跟踪控制，主要是利用运动学模型，或运动学或动力学结合的模型，有些学者提出了线性反馈方法、PID控制法、计算力矩法、反步法、滑模控制、神经网络控制法和模糊控制等等。其中线性反馈方法是一种常用的控制方法，由于ATR模型是非线性的，因此控制精度较低；PID控制法，由于其控制参数固定，对于非线性和结构不确定系统的控制效果较差；计算力矩法依赖于被控对象的动力学模型，而动力学建模本身就十分复杂，且构建困难，因此该方法的理论和实践意义不大；神经网络法虽然能够克服系统的不确定性和未知扰动，但控制算法复杂；模糊控制不需要建立精确的数学模型，适合非线性时变、滞后系统的控制，但模糊规则的选择缺乏系统性，难以在线调整；反步法中运用了虚拟控制量，同时进行迭代求导，使得控制器结构十分复杂，工程实现难度较大。

农田环境的复杂性使得ATR运动过程中具有诸多不确定性，比如：参数摄动和负载扰动以及传感器的测量误差，都会引起ATR运行轨迹偏离参考路径。常规的控制方法难以满足高精度轨迹跟踪控制要求。模变结构控制具有快速瞬态响应的鲁棒性，不依赖被控对象精确的数学模型、对参数和环境变化不敏感和工程实现简单的优点，因此适用于农田环境的机器人的控制。

发明内容

本发明的目的是提供一种用于履带机器人的控制方法，该方法通过建立自适应滑模跟踪控制模型实现对履带机器人的运动轨迹的控制，提高了控制精度。

为了实现上述目的，本发明提供一种用于履带机器人的控制方法，包括以下步骤：

获取履带机器人的当前状态下的位姿；

设定履带机器人的参考轨迹，参考轨迹包括位姿指令和速度指令；

建立描述履带机器人的位姿与履带机器人的速度之间的约束关系的运动学模型，速度包括角速度和线速度；

根据当前状态下的位姿和设定的参考轨迹，建立履带机器人的位姿误差模型；

根据运动学模型和位姿误差模型建立履带机器人的位姿误差微分模型；

建立履带机器人的用于驱动左驱动轮的左电机和用于驱动右驱动轮的右电机的驱动模型，驱动模型包括力矩驱动模型和电势平衡模型；

根据力矩驱动模型和电势平衡模型获得左电机和右电机的动态模型；

建立随参数调整而变化的自适应滑模切换模型；

根据位姿误差微分模型和自适应滑模切换模型获得履带机器人的期望速度，期望速度包括期望线速度和期望角速度；

根据履带机器人的期望速度获得左电机和右电机的期望角速度。

优选地，控制方法还包括：

建立用于修正履带机器人的期望速度的切换控制模型；

采用切换控制模型修正履带机器人的期望速度以及获得履带机器人的修正后的期望速度；

根据修正后的期望速度获得左电机和右电机的期望角速度；

根据左电机和右电机的期望角速度以及左电机和右电机的动态模型计算左电机和右电机的驱动电压。

优选地，描述履带机器人的位姿与履带机器人的速度之间的约束关系的运动学模型采用式(1)表示：

其中，x、y分别为履带机器人的质心在XOY坐标系中的位置坐标，θ为履带机器人的运动方向和X轴的夹角，v、ω分别为履带机器人的线速度和角速度，d为履带机器人的质心和几何中心之间的距离，

和

分别为x、y和θ对时间的导数；

根据履带机器人的当前状态下的位姿和设定的参考轨迹，建立的履带机器人的位姿误差模型采用式(2)表示：

其中，(x,y,θ)^T为履带机器人当前状态下的位姿，x、y分别为履带机器人的质心的当前位置的坐标，θ为履带机器人在当前状态下其运动方向与X轴的夹角，(x_r,y_r,θ_r)^T为位姿指令，x_r、y_r分别为履带机器人的质心的目标位置的坐标，θ_r为履带机器人到达目标位置时其运动方向与X轴的夹角，x_e为履带机器人的质心的当前位置和目标位置沿其当前运动方向的误差值，y_e为履带机器人的质心的当前位置和目标位置在与其当前运动方向垂直方向的误差值，θ_e为θ和θ_r之间的误差值；

根据运动学模型和位姿误差模型建立的履带机器人的位姿误差微分模型：

其中，

分别为x_e、y_e和θ_e对时间的导数，v和ω分别为履带机器人在当前状态下的线速度和角速度，(v_r,ω_r)^T为速度指令，v_r和ω_r分别为履带机器人到达目标位置时的线速度和角速度，d为履带机器人的质心和几何中心之间的距离。

优选地，右电机和左电机的力矩平衡模型分别采用式(4)和式(5)表示：

其中，J_r(t)、J_l(t)分别为右电机和左电机的转轴的转动惯量，F为左电机和右电机的输出轴上的粘性摩擦系数，k_t为左电机和右电机的电磁转矩系数，T_dr(t)、T_dl(t)分别为右电机和左电机受到的干扰力矩，ω_r(t)、ω_l(t)分别为左电机和右电机的转轴转动的角速度，

和

分别为ω_r(t)和ω_l(t)对时间的导数，i_r(t)、i_l(t)分别为右电机和左电机的电枢电流；

右电机和左电机的电势平衡模型分别采用式(6)和式(7)表示：

其中，L为左电机和右电机的电枢电感，R为左电机和右电机的电枢电阻，k_e为左电机和右电机的反电动势系数，且k_e＝0.10472k_t，k_t为左电机和右电机的电磁转矩系数，

和

分别为i_r(t)和i_l(t)对时间的导数，u_r(t)和u_l(t)分别为右电机的驱动电压和左电机的驱动电压；

根据力矩驱动模型和电势平衡模型获得的右电机和左电机的动态模型采用式(8)和式(9)表示：

其中，T_l(t)＝RJ_l(t)/(RF+k_Tk_e)，T_r(t)＝RJ_r(t)/(RF+k_Tk_e)，

k₁＝k_t/(RF+k_Tk_e)，k₂＝R/(RF+k_Tk_e)，R为左电机和右电机的电枢电阻，J_r(t)、J_l(t)分别为右电机和左电机的转轴的转动惯量，F为左电机和右电机的输出轴上的粘性摩擦系数，k_t为左电机和右电机的电磁转矩系数，T_dr(t)、T_dl(t)分别为右电机和左电机受到的干扰力矩。

优选地，建立的随参数调整而变化的自适应滑模切换模型采用式(10)表示：

其中，α₁和α₂为倾斜参数，

c₁、c₂、c₃、c₄、k_k1、k_k2均为正常数，s₁和s₂分别为关于x_e和θ_e的切换函数；

优选地，根据位姿误差微分模型和自适应滑模切换模型获得的履带机器人的期望速度采用式(11)表示：

其中，v_d和ω_d分别为履带机器人的期望线速度和期望角速度，。

优选地，切换控制模型采用式(12)表示：

其中，β₁、β₂为大于零的切换增益，β₁、β₂、Δ₁和Δ₂均为经验值，sat为饱和函数；

履带机器人的修正后的期望速度采用式(13)表示：

其中，v′_d和ω′_d分别为履带机器人的修正后的期望线速度和修正后的期望角速度，

为v_r对时间的导数，β₁、β₂为大于零的切换增益，β₁、β₂、Δ₁和Δ₂均为经验值，sat为饱和函数，；

右电机和左电机的期望角速度分别采用式(14)和式(15)表示：

ω_rd＝(v′_d+ω′_dA)r^-1 式(14)

ω_ld＝(v′_d-ω′_dA)r^-1 式(15)

其中，ω_rd和ω_ld分别为右电机的期望角速度和左电机的期望角速度，A为左驱动轮和右驱动轮之间的间距的一半，r为左驱动轮和右驱动轮的半径。

在另一方面，本发明的实施方式还提供了一种履带机器人，该履带机器人包括：左驱动轮，用于驱动左履带；右驱动轮，用于驱动右履带；左电机，用于驱动左驱动轮；右电机，用于驱动右驱动轮；传感器，用于检测履带机器人的当前状态下的位姿，该位姿包括履带机器人在指定坐标系中的位置和倾斜角度；以及控制器，用于执行上述的用于履带机器人的控制方法。

通过上述技术方案，本发明将履带机器人视为由电机驱动系统和车体运动系统组成的级联系统，构建变倾斜参数的自适应积分滑模切换函数，并根据自适应积分滑模切换函数提出基于等效控制和切换控制的自适应滑模跟踪控制，以机器人的速度，在线辨识所得的驱动电机时变不确定参数，以及在运动学模型中求取的与目标位姿的误差反馈至驱动系统的控制器中，然后根据运动学关系，分解各个电机的期望速度，进而实现机器人的稳定运动控制。

本发明的其它特征和优点将在随后的具体实施方式部分予以详细说明。

附图说明

附图是用来提供对本发明的进一步理解，并且构成说明书的一部分，与下面的具体实施方式一起用于解释本发明，但并不构成对本发明的限制。在附图中：

图1是根据本发明的一实施方式的用于履带机器人的控制方法的流程图；

图2是根据本发明的一实施方式的用于履带机器人的控制方法的流程图；

图3示出了本发明的一实施方式的履带机器人的模型图；

图4示出了本发明的一实施方式的履带机器人的位姿误差模型图；

图5示出了左电机的角速度的响应曲线；

图6示出了左电机的角速度的跟踪误差曲线；

图7示出了左电机的电压输出曲线；

图8示出了履带机器人的折线运动轨迹；

图9示出了履带机器人的折线运动轨迹的跟踪误差曲线；

图10示出了左电机和右电机的角速度响应曲线；

图11示出了履带机器人的圆周运动轨迹；

图12示出了履带机器人的圆周运动轨迹的跟踪误差曲线；

图13示出了左电机和右电机的角速度响应曲线；

图14示出了履带机器人在采用ASMTC控制方法时的运动轨迹；以及

图15示出了履带机器人在采用ASMTC控制方法时的运动轨迹的位姿误差曲线。

具体实施方式

以下结合附图对本发明的具体实施方式进行详细说明。应当理解的是，此处所描述的具体实施方式仅用于说明和解释本发明，并不用于限制本发明。

图1是根据本发明的一实施方式的用于履带机器人的控制方法的流程图。如图1所示，本发明的一实施方式提供了一种用于履带机器人的控制方法，该控制方法可以包括以下步骤：

在步骤S101中，获取履带机器人的当前状态下的位姿；

在步骤S102中，设定履带机器人的参考轨迹，参考轨迹包括位姿指令和速度指令；

在步骤S103中，建立描述履带机器人的位姿与履带机器人的速度之间的约束关系的运动学模型，速度包括角速度和线速度；

在步骤S104中，根据当前状态下的位姿和设定的参考轨迹，建立履带机器人的位姿误差模型；

在步骤S105中，根据运动学模型和位姿误差模型建立履带机器人的位姿误差微分模型；

在步骤S106中，建立履带机器人的用于驱动左驱动轮的左电机和用于驱动右驱动轮的右电机的驱动模型，驱动模型包括力矩驱动模型和电势平衡模型；

在步骤S107中，根据力矩驱动模型和电势平衡模型获得左电机和右电机的动态模型；

在步骤S108中，建立随参数调整而变化的自适应滑模切换模型；

在步骤S109中，根据位姿误差微分模型和自适应滑模切换模型获得履带机器人的期望速度，期望速度包括期望线速度和期望角速度。

在步骤S110中，根据履带机器人的期望速度获得左电机和右电机的期望角速度。

图3示出了本发明的一实施方式的履带机器人的模型图。如图3所示，履带机器人的位姿是指履带机器人在XOY坐标系中的位置和姿态(倾斜程度)，在本发明中采用p＝(x,y,θ)^T表示履带机器人的位姿。

履带机器人的参考轨迹是指履带机器人所要到达的指定位置以及到达指定位置时履带机器人的倾斜角度、运行的线速度和角速度。

如图3所示，点m为履带机器人的质心，点O_a为履带机器人的几何中心，描述履带机器人的位姿与履带机器人的速度之间的约束关系的运动学模型例如可以采用式(1)表示：

其中，x、y分别为履带机器人的质心在XOY坐标系中的位置坐标，θ为履带机器人的运动方向和X轴的夹角，v、ω分别为履带机器人的线速度和角速度，d为履带机器人的质心和几何中心之间的距离，

和

分别为x、y和θ对时间的导数。

图4示出了本发明的一实施方式的履带机器人的位姿误差模型图。如图4所示，根据履带机器人的当前状态下的位姿和设定的参考轨迹，建立的履带机器人的位姿误差模型例如可以采用式(2)表示：

其中，(x,y,θ)^T为履带机器人当前状态下的位姿，x、y分别为履带机器人的质心的当前位置的坐标，θ为履带机器人在当前状态下其运动方向与X轴的夹角，(x_r,y_r,θ_r)^T为位姿指令，x_r、y_r分别为履带机器人的质心的目标位置的坐标，θ_r为履带机器人到达目标位置时其运动方向与X轴的夹角，x_e为履带机器人的质心的当前位置和目标位置沿其当前运动方向的误差值，y_e为履带机器人的质心的当前位置和目标位置在与其当前运动方向垂直方向的误差值，θ_e为θ和θ_r之间的误差值。

将式(2)微分，结合式(1)获得履带机器人的位姿误差微分模型，该位姿误差微分模型例如可以采用式(3)表示：

其中，

分别为x_e、y_e和θ_e对时间的导数，v和ω分别为履带机器人在当前状态下的线速度和角速度，(v_r,ω_r)^T为速度指令，v_r和ω_r分别为履带机器人到达目标位置时的线速度和角速度。

履带机器人的驱动执行器是用于驱动左驱动轮的左电机和用于驱动右驱动轮的右电机，左电机和右电机例如可以是直流电机。在不同的路况下，根据不同的控制目标，左电机和右电机协调转动，驱动履带机器人运动，因此履带机器人的运动控制是左电机和右电机的协调控制。

右电机和左电机的力矩平衡模型例如可以分别采用式(4)和式(5)表示：

其中，J_r(t)、J_l(t)分别为右电机和左电机的转轴的转动惯量，F为左电机和右电机的输出轴上的粘性摩擦系数，k_t为左电机和右电机的电磁转矩系数，T_dr(t)、T_dl(t)分别为右电机和左电机受到的干扰力矩，ω_r(t)、ω_l(t)分别为右电机和左电机的转轴转动的角速度，

和

分别为ω_r(t)和ω_l(t)对时间的导数，i_r(t)、i_l(t)分别为右电机和左电机的电枢电流。

右电机和左电机的电势平衡模型例如可以分别采用式(6)和式(7)表示：

其中，L为左电机和右电机的电枢电感，R为左电机和右电机的电枢电阻，k_e为左电机和右电机的反电动势系数，且k_e＝0.10472k_t，k_t为左电机和右电机的电磁转矩系数，ω_r(t)、ω_l(t)分别为右电机和左电机的转轴转动的角速度，i_r(t)、i_l(t)分别为右电机和左电机的电枢电流，

和

分别为i_r(t)和i_l(t)对时间的导数，u_r(t)和u_l(t)分别为右电机的驱动电压和左电机的驱动电压。

由于左电机和右电机为履带机器人的执行机构，响应速度快，在忽略电机的电枢电感的情况下，右电机和左电机的动态模型例如可以采用式(8)和式(9)表示：

其中，T_l(t)＝RJ_l(t)/(RF+k_tk_e)，T_r(t)＝RJ_r(t)/(RF+k_tk_e)，

k₁＝k_t/(RF+k_tk_e)，k₂＝R/(RF+k_tk_e)，R为左电机和右电机的电枢电阻，J_r(t)、J_l(t)分别为右电机和左电机的转轴的转动惯量，F为左电机和右电机的输出轴上的粘性摩擦系数，k_t为左电机和右电机的电磁转矩系数，T_dr(t)、T_dl(t)分别为左电机和右电机受到的干扰力矩。

由于位姿误差微分模型的多输入、非线性的特点，在本发明的一实施方式中，建立了随参数调整而变化的自适应滑模切换模型，使得在位姿误差较大的情况下，能够起到限制积分项的作用，在位姿误差较小的情况下，具有一定的放大作用，提高控制精度。

随参数调整而变化的自适应滑模切换模型例如可以采用式(10)表示：

其中，α₁和α₂为倾斜参数，

v_r为履带机器人到达目标位置时的线速度，c₁、c₂、c₃、c₄、k_k1、k_k2均为正常数，s₁和s₂分别为关于x_e和θ_e的切换函数；

对式(10)求时间的导数可以得到式(11)：

其中，，

和

分别为s₁和s₂对时间的导数。

将式(3)带入式(11)可以得到式(12)：

其中，

为v_r对时间的导数，。

令式(12)等于零可以得到履带机器人的期望速度，履带机器人的期望速度例如可以采用式(13)表示：

其中，v_d和ω_d分别为履带机器人的期望线速度和期望角速度。

图2是根据本发明的一实施方式的用于履带机器人的控制方法的流程图。如图2所示，在本发明的一实施方式中提供了一种用于履带机器人的控制方法，图2所示的控制方法与图1所示的控制方法相比还可以包括以下步骤：

在步骤S210中，建立用于修正履带机器人的期望速度的切换控制模型；

在步骤S211中，采用切换控制模型修正履带机器人的期望速度以及获得履带机器人的修正后的期望速度；

在步骤S212中，根据修正后的期望速度获得左电机和所述右电机的期望角速度；

在步骤S213中，根据左电机和右电机的期望角速度以及左电机和右电机的动态模型计算左电机和右电机的驱动电压。

在本发明的一实施方式中，用于修正履带机器人的期望速度的切换控制模型例如可以采用式(12)表示：

其中，β₁、β₂为大于零的切换增益，β₁、β₂、Δ₁和Δ₂均为经验值，sat为饱和函数，s₁和s₂分别为关于x_e和θ_e的切换函数；；

结合式(13)履带机器人的修正后的期望速度采用式(15)表示：

其中，v′_d和ω′_d分别为履带机器人的修正后的期望线速度和修正后的期望角速度；

右电机和左电机的期望角速度分别采用式(14)和式(15)表示：

ω_rd＝(v′_d+ω′_dA)r^-1 式(14)

ω_ld＝(v′_d-ω′_dA)r^-1 式(15)

其中，ω_rd和ω_ld分别为右电机的期望角速度和左电机的期望角速度，A为左驱动轮和右驱动轮之间的间距的一半，r为左驱动轮和所述右驱动轮的半径。

β₁和β₂的值可以取为0.01％，Δ₁的值可以取为±0.01％，Δ₂的值可以取为±0.01％。

将右电机的期望角速度ω_rd和左电机的期望角速度ω_ld分别带入左电机动态模型式(8)和右电机的动态模型式(9)，可以计算得到应当施加给右电机的驱动电压u_r(t)和应当施加给左电机的驱动电压u_l(t)。

虽然在图1和图2中以特定的顺序示出了用于履带机器人的控制方法的特定的步骤，但是本领域技术人员可以理解，除非在逻辑上必须存在先后关系，否则控制方法不必必须按照图中示出的步骤来执行。

本发明的一实施方式还提供一种履带机器人，该履带机器人可以包括：

左驱动轮，用于驱动左履带；右驱动轮，用于驱动右履带；

左电机，用于驱动所述左驱动轮；右电机，用于驱动所述右驱动轮；

传感器，用于检测履带机器人的当前状态下的位姿，该位姿包括履带机器人在指定坐标系中的位置和倾斜角度；以及

控制器，用于执行上述任意实施方式中的用于履带机器人的控制方法。

为了验证本发明的实施方式的用于履带机器人的控制方法的有效性，本发明提供以下几个实施例。

在本发明的一实施例中，履带机器人的左电机和右电机的各个参数例如可以为：J_r＝J_l＝0.155kg·m²，L＝0.45·10^-3h，k_e＝0.265V/rad，k_t＝2.52nm/A,R＝3.68Ω,F＝0.001，k₁＝2，k₂＝2,左电机和右电机的控制电压u满足|u|≤15V。履带机器人的各个参数例如可以为：履带机器人的长度l＝0.555m，宽度w＝0.4m，左驱动轮和右驱动轮的半径r＝0.1m，左驱动轮和右驱动轮之间的间距2A＝0.36m，白噪声干扰d(t)＝50×randn(1，1)。控制器的各个参数例如可以为：k_k1＝k_k2＝2，c₁＝c₂＝2，c₃＝c₄＝4，k_ω1＝k_ω2＝1000,采样时间为50ms。

对左电机和电机的控制的验证

分别采用本发明的实施方式的控制方法(以下及附图中简称ASMTC控制方法)和常规指数趋近律的滑模控制(Sliding Mode Control,以下及附图中简称SMC控制方法)对履带机器人进行仿真计算，控制履带机器人以线速度v＝2m/s和角速度ω＝0的速度直线运行。

图5示出了左电机的角速度的响应曲线，图6示出了左电机的角速度的跟踪误差曲线，图7示出了左电机的电压输出曲线。由于左驱动轮和右驱动轮的仿真计算条件、参数和模型完全相同，在履带机器人直线运行的情况下，右驱动轮的仿真结果和左驱动轮完全相同。如图5、图6所示，采用本发明的实施方式的控制方法对左电机和右电机进行控制时，左电机的角速度响应曲线平滑，且在0.375s时达到稳定状态，左电机输出的角速度的终止值为20rad/s，角速度跟踪误差收敛到零；而采用SMC方法进行控制时，履带机器人需要0.75s才能达到稳定状态，且有抖振现象。由图7可知，与SMC方法比较，本发明得ASMTC方法输出的电压曲线较光滑，抖振幅值不大于0.01V。由于滑模切换函数中变倾斜参数积分项的饱和特性，在履带机器人出现较大误差的情况时，能够限制积分项作用，使系统不出现过大的超调；在误差较小时有放大作用，在不引起抖振的情况下改善控制精度。

不同轨迹滑模跟踪控制仿真计算

本发明提供如下的对履带机器人进行折线和圆轨迹跟踪的仿真计算的实施例。

(a)折线轨迹滑模跟踪控制

在本发明的一实施例中，以折线路径为仿真计算路径，履带机器人的位姿指令为[0,0,pi/4]^T，履带机器人的初始位姿为[-2,-2,pi/4]^T，恒定线速度为2m/s，仿真计算的时间为0<t<12s。

图8示出了履带机器人的折线运动轨迹，图9示出了履带机器人的折线运动轨迹的跟踪误差曲线，图10示出了左电机和右电机的角速度响应曲线。由图8和图9可见，履带机器人从初始位置启动，机履带器人的位姿误差能够在较短的时间内收敛到0，在8<t<10s的时间段内，虽然参考路径的曲率变化较大，但是控制器仍然能够较快地跟踪参考路径，稳定后的位姿误差为：x_e在(-0.08,0.04)区间内，y_e在(-0.07,0.07)区间内，θ_e在(-0.02,0.045)区间内。由图10可知，左电机/右电机具有较快的响应速度，达到期望角速度后能保持平稳。

(b)圆轨迹滑模跟踪控制

在本发明的一实施例中，以半径为10m的圆形路径为仿真计算路径，以2m/s的恒定线速度匀速跟踪圆形路径，参考轨迹为：x＝10cosθ，y＝10sinθ。

履带机器人的初始位姿为[10,0,pi/2]^T，位姿指令为[7,0,pi/2]^T，左驱动轮和右驱动轮的角速度分别为10rad/s,30rad/s，恒定线速度为2m/s，逆时针方向运行，仿真计算时间为0<t<32s。

图11示出了履带机器人的圆周运动轨迹，图12示出了履带机器人的圆周运动轨迹的跟踪误差曲线，图13示出了左电机和右电机的角速度响应曲线。如图11至图13所示，采用本发明的实施方式的控制方法，能够使得左电机和右电机的角速度快速响应，达到期望速度后保持平稳。履带机器人能较好地跟踪所设计的圆周轨迹，位姿误差趋于0。特别是在跟踪圆形路径时，路径曲率时刻变化，采用ASMTC控制方法能及时调整输出控制，输出的左电机和右电机的角速度曲线较为光滑，保证跟踪不脱离参考轨迹，控制精度高。

滑模跟踪控制实验

在本发明的一实施例中，运用本发明的实施方式的履带机器人进行野外控制实验，采用型号为S3C2440的微控制器作为履带机器人的控制器，实验地面条件是沙瓤土和杂草混杂的农田。将组合导航定位系统SPAN-CPT作为履带机器人的状态信息的接收设备，SPAN-CPT安装在履带机器人上，其信息更新率为10hz，速度精度为0.01m/s，角度精度为0.02rad，位置测量精度为0.01m，履带机器人的跟踪轨迹路径为：

履带机器人的运行线速度为2m/s，

初始位姿为：[x(0) y(0) θ(0)]^T＝[0 20 pi/12]^T，

位姿指令为：[x_r(0) y_r(0) θ_r(0)]^T＝[10 40 pi/4]^T，

初始位姿误差为：[x_e(0) y_e(0) θ_e(0)]^T＝[10 20 pi/6]^T。

图14示出了履带机器人在采用ASMTC控制方法时的运动轨迹，除初始位置和跟踪轨迹曲率变化较大的区域，该运动轨迹较为平滑。图15示出了履带机器人在采用ASMTC控制方法时的运动轨迹的位姿误差曲线。从图15中可以看出，在履带机器人运动的初始阶段，由于履带机器人的初始位姿与位姿指令不一致，使得初始位姿偏差较大，在39-50s和79-90s的时间段内，由于路径曲率变化较大，机械转向幅度较大，履带机器人受到的的侧滑和离心力影响也较为严重，产生较为严重的参数摄动和外界干扰，引起较大的位姿误差，所产生的位姿参数误差范围分别为：-0.03≤x_e≤0.04m，-0.08≤y_e≤0.06m，-0.03≤y_e≤0.05rad。履带机器人运行在曲率变化较小的区域时，跟踪轨迹十分平滑，与参考曲线之间的偏差接近零。

表1示出了在相同的实验条件，运行的线速度不同时，履带机器人对同一轨迹跟踪时产生的位姿误差。由表1可知，当履带机器人在其运行线速度分别为1m/s,3m/s和4m/s的条件下，沿规定曲线运行时，位姿误差会快速减小，且接近于零，满足控制精度的要求。

表1低速条件下的轨迹跟踪位姿误差

通过上述实施方式，本发明将履带机器人视为由电机驱动系统和车体运动系统组成的级联系统，构建变倾斜参数的自适应积分滑模切换函数，并根据自适应积分滑模切换函数函数提出基于等效控制和切换控制的自适应滑模跟踪控制，以机器人的速度，在线辨识所得的驱动电机时变不确定参数，以及在运动学模型中求取的与目标位姿的误差反馈至驱动系统的控制器中，然后根据运动学关系，分解各个电机的期望速度，进而实现机器人的稳定运动控制。

以上结合附图详细描述了本发明的优选实施方式，但是，本发明并不限于上述实施方式，在本发明的技术构思范围内，可以对本发明的技术方案进行多种变型，这些简单变型均属于本发明的保护范围。

另外需要说明的是，在上述具体实施方式中所描述的各个具体技术特征，在不矛盾的情况下，可以通过任何合适的方式进行组合，为了避免不必要的重复，本发明对各种可能的组合方式不再另行说明。

此外，本发明的各种不同的实施方式之间也可以进行任意组合，只要其不违背本发明的思想，其同样应当视为本发明所公开的内容。

Claims (2)

1.用于履带机器人的控制方法，其特征在于，包括以下步骤：

获取履带机器人的当前状态下的位姿；

设定所述履带机器人的参考轨迹，所述参考轨迹包括位姿指令和速度指令；

建立描述所述履带机器人的位姿与所述履带机器人的速度之间的约束关系的运动学模型，所述速度包括线速度和角速度；

根据所述当前状态下的位姿和设定的参考轨迹，建立所述履带机器人的位姿误差模型；

根据所述运动学模型和所述位姿误差模型建立所述履带机器人的位姿误差微分模型；

建立所述履带机器人的用于驱动左驱动轮的左电机和用于驱动右驱动轮的右电机的驱动模型，所述驱动模型包括力矩驱动模型和电势平衡模型；

根据所述力矩驱动模型和电势平衡模型获得所述左电机和所述右电机的动态模型；

建立随参数调整而变化的自适应滑模切换模型；

根据所述位姿误差微分模型和所述自适应滑模切换模型获得所述履带机器人的期望速度；

所述控制方法还包括：

建立用于修正所述履带机器人的期望速度的切换控制模型；

采用所述切换控制模型修正所述履带机器人的期望速度以及获得所述履带机器人的修正后的期望速度；

根据所述修正后的期望速度获得所述左电机和所述右电机的期望角速度；

根据所述左电机和所述右电机的期望角速度以及所述左电机和所述右电机的动态模型计算所述左电机和所述右电机的驱动电压；

描述履带机器人的位姿与所述履带机器人的速度之间的约束关系的运动学模型采用式(1)表示：

其中，x、y分别为所述履带机器人的质心在XOY坐标系中的位置坐标，θ为所述履带机器人的运动方向和X轴的夹角，v、ω分别为所述履带机器人的线速度和角速度，d为所述履带机器人的质心和几何中心之间的距离，

和

分别为x、y和θ对时间的导数；

根据所述履带机器人的当前状态下的位姿和设定的参考轨迹，建立的所述履带机器人的位姿误差模型采用式(2)表示：

其中，(x,y,θ)^T为所述履带机器人当前状态下的位姿，x、y分别为所述履带机器人的质心的当前位置的坐标，θ为所述履带机器人在当前状态下其运动方向与X轴的夹角，(x_r,y_r,θ_r)^T为所述位姿指令，x_r、y_r分别为所述履带机器人的质心的目标位置的坐标，θ_r为所述履带机器人到达目标位置时其运动方向与X轴的夹角，x_e为所述履带机器人的质心的当前位置和目标位置沿其当前运动方向的误差值，y_e为所述履带机器人的质心的当前位置和目标位置在与其当前运动方向垂直方向的误差值，θ_e为θ和θ_r之间的误差值；

根据所述运动学模型和位姿误差模型建立的所述履带机器人的位姿误差微分模型：

其中，

分别为x_e、y_e和θ_e对时间的导数，v和ω分别为所述履带机器人在当前状态下的线速度和角速度，(v_r,ω_r)^T为所述速度指令，v_r和ω_r分别为所述履带机器人到达目标位置时的线速度和角速度，d为所述履带机器人的质心和几何中心之间的距离；

所述右电机和所述左电机的力矩平衡模型分别采用式(4)和式(5)表示：

其中，J_r(t)、J_l(t)分别为所述右电机和所述左电机的转轴的转动惯量，F为所述左电机和所述右电机的输出轴上的粘性摩擦系数，k_t为所述左电机和所述右电机的电磁转矩系数，T_dr(t)、T_dl(t)分别为所述右电机和所述左电机受到的干扰力矩，ω_dr(t)、ω_dl(t)分别为所述右电机和所述左电机的转轴转动的角速度，

和

分别为ω_dr(t)和ω_dl(t)对时间的导数，i_r(t)、i_l(t)分别为所述右电机和所述左电机的电枢电流；

所述右电机和所述左电机的电势平衡模型分别采用式(6)和式(7)表示：

其中，L为所述左电机和所述右电机的电枢电感，R为所述左电机和所述右电机的电枢电阻，k_e为所述左电机和所述右电机的反电动势系数，且k_e＝0.10472k_t，

和

分别为i_r(t)和i_l(t)对时间的导数，u_r(t)和u_l(t)分别为所述右电机的驱动电压和所述左电机的驱动电压；

根据所述力矩驱动模型和电势平衡模型获得的所述右电机和所述左电机的动态模型采用式(8)和式(9)表示：

其中，T_l(t)＝RJ_l(t)/(RF+k_tk_e)，T_r(t)＝RJ_r(t)/(RF+k_tk_e)，

k₁＝k_t/(RF+k_tk_e)，k₂＝R/(RF+k_tk_e)；

建立的随参数调整而变化的自适应滑模切换模型采用式(10)表示：

其中，α₁和α₂为倾斜参数，

c₁、c₂、c₃、c₄、k_k1、k_k2均为正常数，s₁和s₂分别为关于x_e和θ_e的切换函数；

根据所述位姿误差微分模型和所述自适应滑模切换模型获得的所述履带机器人的期望速度采用式(11)表示：

其中，v_d和ω_d分别为所述履带机器人的期望线速度和期望角速度，

为v_r对时间的导数；

所述切换控制模型采用式(12)表示：

其中，β₁、β₂为大于零的切换增益，β₁、β₂、Δ₁和Δ₂均为经验值，sat为饱和函数；

所述履带机器人的修正后的期望速度采用式(13)表示：

其中，v′_d和ω′_d分别为所述履带机器人的修正后的期望线速度和修正后的期望角速度；

所述右电机和所述左电机的期望角速度分别采用式(14)和式(15)表示：

ω_rd＝(v′_d+ω′_dA)r^-1 式(14)

ω_ld＝(v′_d-ω′_dA)r^-1 式(15)

其中，ω_rd和ω_ld分别为所述右电机的期望角速度和左电机的期望角速度，A为所述左驱动轮和右驱动轮之间的间距的一半，r为所述左驱动轮和所述右驱动轮的半径。

2.一种履带机器人，其特征在于，包括：

左驱动轮，用于驱动左履带；

右驱动轮，用于驱动右履带；

左电机，用于驱动所述左驱动轮；

右电机，用于驱动所述右驱动轮；

传感器，用于检测所述履带机器人的当前状态下的位姿，该位姿包括所述履带机器人在指定坐标系中的位置和倾斜角度；以及控制器，用于执行根据权利要求1所述的用于履带机器人的控制方法。

Images