CN107656500B - 基于数值加速度控制的全方位移动机器人轨迹跟踪方法 - Google Patents

Abstract

本发明公开了一种基于数值加速度控制的全方位移动机器人轨迹跟踪方法，包括以下步骤：针对全方位移动机器人进行物理建模；设定采样周期，建立全方位移动机器人的离散型动力学模型；设定目标轨迹，为使全方位移动机器人跟踪目标轨迹，基于数值加速度控制，计算作用在各个全方位移动部件上的驱动力；构建全方位移动机器人跟踪误差的离散状态反馈系统，求出实现轨迹跟踪的数值加速度控制器的最优参数；导入目标轨迹的未来信息，设计可使轨迹跟踪精度得到进一步有效提高的数值加速度控制器。本发明的轨迹跟踪控制方法，在各个全方位移动部件所承受的地面摩擦力实时变化情况下也能实现高精度的轨迹跟踪，并且具有良好的鲁棒性。

Description

基于数值加速度控制的全方位移动机器人轨迹跟踪方法

技术领域

本发明属于机器人控制技术领域，尤其涉及一种基于数值加速度控制的全方位移动机器人轨迹跟踪方法。

背景技术

随着时代的进步和科学技术的不断发展，机器人被广泛应用到物质生产和人类生活中。全方位移动机器人摆脱了转弯半径的限制，可进行零曲率半径运动，与普通移动机器人相比,具有机动灵活的运动特性，在机器人研究领域人已经成为一个重要分支。它可以在任意时刻实现前后、左右、自转、向任意方向的运动。

现代社会正处于不可避免的老龄化阶段，辅助老人生活的移动机器人将成为老人居家生活的重要支撑之一。而老人居家空间有限，家具摆设又没有规律，所以作为辅助老人生活的移动机器人，可在狭小空间内移动的全方位移动机器人技术，是比较合适的理想技术方案。利用全方位移动机器人辅助老人生活，最关键的是安全性。全方位移动机器人行走时为避免撞击周围的人和物，在室内全局或局部路径规划好的轨迹上，必须具备有高精度的轨迹跟踪能力。但是全方位移动机器人在室内使用时，不同的房间可能会有不同的地面材质，如地毯，地板等，另外还有湿度，温度，机械磨损等因素影响，导致机器人所承受的地面摩擦力不同。即全方位移动机器人是典型的非线性时变系统。作为非线性时变系统轨迹跟踪控制的常用方法有，PID控制，神经网络法，模糊控制等。对于给定的目标轨迹，PID控制法在一定的地面条件，即当摩擦一定时，通过调节其控制参数，可以达到一定的追踪精度。但是当改变轨迹，或者地面条件变化，即摩擦实时变化时，需要重新调节PID控制参数，否则将不能保证轨迹跟踪的精度。因此说用PID控制实现全方位移动机器人轨迹跟踪时，很难实时对应各种轨迹以及适应各种不同的地面条件。神经网络法，可以克服全方位移动机器人系统由于摩擦等因素所引起非线性时变外部扰动，但其算法复杂，在线学习时间长，所以神经网络法不容易实现全方位移动机器人的实时轨迹跟踪控制。模糊控制法不需要对系统进行建模，但模糊控制规则把各种轨迹，各种时变摩擦等因素都面面俱到地包含进去是不现实的。目前，全方位移动机器人的轨迹跟踪问题是国内外研究的难点问题。

发明内容

本发明的目的在于：针对现有技术的不足，而提供一种基于数值加速度控制的全方位移动机器人轨迹跟踪方法，该方法的数值控制器设计不是单纯将通常的连续时间控制法进行数值化，而是基于作用在全方位移动机器人的万向轮上的驱动力和全方位移动机器人运动中的加速度、速度和位置三者的瞬间关系，利用目标轨迹的未来信息而实现的。另外，该方法的数值控制器能很好地处理全方位移动机器人所承受的地面摩擦等非线性时变外扰，并且由构成的跟踪误差状态反馈形式，用极点配置法或者最优理论可以选出数值控制器中的最优调节参数，轨迹跟踪精度将得到进一步有效提高。

为了实现上述目的，本发明采用以下技术方案：

一种基于数值加速度控制的全方位移动机器人轨迹跟踪方法，包括以下步骤：

步骤一，全方位移动机器人的物理建模，全方位移动部件为四个万向轮，考虑地面对全方位移动机器人的非线性摩擦力影响的运动学模型和动力学模型分别如式(1)和式(2)所示，

其中，(x,y)为绝对坐标系，θ为机器人的姿势角，F_i为作用在各个万向轮上的驱动力，f_i为地面对机器人的非线性摩擦力,v_i为各个万向轮速度，M为机器人的质量，I为机器人的转动惯量，D为驱动力相对机器人中心位置的力臂长度；

当

F(t)＝[F₁(t),F₂(t),F₃(t),F₄(t)]^T，

f(t)＝[f₁(t),f₂(t),f₃(t),f₄(t)]^T

动力学模型以表示；

步骤二，假设在时刻kT之后，经过非常微小时间后的时刻为kT⁺，在时刻t＝kT⁺时，作用于全方位移动机器人万向轮上的驱动力为F(kT⁺)，在一个采样周期T内作用于各个万向轮驱动力为不变的，则时刻t＝kT和时刻t＝kT⁺的全方位移动机器人的离散型动力学模型分别如式(3)和式(4)所示，

其中F((k-1)T⁺)为时间区间[(k-1)T⁺,kT]内作用于各个万向轮的输入驱动力，F(kT⁺)是经过一个采样周期T后，下一个时间区间[kT⁺,(k+1)T]内的作用于各个万向轮的驱动力；

步骤三，把X_cd(t)作为全方位移动机器人的目标轨迹，为了使全方位移动机器人跟踪目标轨迹，时刻t＝kT的万向轮驱动力F((k-1)T⁺)作为已知的，则在时刻t＝kT⁺，基于数值加速度控制，作用在各个万向轮上的驱动力F(kT⁺)的计算式如下：

其中，K_D为速度偏差系数，K_D＝diag(k_dii)；K_P为位置偏差系数，K_P＝diag(k_pii)；

步骤四，决定K_D和K_P的最优值，将目标轨迹设为X_cd(t),全方位移动机器人跟踪误差定义为e(t)＝X_cd(t)-X(t),当作用在全方位移动机器人万向轮上的驱动力瞬间变化时，其全方位移动机器人加速度也将瞬间变化，而全方位移动机器人速度和位置是不会发生瞬间变化，又由全方位移动机器人的离散型动力学模型和作用于各个万向轮上的驱动力的计算式，得到全方位移动机器人跟踪误差的离散模型如下：

在时间区间[kT⁺,(k+1)T]内，

根据(6)～(8)三式得到下式，

令以及

得到E[(k+1)T]＝AE(kT)，

进一步地，

得到E[(k+1)T]＝(A₁-A₂K)E(kT)，

对于上式的误差状态反馈系统，为保证系统稳定性，用极点配置法或者最优理论可以选出K_P和K_D的最优值；

步骤五，基于离散型最优预见控制理论，利用已知的未来目标信息，进一步改善系统动态响应性能，在时刻t＝kT⁺，基于数值加速度控制，导入目标轨迹未来信息，计算作用在各个万向轮上的驱动力，进行全方位移动机器人系统的数值加速度控制器的设计。

作为本发明所述的基于数值加速度控制的全方位移动机器人轨迹跟踪方法的一种改进，在步骤五中，在时刻t＝kT⁺，导入目标轨迹未来位置信息时，基于数值加速度控制，设计的全方位移动机器人系统的数值加速度控制器的控制则如下：

作为本发明所述的基于数值加速度控制的全方位移动机器人轨迹跟踪方法的一种改进，在步骤五中，在时刻t＝kT⁺，导入目标轨迹未来位置信息，且还导入未来速度信息时，基于数值加速度控制，设计的全方位移动机器人系统的数值加速度控制器的控制则如下：

本发明的有益效果在于：

1)本发明着眼于作用在各个万向轮上的驱动力和全方位移动机器人运动加速度之间的瞬间关系，利用目标轨迹的未来信息，提供了基于数值加速度控制的全方位移动机器人的轨迹跟踪控制方法。所设计的离散数值加速度控制器算法可以抵消，全方位移动机器人系统实时运动过程中存在的非线性时变干扰。最优化设计的离散数值加速度控制器中的速度调节参数值和位置调节参数值，可以对应于不同的目标轨迹，抑制摩擦力等非线性时变干扰，从而解决了全方位移动机器人非线性系统中的参数时变特性问题。

2)本发明的数值加速度控制器利用了目标轨迹的未来信息，使全方位移动机器人的轨迹跟踪精度得到进一步有效提高。

3)本发明易于计算机实时控制的实现，在各个万向轮所承受的地面摩擦力实时变化情况下也能实现高精度的轨迹跟踪，具有良好的鲁棒性。

附图说明

图1为本发明的全方位移动机器人的结构示意图。

图2为本发明中全方位移动机器人万向轮的坐标配置图。

图3为本发明中全方位移动机器人数值加速度控制系统的框图。

图4为本发明中实施例1的仿真实验结果。

图5为本发明中实施例1进行仿真实验时作用于各个万向轮上的驱动力和摩擦力。

图6为本发明中实施例2的仿真实验结果。

图7为本发明中实施例2进行仿真实验时作用于各个万向轮上的驱动力和摩擦力。

图8为本发明中实施例3的仿真实验结果。

具体实施方式

下面将结合具体实施方式和说明书附图对本发明及其有益效果作进一步详细说明，但是，本发明的具体实施方式并不局限于此。

实施例1

本实施例提供一种基于数值加速度控制的全方位移动机器人轨迹跟踪控制方法，包括以下步骤：

步骤一，当全方位移动机器人的质量M＝100kg，全方位移动机器人的转动惯量I＝6kg·m，驱动力相对机器人中心位置的力臂长度D＝0.3m时，图1所示的全方位移动机器人，按图2所示的万向轮的坐标配置，其运动学模型和动力学模型以及非线性摩擦模型如下：

其中，μ＝0.2N/kg,

上述动力学模型可以用如下方程式表示，即

其中

F(t)＝[F₁(t),F₂(t),F₃(t),F₄(t)]^T，

f(t)＝[f₁(t),f₂(t),f₃(t),f₄(t)]^T

步骤二，设采样周期为T＝10ms，在一个采样周期T内对各个万向轮的驱动力为不变，在时刻t＝kT和时刻t＝kT⁺，全方位移动机器人的离散型动力学模型如下：

步骤三，把X_cd(t)作为全方位移动机器人的目标轨迹，为了使全方位移动机器人跟踪目标轨迹，基于数值加速度控制，在时刻t＝kT⁺时，作用在各个万向轮驱动力的计算式如下：

其中，K_D为速度偏差系数，K_D＝diag(k_dii)；K_P为位置偏差系数，K_P＝diag(k_pii)；

步骤四，目标轨迹为X_cd(t)时，全方位移动机器人跟踪误差定义为e(t)＝X_cd(t)-X(t)，全方位移动机器人跟踪误差的离散模型如下：

E[(k+1)T]＝AE(kT)

采样周期为T＝10ms，机器人的转动惯量为I＝26kg·m时，为了保证系统稳定性，基于跟踪误差的离散模型A的特征值计算，选出的K_P和K_D的最优值如下：

K_P＝{0.003,0.003,0.003}，K_D＝{0.1,0.1,0.1}。

实施例2

与实施例1不同的是：本实施例的基于数值加速度控制的全方位移动机器人轨迹跟踪控制方法，还包括以下步骤：

步骤五，在时刻t＝kT⁺，导入目标轨迹未来位置信息的全方位移动机器人系统数值加速度控制器的控制则如下：

其中，预见控制参数：P＝6，K1＝dig{0.004；0.006；0.004}K2＝dig{0.0008；0.0008；0.0008}；K3＝dig{0.0006；0.0006；0.00001}K4＝dig{0.0001；0.0001；0.00001}；K5＝K6＝K4。

其它的与实施例1相同，这里不再赘述。

实施例3

与实施例1不同的是：本实施例的基于数值加速度控制的全方位移动机器人轨迹跟踪控制方法，还包括以下步骤：

步骤五，在时刻t＝kT⁺，导入目标轨迹未来位置信息以及未来速度信息时，基于数值加速度控制，设计的全方位移动机器人系统的数值加速度控制器的控制则如下：

其中，预见控制参数：P＝6，K1＝dig{0.004；0.006；0.004}K2＝dig{0.0008；0.0008；0.0008}；K3＝dig{0.0006；0.0006；0.00001}K4＝dig{0.0001；0.0001；0.00001}；K5＝K6＝K4。

其它的与实施例1相同，这里不再赘述。

测试实验

分别对实施例1～3的对全方位移动机器人轨迹跟踪方法进行仿真实验，仿真实验的径路如下：

其中，圆心为(x0,y0)＝(5m,5m)，半径为r＝4。

所规划的目标轨迹如下：

仿真实验时间设为t₀＝150s。

需要说明的是，图4中的实线为目标值，点线为全方位移动机器人的仿真实验跟踪结果，图4(a)的纵轴表示机器人x方向位置的跟踪，图4(b)的纵轴表示机器人y方向位置的跟踪，图4(c)的纵轴表示机器人姿势角的跟踪，图4(d)表示全方位移动机器人在圆形目标轨迹上的跟踪情况。图6中的实线为目标值，点线为全方位移动机器人的仿真实验跟踪结果，图6(a)的纵轴表示机器人x方向位置的跟踪，图6(b)的纵轴表示机器人y方向位置的跟踪，图6(c)的纵轴表示机器人姿势角的跟踪，图6(d)表示全方位移动机器人在圆形目标轨迹上的跟踪情况。图8中的实线为目标值，点线为全方位移动机器人的仿真实验跟踪结果，图8(a)的纵轴表示机器人x方向位置的跟踪，图8(b)的纵轴表示机器人y方向位置的跟踪，图8(c)的纵轴表示机器人姿势角的跟踪，图8(d)表示全方位移动机器人在圆形目标轨迹上的跟踪情况。

从图4、图6和图8的结果可以看出，导入目标轨迹的未来位置信息时的跟踪的仿真实验结果，要比没有导入目标轨迹的未来信息时的跟踪精度高，导入目标轨迹的未来位置信息和速度信息时跟踪精度是最高的。

根据上述说明书的揭示和教导，本发明所属领域的技术人员还能够对上述实施方式进行变更和修改。因此，本发明并不局限于上述的具体实施方式，凡是本领域技术人员在本发明的基础上所作出的任何显而易见的改进、替换或变型均属于本发明的保护范围。此外，尽管本说明书中使用了一些特定的术语，但这些术语只是为了方便说明，并不对本发明构成任何限制。

Claims (3)

1.一种基于数值加速度控制的全方位移动机器人轨迹跟踪方法，其特征在于，包括以下步骤：

步骤一，全方位移动机器人的物理建模，全方位移动部件为四个万向轮，考虑地面对全方位移动机器人的非线性摩擦力影响的运动学模型和动力学模型分别如式(1)和式(2)所示，

其中，(x,y)为绝对坐标系，θ为机器人的姿势角，整数变量i∈{1，2，3，4}表示万向轮的标号，F_i为作用在第i个万向轮上的驱动力，f_i为地面第i个万向轮上的非线性摩擦力,v_i为第i个万向轮的速度，M为机器人的质量，I为机器人的转动惯量，D为驱动力相对机器人中心位置的力臂长度；

当

F(t)＝[F₁(t),F₂(t),F₃(t),F₄(t)]^T，

f(t)＝[f₁(t),f₂(t),f₃(t),f₄(t)]^T，

时，

动力学模型以表示，其中M₀为机器人惯性矩阵；

步骤二，假设在时刻kT之后，经过非常微小时间后的时刻为kT⁺，在时刻t＝kT⁺时，作用于全方位移动机器人万向轮上的驱动力为F(kT⁺)，在一个采样周期T内作用于各个万向轮驱动力为不变的，则时刻t＝kT和时刻t＝kT⁺的全方位移动机器人的离散型动力学模型分别如式(3)和式(4)所示，

其中F[(k-1)T⁺]为时间区间[(k-1)T⁺,kT]内作用于各个万向轮的输入驱动力，F[(kT⁺)]是经过一个采样周期T后，下一个时间区间[kT⁺,(k+1)T]内的作用于各个万向轮的驱动力；

步骤三，把X_cd(t)作为全方位移动机器人的目标轨迹，为了使全方位移动机器人跟踪目标轨迹，时刻t＝kT的万向轮驱动力F[(k-1)T⁺]作为已知的，则在时刻t＝kT⁺，基于数值加速度控制，作用在各个万向轮上的驱动力F(kT⁺)的计算式如下：

其中，整数变量i∈{1，2，3，4}表示万向轮的标号；K_D为速度偏差系数矩阵，K_D＝diag(k_dii)、k_dii为第i个万向轮的速度偏差系数；K_P为位置偏差系数矩阵，K_P＝diag(k_pii)、k_pii为第i个万向轮的位置偏差系数；X_cd(kT⁺)为目标轨迹，X_c(kT)为实际轨迹；

步骤四，决定K_D和K_P的最优值，全方位移动机器人跟踪误差定义为e(t)＝X_cd(t)-X_c(t)当作用在全方位移动机器人万向轮上的驱动力瞬间变化时，其全方位移动机器人加速度也将瞬间变化，而全方位移动机器人速度和位置是不会发生瞬间变化，又由全方位移动机器人的离散型动力学模型和作用于各个万向轮上的驱动力的计算式，得到全方位移动机器人跟踪误差的离散模型如下：

在时间区间[kT⁺,(k+1)T]内，

根据(6)～(8)三式得到下式，

令以及

得到Ε[(k+1)T]＝AΕ(kT)，

进一步地，

得到Ε[(k+1)T]＝(A₁-A₂K)Ε(kT)，

对于上式的误差状态反馈系统，为保证系统稳定性，用极点配置法或者最优理论可以选出K_P和K_D的最优值；

步骤五，基于离散型最优预见控制理论，利用已知的未来目标信息，进一步改善系统动态响应性能，在时刻t＝kT⁺，基于数值加速度控制，导入目标轨迹未来信息，计算作用在各个万向轮上的驱动力，进行全方位移动机器人系统的数值加速度控制器的设计。

2.根据权利要求1所述的基于数值加速度控制的全方位移动机器人轨迹跟踪方法，其特征在于，在步骤五中，在时刻t＝kT⁺，导入目标轨迹未来位置信息时，基于数值加速度控制，设计的全方位移动机器人系统的数值加速度控制器的控制则如下：

其中，对角常数矩阵K₁,K₂,···,K_P分别表示未来时刻[(k+1)T],[(k+2)T],···,[(k+P)T]的目标轨迹与现时刻轨迹的偏差系数。

3.根据权利要求1所述的基于数值加速度控制的全方位移动机器人轨迹跟踪方法，其特征在于，在步骤五中，在时刻t＝kT⁺，导入目标轨迹未来位置信息，且还导入未来速度信息时，基于数值加速度控制，设计的全方位移动机器人系统的数值加速度控制器的控制则如下：

其中，对角常数矩阵分别表示未来时刻[(k+1)T],[(k+2)T],···,[(k+P)T]的目标速度与现时刻速度的偏差系数。