CN109960150B - 巡检机器人轨迹跟踪控制方法 - Google Patents

Abstract

本发明公开了一种巡检机器人轨迹跟踪控制方法，具体步骤为：建立车轮轴中心与机器人参考位置不重合时的机器人前轮转向后轮驱动的运动学模型；根据机器人前轮转向后轮驱动的运动学模型建立机器人的原地转向模型；确定轨迹跟踪控制率，完成巡检机器人轨迹跟踪控制。本发明控制精确度高，鲁棒性好。

Description

巡检机器人轨迹跟踪控制方法

技术领域

本发明属于轨迹跟踪技术，具体为一种巡检机器人轨迹跟踪控制方法。

背景技术

轨迹跟踪控制是电力巡检机器人的重要核心技术之一。控制巡检机器人按既定的巡检路线到达巡检目标点是安全完成巡检任务的关键。变电站巡检由于其场所的特殊性，机器人必须与电力设备保持一定的安全距离，并且有些情况下必须在高于地面10-20cm的盖板上运动，这就要求机器人能够完全按照设定路线移动。设计合理的运动控制律是保障机器人安全巡检的有效手段。

目前常见的巡检机器人轨迹跟踪控制方法有：模糊自适应PID控制算法：用模糊推理在线得到PID控制其参数；反步法：针对方向角误差设计了具有全局渐近稳定的跟踪控制律；以机器人的力矩为控制输入，根据动力学模型设计的自适应滑膜控制器，用来消除机器人系统的参数不确定性；根据侧向误差和角度误差设计引导角，并将该引导角作为虚拟输入，设计控制律等。以上控制方法均考虑机器人的运动中心和几何中心一致，实际上很多情况下并不一定重合，控制精度不够。

发明内容

本发明的目的在于提出了一种巡检机器人轨迹跟踪控制方法。

实现本发明的技术解决方案为：一种巡检机器人轨迹跟踪控制方法，具体步骤为：

步骤1、建立车轮轴中心与机器人参考位置不重合时的机器人前轮转向后轮驱动的运动学模型；

步骤2、根据机器人前轮转向后轮驱动的运动学模型建立机器人的原地转向模型；

步骤3、确定轨迹跟踪控制率，完成巡检机器人轨迹跟踪控制。

优选地，步骤1中建立机器人前轮转向后轮驱动的运动学模型的具体步骤为：

步骤1-1、建立机器人与世界坐标系的关系，即：

步骤1-2、根据机器人与世界坐标系的关系求得旋转中心坐标：

式中，L表示机器人前后两轮的轮距，l表示机器人的几何中心与机器人后轮轴中心之间的距离，θ为机器人的航向角，α阿尔法为前轮的转角，d为采样时间内两驱动轮编码器值的平均值，k-1时刻机器人在全局坐标系中的位姿为O_k-1(x_k-1,y_k-1,θ_k-1)，k时刻的位姿为O_k(x_k,y_k,θ_k)。

优选地，步骤2建立的原地转向模型为：

V₁＝-V₂＝V₃＝-V₄

式中，V₁，V₂，V₃，V₄分别是左前轮、右前轮、左后轮、右后轮的线速度。

本发明与现有技术相比，其显著优点为：本发明建立的运动学模型，以及原地转向模型，更加贴合实际情况，使得控制精确度高，鲁棒性好。

下面结合附图对本发明做进一步详细的描述。

附图说明

图1为机器人坐标系示意图。

图2为前轮转向后轮驱动模型示意图。

图3为机器人原地旋转模型示意图。

图4为轨迹跟踪示意图。

具体实施方式

一种巡检机器人轨迹跟踪控制方法，具体步骤为：

步骤1、建立车轮轴中心与机器人参考位置不重合时的机器人前轮转向后轮驱动的运动学模型，具体步骤为：

步骤1-1、建立机器人与世界坐标系。如图1所示以机器人的几何中心代表机器人的位置，建立原点，以机器人前进方向为X轴，建立坐标系X_rO_rY_r，世界坐标系为X_wO_wY_w。其中D表示机器人的左右两轮轴距，L表示机器人前后两轮的轮距，θ为机器人的航向角。机器人和世界坐标系的关系^rR_w可以表示为：

步骤1-2、根据机器人与世界坐标系的关系求得旋转中心坐标，首先，机器人依靠后轮驱动，前轮转向，实现直线以及弧线的运动，进行简化模型，做以下设置：

(1)每根轴上的两个轮子都简化成位于车轴中点的单个轮子；

(2)车轮与地面之间满足纯滚动无滑动条件；纯滚动无滑动是指驱动轮只能绕着轮轴方向进行运动，而与轴向平行方向没有滑动；

(3)直接控制前轮转向角，调节转向角的时间忽略不计。

当满足上述设置后，进行模型的运动学分析。当前轮的转角较小时，α＜α_min，机器人可以近似认为是直线运动。记机器人k时刻的位姿为(x_k,y_k,θ_k)，那么机器人从k-1时刻运动到k时刻里程计推算为：

其中d＝(d₃+d₄)/2为采样时间内两驱动轮编码器值的平均值。

当前轮转角较大时，其基本运动模型如图2。机器人的运动看作是以I为圆心，以r为半径的一段圆弧运动。图中点I为机器人运动的旋转中心，C为机器人后轮轴中心，O为机器人的几何中心，l表示两者之间的距离。β为转过的角度，以逆时针旋转为正，α为前轮的转角，其范围为(-π/2，π/2)，d为转过的弧长。

由于在整个系统中都以机器人的几何中心O作为机器人的参考位置，而机器人的轴中心C与机器人中心O不重合，故与一般前轮转向后轮驱动模型有所不同。记k-1时刻机器人在全局坐标系中的位姿为O_k-1(x_k-1,y_k-1,θ_k-1)，k时刻的位姿为O_k(x_k,y_k,θ_k)。

设C点坐标为(x,y,θ),C′的坐标为(x′,y′,θ′)，求得旋转中心坐标为:

根据几何关系可以得到：

车后两轮轴中心与机器人几何中心相对位置固定，距离为l即：

可以得到：

由于采样时间足够小，可以认为相邻时刻机器人转过的角度β足够小，cosβ≈1,sinβ≈β，旋转中心坐标化简可得：

其中，

为采样时间内两驱动轮编码器值的平均值。

当α很小时，与直线行走时模型一致。里程计推算的结果作为机器人位姿的初步估计。

步骤2、根据机器人前轮转向后轮驱动的运动学模型建立机器人的原地转向模型。设计机器人为原地转向模型，可以控制机器人的转弯半径为零。其简易模型如图3。其中α₁-α₄为轮子方向与X轴的夹角，以逆时针为正，其范围为(-π/2，π/2)。l₁，l₂分别是前后轮距和左右轴距的一半。V₁，V₂，V₃，V₄分别是左前轮，右前轮，左后轮，右后轮的线速度，正方向为前进方向。具体步骤为：

步骤2-1、实现原地转向必须绕机器人的中心也就是O_r旋转，则轮子的前进方向设定为切线方向，以左后轮为例，OO_r垂直于V₃,，其他轮子的中心与质心O_r的连线也必须与速度方向垂直。同时设定机器人的在X轴的速度分量和为0，在Y轴的分量和旋转角速度w相等，并且得到α₃,α₄∈[0,π/2)、α₁,α₂∈(-π/2,0]。

步骤2-2、轮子i在X轴的速度分量V_ix与在Y轴的分量V_iy，i∈[1,4]满足下列关系：

其中：

求解得到约束关系：

同时以右后轮为例，根据几何关系可得：

步骤2-3、实际设计中，根据前后轮距与左右轴距相等，即l₁＝l₂，可得

同理可得

根据转角范围，得到

代入公式得到：

V₁＝-V₂＝V₃＝-V₄ (14)

那么根据轮子的正转反转两种情形，当左前轮左后轮正转，右前轮右后轮反转时，机器人按顺时针方向旋转，反之机器人逆时针旋转。

步骤3、确定轨迹跟踪控制率，完成巡检机器人轨迹跟踪控制，具体步骤为：

步骤3-1、两个步骤分析了机器人的运动学模型，不考虑原地旋转的情况，机器人的运动学方程可以表示为式(15)。v，α分别表示机器人的速度和前轮转向角。l表示机器人参考位置与后轮轴中心的距离，L表示前后轮轮距。

在驱动轮后轮纯滚动，无滑动的情况下，该机器人模型的非完整约束为

步骤3-2、建立轨迹跟踪误差方程。设存在参考的移动机器人，其位姿用向量P_r＝[x_r,y_r,θ_r]^T表示，控制输入为[v_r,α_r]^T，v_r表示车体线速度，α_r表示前轮相对车体的转向角。同结构的参考机器人运动学模型为：

根据图4可以得到机器人坐标系下的轨迹跟踪误差方程：

x_e,y_e,θ_e分别表示机器人在x，y方向以及航向角的误差。对误差方程求微分可得：

综上所述，基于移动机器人运动学模型的轨迹跟踪问题转化为：确定合适的有界输入[v,α]，使得机器人在任意初始误差下，保证机器人的位置误差以及方向角误差有界，并且收敛到零。

步骤3-3、针对(19)所示的误差系统，设计控制律，其中k₁，k₂均为正实数：

针对本发明，利用Lyapunov稳定性定理对系统稳定性进行分析，选取Lyapunov函数如下：

显然，V≥0，当且仅当(x_e,y_e,θ_e)＝(0,0,0)时V＝0。且当(x_e,y_e,θ_e)→∞时，V→∞。

对式(20)求导可得：

将控制律式(22)代入(21)可得：

由于k₁＞0,k₂＞0，显然

当

时，有x_e+l(1-cosθ_e)＝0，sin(θ_e)＝0，则：

进一步的有

由此可得

而

得

综上所述，在控制律(22)的作用下系统渐近稳定，当t→∞时，跟踪误差(x_e,y_e,θ_e)均收敛于零。

考虑当系统误差较大时，控制律产生的控制量(v,α)将可能超出机器人的实际运动能力，即超过最大速度v_max，或者超出转向角调节范围α_max。为了保证机器人的运动平滑，对控制量进行约束。

其中sign为取符号函数。

Claims (4)

1.一种巡检机器人轨迹跟踪控制方法，其特征在于，具体步骤为：

步骤1、建立车轮轴中心与机器人参考位置不重合时的机器人前轮转向后轮驱动的运动学模型；

步骤2、根据机器人前轮转向后轮驱动的运动学模型建立机器人的原地转向模型；

步骤3、确定轨迹跟踪控制律，完成巡检机器人轨迹跟踪控制，确定轨迹跟踪控制律的具体步骤为：

步骤3-1、建立机器人的运动学方程

式中，v，α分别表示机器人的速度和前轮转向角，l表示机器人参考位置与后轮轴中心的距离，L表示前后轮轮距；

求解机器人的运动学方程的非完整约束：

步骤3-2、根据机器人的运动学方程，建立轨迹跟踪误差方程：

式中，位姿用向量P_r＝[x_r,y_r,θ_r]^T表示，控制输入为[v_r,α_r]^T，v_r表示车体线速度，α_r表示前轮相对车体的转向角；

求取微分得到：

式中：x_e,y_e,θ_e分别表示机器人在x，y方向以及航向角的误差；

步骤3-3、根据步骤3-1以及3-2，设计控制律：

其中，k₁，k₂均为正实数。

2.根据权利要求1所述的巡检机器人轨迹跟踪控制方法，其特征在于，步骤1中建立机器人前轮转向后轮驱动的运动学模型的具体步骤为：

步骤1-1、建立机器人与世界坐标系的关系，即：

步骤1-2、根据机器人与世界坐标系的关系求得旋转中心坐标：

式中，L表示机器人前后两轮的轮距，l表示机器人的几何中心与机器人后轮轴中心之间的距离，θ为机器人的航向角，α阿尔法为前轮的转角，d为采样时间内两驱动轮编码器值的平均值，k-1时刻机器人在全局坐标系中的位姿为O_k-1(x_k-1,y_k-1,θ_k-1)，k时刻的位姿为O_k(x_k,y_k,θ_k)。

3.根据权利要求1所述的巡检机器人轨迹跟踪控制方法，其特征在于，步骤2建立的原地转向模型为：

V₁＝-V₂＝V₃＝-V₄

式中，V₁，V₂，V₃，V₄分别是左前轮、右前轮、左后轮、右后轮的线速度。

4.根据权利要求3所述的巡检机器人轨迹跟踪控制方法，其特征在于，原地转向模型的建立过程为：

步骤2-1、设定轮子的前进方向为切线方向，同时机器人在X轴的速度分量和为0，在Y轴的分量和旋转角速度w相等，并且

其中α₁，α₂，α₃，α₄分别为左前轮、右前轮、左后轮、右后轮方向与X轴的夹角；

步骤2-2、轮子i在X轴的速度分量V_ix与在Y轴的分量V_iy，i∈[1,4]满足下列关系：

其中：

对关系求解得到约束关系：

步骤2-3、根据前后轮距与左右轴距相等，求得：

根据转角范围，得到

则得到原地转向模型：

V₁＝-V₂＝V₃＝-V₄。

Images