一种轮式移动机器人的近似解耦、快速轨迹跟踪控制方法与流程

本发明涉及一种轮式移动机器人的近似解耦、快速轨迹跟踪控制方法，特别是涉及一种控制结构简单有效、控制解耦的轮式移动机器人的轨迹跟踪控制方法。

背景技术：

轨迹跟踪控制是轮式移动机器人控制当中重要内容之一。轨迹跟踪控制的方法有很多，例如：李雅普诺夫直接法控制、滑模变结构控制、反步法控制等。上述方法虽然能够达到跟踪控制要求，但控制器解构复杂，且控制器参数与被控量之间缺乏明确的调节关系，即调节某一个参数，可能同时影响多个被控量性能，在实践应用中参数难以整定。

此外，由于轮式移动机器人是典型的欠驱动、非线性对象，系统响应的快速性调节需要复杂的优化计算，难以适用于实际系统应用。

综上所述，在轮式移动机器人的轨迹跟踪控制当中，如何设计控制器，保证系统稳定的同时，能够使得控制器参数对跟踪控制性能有明确的调节关系，以适应实际应用要求，具有重要的理论意义和实际应用价值。

技术实现要素：

本发明的主要目的在于克服现有技术中的上述缺陷，提出一种轮式移动机器人的近似解耦、快速轨迹跟踪控制方法，具有结构简单、被控对象近似解耦、调节参数少，收敛速度快等优点。

本发明采用如下技术方案：

一种轮式移动机器人的近似解耦、快速轨迹跟踪控制方法，其特征在于，包括如下步骤：1)根据轮式移动机器人的运动学模型和期望轨迹模型，建立误差模型和误差微分方程；2)采用新的控制律，给出轮式移动机器人的前进速度和转向角速度的表达式；3)设定表达式的参数以满足解耦和快速调节要求。

步骤1)中的所述误差模型为：

其中[xeyeθe]^t为轮式移动机器人实际位姿与期望位姿的偏差，在角度误差中，引入角度θe/2的正切值，能够将θe∈(-π，π)的角度转化为(-∞，∞)的常规数值范围，(xr，yr)是质心在世界坐标系下的期望位置坐标，θr为轮式移动机器人的运动方向与世界坐标系的x方向的期望夹角，(x，y)是质心在世界坐标系下的实际位置坐标，θ为轮式移动机器人的运动方向与世界坐标系的x方向的实际夹角，θe∈(-π，π）。

步骤1)中的误差微分方程为：

其中v、w分别是轮式移动机器人的实际线速度、实际角速度，vr、wr分别是轮式移动机器人的期望线速度、期望角速度，分别是xe、ye、θ的导数。

所述步骤2)中针对vr≠0设计的所述新控制律为

在控制律vc下，有此时状态xe与其他状态无关，且当k1＞0时，xe快速收敛到0；而闭环系统中设计的其他状态ye和可以通过李雅普诺夫稳定性理论分别针对vr＞0和vr＜0证明闭环系统稳定性，k1、k2、k3是大于零的常数。

所述步骤2)中，针对vr＝0设计的所述控制律为：

在控制律wc下，有此时状态θe与其他状态无关，且当k3＞0时，θe快速收敛到0；而闭环系统中设计的其他状态xe和ye，可以通过李雅普诺夫稳定性理论证明闭环系统稳定性。

所述步骤3)中，所述参数包括参数k1，k2，k3：

对vr≠0的情况，如果参数满足0＜＜k1，k2，k3，k3＜k1，k2|vr|＜＜k1k3和则满足近似解耦、快速跟踪条件；

对vr＝0的情况，如果参数满足0＜＜k1，0＜＜k3，和则满足近似解耦、快速跟踪条件。

由上述对本发明的描述可知，与现有技术相比，本发明具有如下有益效果：

本发明的一种轮式移动机器人的近似解耦、快速轨迹跟踪控制方法，使得轮式移动机器人的位姿参数能够近似解耦并实现快速轨迹跟踪。所提出的轨迹跟踪控制器具有控制结构简单、被控对象近似解耦、调节参数少，收敛速度快等优点。

附图说明

图1是轮式移动机器人轨迹跟踪控制的闭环反馈控制系统示意图；

图2是轮式移动机器人模型示意图；

图3是轮式移动机器人误差模型示意图；

图4是匀速圆周运动的参考轨迹曲线图与实际轨迹曲线图；

图5是匀速圆周运动的期望线速度曲线图与实际线速度曲线图；

图6是匀速圆周运动的期望角速度曲线图与实际角速度曲线图；

图7是匀速圆周运动的轨迹跟踪的位姿误差曲线图；

图8是变速余弦运动的参考轨迹曲线图与实际轨迹曲线图；

图9是变速余弦运动的期望线速度曲线图与实际线速度曲线图；

图10是变速余弦运动的期望角速度曲线图与实际角速度曲线图；

图11是变速余弦运动的轨迹跟踪的位姿误差曲线图。

图12是原地旋转运动的参考轨迹曲线图与实际轨迹曲线图；

图13是原地旋转运动的期望线速度曲线图与实际线速度曲线图；

图14是原地旋转运动的期望角速度曲线图与实际角速度曲线图；

图15是原地旋转运动的轨迹跟踪的位姿误差曲线图。

具体实施方式

以下通过具体实施方式对本发明作进一步的描述。

一种轮式移动机器人的近似解耦、快速轨迹跟踪控制方法，使得轮式移动机器人的位姿参数能够近似解耦并实现快速轨迹跟踪。所提出的轨迹跟踪控制器具有控制结构简单、被控对象近似解耦、调节参数少，收敛速度快等优点。

图1是轮式移动机器人轨迹跟踪控制结构示意图。内环采用常规速度控制器，实现移动机器人直线速度和转向角速度的控制。外环采用本发明提出的轨迹跟踪控制器，将期望的轨迹坐标xr、yr、θr与反馈x、y、θ进行比较，形成偏差xe、ye、θe.采用本发明提出的轨迹跟踪控制方法，得到机器人的直线速度vc和转向角速度wc。

图2是轮式移动机器人的模型示意图，其中xoy是世界坐标系，p是轮式移动机器人的质心及几何中心，d是轮式移动机器人的质心与几何中心之间的距离，l是两个驱动轮几何中心的间距。x、y、θ是用来表示轮式移动机器人实际位姿的3个空间定位自由度，θ是轮式移动机器人的运动方向与世界坐标系的x方向的实际夹角。vl、vr是轮式移动机器人左、右轮的实际线速度，v、w分别是轮式移动机器人的实际线速度、实际角速度。

图3是轮式移动机器人的误差模型示意图，其中xoy是世界坐标系，xr、yr、θr是用来表示轮式移动机器人参考位姿的3个空间定位自由度，θr是轮式移动机器人的运动方向与世界坐标系的x方向的参考夹角。vr、wr是轮式移动机器人的参考线速度和参考角速度，[xeyeθe]^t是轮式移动机器人的位姿误差。

步骤1)建立轮式移动机器人的运动学模型：

其中v、w分别是轮式移动机器人的实际线速度、实际角速度，(x，y)是质心在世界坐标系下的实际位置坐标，θ为轮式移动机器人的运动方向与世界坐标系的x方向的实际夹角，[xyθ]^t是轮式移动机器人在全局坐标系下的实际位姿。轮式移动机器人的约束条件是其中d是轮式移动机器人的质心与几何中心之间的距离，是x、y、θ的导数。

本发明考虑机器人的质心和几何中心重合，所以d＝0，即约束条件是该约束条件保证轮式移动机器人两轮轴线上的瞬时速度为0。可以看到，轮式移动机器人为一类典型的欠驱动控制系统，即输入量个数少于输出量个数。其次，控制量与被控量之间，存在耦合关系，难以精确控制。

本发明提出一种新的设计方法，在设计控制v和w中，引入新的参数，实现对[xyθ]^t的近似解耦调节。建立期望轨迹模型：

其中(xr，yr)是质心在世界坐标系下的期望位置坐标，θr为轮式移动机器人的运动方向与世界坐标系的x方向的期望夹角，[xryrθr]^t是轮式移动机器人的期望姿态，vr、wr分别是轮式移动机器人的期望线速度、期望角速度。

根据运动学模型和期望轨迹模型建立误差模型：

其中[xeyeθe]^t为轮式移动机器人实际位姿与期望位姿的偏差。在角度误差中，引入角度θe/2的正切值，能够将θe∈(-π，π)的角度转化为(-∞，∞)的常规数值范围。

建立误差模型的微分方程

其中分别是xe、ye、θ的导数。

针对微分方程，构建线性表达式：

其中λij为待设计未知量。由于上式为非线性方程，因此λij可以取任意变量。

步骤2)针对vr≠0和vr＝0两类情况，给出控制律设计和稳定性分析：

(1)考虑vr≠0的情况。

考虑第一个微分方程，有

取λ12＝λ13＝0，λ11＝-k1，此时k1＞0。

因此，有此时有

v＝yew+vrcosθe+k1xe。

在频域上有，其中s为频域复变量，xe(0)为误差初始值。因此，参数满足k1＞0，则误差xe自动收敛到0，且收敛速度由k1决定，k1越大，收敛速度越快。

考虑第三个方程，有

取λ31＝0，λ33＝-k3，此时k3＞0。频域上有其中s为频域复变量，θe(0)为误差初始值。因此，参数满足k3＞0，则误差θ自动收敛到且收敛速度由k3决定，k3越大，收敛速度越快。λ32为待定。此时，有

考虑第二个方程，

带入w得到可取λ21＝-wr-k3sinθe，使得方程成立。此时，当k1＞0k3＞0，有xe→0，对第二个方程，可以进行近似分析，有

为了使ye收敛，要求可取λ32＝-k2sign[vr]，k2＞0则

基于上述分析，从快速性角度来看，vr≠0的情况有如下特性：

1)如果有k3＜k1，则xe能够快速收敛，且对ye和的收敛几乎不产生影响。此时，参数条件可以近似写成：k3＜k1和k2|vr|＜＜k1k3。

2)假设角度误差在合适范围内时，取则能够快速跟踪此时，参数条件写成：

3)k2取值若足够大，且让k1和k3满足上述条件，则有ye快速收敛到0。

上述特征表明，在一定条件下，所设定的控制器参数k1、k2和k3具有对轮式移动机器人位姿的独立调节特性，即实现了解耦控制。

最终，vr≠0时系统的控制律为：

其中k1、k2、k3是大于零的常数。

结合上述分析得到的轨迹跟踪控制律，给出vr≠0情况的闭环系统稳定性证明。

考虑到在控制律v下，有此时状态xe与其他状态无关，且当k1＞0时，xe快速收敛到0。因此，xe不会影响闭环系统的稳定性。仅仅考虑状态ye和即可。

进一步分析ye，对控制系统稳定性的影响。令

选取能量方程：v＝x^tpx，其中令为对称正定矩阵。对v求导，得：

为了简化计算，取p12＝0。将误差模型的微分方程以及控制律代入上式，得：

由于存在sign[vr]，对vr＞0和vr＜0的情况进行讨论。

1)当vr＞0时，

其中，此时，考虑θe∈(-π，π)，在该范围任意取值，总能找到p11＞0，p22＞0满足

2)当vr＜0时，

其中，此时，考虑θe∈(-π，π)，在该范围任意取值，总能找到p11＞0，p22＞0满足

因此，vr≠0时，闭环系统的稳定性得到证明。

(2)考虑vr＝0的情况。

考虑第一个微分方程，有

取λ13＝0，λ11＝-k1，λ12＝k2sign[wr]此时0＜k1，k2。

因此，有此时有

v＝yew+vrcosθe+k1xe-k2sign[wr]ye。

则xe收敛至收敛速度由k1决定，k1越大，收敛速度越快。

考虑第三个方程，有

取λ31＝0，λ32＝0，λ33＝-k3，此时k3＞0。有此时θe自动收敛到0，且收敛速度由k3决定，k3越大，收敛速度越快。此时，有

w＝wr+k3sinθe。

考虑第二个方程，

带入w得到可取λ21＝-wr-k3sinθe，λ22＝0，使得方程成立。此时，当k1＞0，k3＞0，有θe→0，对第二个方程，可以进行近似分析，有

基于上述分析，从快速性角度来看，vr＝0的情况有如下特性：

1)如果有0＜＜k1，则xe能够快速收敛至此时，参数条件写成：0＜＜k1。

2)如果有0＜＜k3，则或θe能快速收敛至0。此时，参数条件写成：0＜＜k3。

3)且则有ye能够收敛到0。因此，参数条件写成：

上述特征表明，在一定条件下，所设定的控制器参数k1、k2和k3具有对轮式移动机器人位姿的独立调节特性，即实现了解耦控制。

最终，系统的控制律为：

其中k1、k2、k3是大于零的常数。

然后，对vr＝0的情况给出闭环系统的稳定性证明。

考虑到在控制律w下，有此时状态θe与其他状态无关，且当k3＞0时，θe快速收敛到0。因此，θe不会影响闭环系统的稳定性。仅仅考虑状态xe和ye即可。

进一步分析xe，ye对控制系统稳定性的影响。令x＝[xeye]^t

选取能量方程：v＝x^tpx，其中令为对称正定矩阵。对v求导，得：

在上式中，为了简化计算，取p12＝0。将误差模型的微分方程以及控制律代入上式，得：

由于存在sign[wr]，对wr＞0和wr＜0的情况进行讨论。

1)当wr＞0时，

其中，此时，总能找到p11＞0，p22＞0满足(p11k2-p22wr)＝0。

2)当wr＜0时，

其中，此时，总能找到p11＞0，p22＞0满足(-p11k2-p22wr)＝0。

可见，总能找到能量方程v满足李雅普诺夫稳定性定理。因此，vr＝0时，闭环系统的稳定性得到证明。

步骤3)，根据vr≠0和vr＝0，选定参数k1，k2和k3，满足近似解耦、快速跟踪条件，其中对vr≠0的情况，有0＜＜k1，k2，k3，k3＜k1，k2|vr|＜＜k1k3和vr＝0的情况，有0＜＜k1，0＜＜k3，

在本发明的实施例中，跟踪轨迹采用如下两种模型：

(1)匀速圆周运动，其中vr＝0.2m/s，wr＝0.2rad/s。假设系统的速度初始值为v0＝0.1m/s，w0＝0.1rad/s，参考位姿的初始值设为实际位姿的初始值设为此时选取的参数为k1＝20，k2＝35，k3＝12。图4是匀速圆周运动的参考轨迹曲线图与实际轨迹曲线图，图5是匀速圆周运动的期望线速度曲线图与实际线速度曲线图，图6是匀速圆周运动的期望角速度曲线图与实际角速度曲线图，图7是匀速圆周运动的轨迹跟踪的位姿误差曲线图。

(2)变速余弦运动，其中假设系统的速度初始值为v0＝0.1m/s，w0＝0.1rad/s，参考位姿的初始值设为实际位姿的初始值设为此时选取的参数为k1＝20，k2＝35，k3＝12。图8是变速余弦运动的参考轨迹曲线图与实际轨迹曲线图，图9是变速余弦运动的期望线速度曲线图与实际线速度曲线图，图10是变速余弦运动的期望角速度曲线图与实际角速度曲线图，图11是变速余弦运动的轨迹跟踪的位姿误差曲线图。

(3)原地旋转运动，其中vr＝0，wr＝sintrad/s。假设系统的速度初始值为v0＝0.1m/s，w0＝0.1rad/s，参考位姿的初始值设为实际位姿的初始值设为此时选取的参数为k1＝20，k2＝35，k3＝12。图12是原地旋转运动的参考轨迹曲线图与实际轨迹曲线图，图13是原地旋转运动的期望线速度曲线图与实际线速度曲线图，图14是原地旋转运动的期望角速度曲线图与实际角速度曲线图，图15是原地旋转运动的轨迹跟踪的位姿误差曲线图。

上述仅为本发明的具体实施方式，但本发明的设计构思并不局限于此，凡利用此构思对本发明进行非实质性的改动，均应属于侵犯本发明保护范围的行为。