CN110095983A - 一种基于路径参数化的移动机器人预测跟踪控制方法 - Google Patents

Abstract

一种基于路径参数化的移动机器人预测跟踪控制方法，包括以下步骤：1)建立移动机器人路径跟踪误差模型；2)定义参数化路径更新规则；3)设计性能指标函数；4)定义预测模型向量描述；5)牛顿法求解二次型最优控制量。本发明提供了一种可以有效解决移动机器人设定速度与实际速度无法快速匹配的预测跟踪控制方法。

Description

一种基于路径参数化的移动机器人预测跟踪控制方法

技术领域

本发明涉及移动机器人路径跟踪控制领域，尤其涉及移动机器人实际速度与设定速度无法快速匹配而提出基于路径参数化的模型预测控制方法。

背景技术

随着软硬件技术和控制技术的发展，机器人已在在各大行业广泛应用。其中，移动机器人路径跟踪控制技术涉及机械工程、电气自动化、传感技术、计算机技术、图像处理技术等多交叉学科的知识成果，在民用、工业、军事各大领域得到了全球国家的高度重视。移动机器人路径跟踪控制技术还适用其他很多场景，例如轮船路径、车床切削路径、自动驾驶等。因此，针对移动机器人路径跟踪控制技术的研究，不仅可以丰富移动机器人运动控制的理论成果，还可以满足多领域对运动控制技术越来越高的要求，具有重大的理论和工程意义。

然而在实际环境中，尤其在复杂的工作环境下，存在各种不确定性因素干扰移动机器人的路径跟踪，其中，在机器人运作过程中，存在实时设定速度与实际速度无法快速匹配导致误差变大的问题，给移动机器人技术带来机遇与挑战。

模型预测控制方法涉及预测模型、滚动时域优化、反馈优化几个部分，相较于其他的控制方法，预测控制能够及时的校正模型失配以及干扰等引起的不确定性，具有建模方便、系统稳定、扩展性好等优点，备受科研工作者的青睐。F.Cortes等人基于预测模型设计多机器人链系统，将预测模型分为六个独立的模块，分别对单个进行模型验证，并分别设计控制器，超调量和稳定时间得到有效控制。Karl Worthmann等人针对非完整移动机器人转向问题，提出基于模型预测控制方案，先建立预测模型，严格分析稳定性，并验证非完整移动机器人的转向效果。肖涵臻等人在论文(基于预测控制方法的轮式机器人编队)中为了能成功控制住者两个系统，实验中采用了模型预测控制(MPC)作为控制方法。模型预测控制通过构建一个带约束的二次规划(QP)求最优问题，并迭代地实时求这个最优问题，得到最优控制输入。刘洋等人在论文(基于模型预测控制的移动机器人路径跟踪控制)中利用非线性模型预测控制(Nonlinear Model Predictive Control,NMPC)具有滚动优化和反馈校正的机理，能够处理系统的状态约束和输入约束。然而，这些结果没有将移动机器人实际的速度匹配问题考虑进来，当经过预测模型、滚动优化、二次型问题求解过程后输入控制量时，移动移动机器人需要加速或者减速达到设定值，该过程受电池、电机、驱动以及惯性等问题的影响，因此，针对移动机器人输入速度与实际速度匹配问题的研究非常有必要。

发明内容

为了克服现有技术无法解决移动机器人控制量设定速度与实际速度无法快速匹配的问题，本发明提供一种基于路径参数化的移动机器人预测跟踪控制方法，通过该问题建立跟踪误差模型，基于该模型给出预测性能指标函数，并利用二次规划求解最优控制量。

本发明解决其技术问题所采用的技术方案是：

一种基于路径参数化的移动机器人预测跟踪控制方法，包括以下步骤：

1)建立机器人运动学模型，x＝[x,y,θ]^T为机器人实际位姿，(x,y)为机器人实际位置，θ为机器人实际角度，定义r＝[x_r,y_r,θ_r]^T为虚拟机器人参考位姿，(x_r,y_r)为虚拟机器人参考位置，θ_r为虚拟机器人参考角度，则机器人运动学模型为：

其中，v为机器人实际线速度，ω为机器人实际角速度，v_r为虚拟机器人参考线速度，ω_r为机器人参考角速度，跟踪误差模型为：

其中，[x_e,y_e,θ_e]为误差向量，(x_e,y_e)为实际位置与参考位置偏差，θ_e为角度偏差；

2)建立移动机器人线性误差模型，对式(3)求导得：

根据式(4)在平衡点线性化得如下的状态空间方程：

其中，为状态误差向量，为机器人控制输入偏差向量，矩阵矩阵对式(5)进行离散化得：

其中，k为采样时刻，为k时刻机器人状态误差向量，为k时刻控制输入偏差量， T_s为采样周期；

3)定义参数化的期望路径：

P＝{r(k)∈Rⁿ|r(k)＝p(θ_r(k))} (7)

其中，P为参数化期望路径，r(k)为k时刻参考位置，p(θ_r(k))为k时刻的路径，θ_r(k)为k时刻的路径参数，θ_r(k)参数更新方式为：

其中，ω_p(k)为k时刻路径期望角速度，为k时刻与控制输入偏差量相关的线性表达式，关系如下：

其中，λ是增益标量，C＝[c₁ c₂]是与控制输入误差向量有关的增益矩阵；

4)定义如下预测性能指标：

其中，是状态偏差惩罚项，Q是状态加权矩阵，表示k时刻对k+i时刻的状态预测值，是k时刻参考控制量与路径期望控制量的惩罚项，v_r(k+i|k)是k+i时刻参考线速度预测值，ω_r(k+i|k)是k+i时刻参考角速度预测值，v_p(k+i|k)是k+i时刻路径期望线速度预测值，ω_p(k+i|k)是k+i时刻路径期望角速度预测值，N是状态偏差的预测时域，是控制输入偏差惩罚项，R是输入加权矩阵，表示k时刻对k+i时刻的控制输入偏差量的预测值，M是控制输入偏差量的预测时域；

5)定义预测模型向量描述，根据式(6)得预测模型为：

其中，是误差状态预测向量，是控制输入偏差预测向量，根据式(8)求得则Δg(k+i|k)的预测模型为：

其中，G(k)＝[Δg(k) … Δg(k+M-1)]为增益矩阵，R_r为ω_r时刻对应的曲率半径，综上得优化性能指标为：

6)令并根据式(11)、(12)、(13)定义二次规划问题描述：

其中，D＝H^TQH+G^TG+R，E^T＝(Fx(k))^TQH，d＝(Fx(k))^TQFx(k)，牛顿法迭代公式为：

其中，D称为海森矩阵，根据推算并依次后推，而根据牛顿法中的二次终止性求得：

故二次规划问题描述式(14)的极小值点为且中的第一项计算出当前k时刻的控制输入量

本发明的技术构思为：首先，建立线性系统下移动机器人的动力学模型，给出参数化期望路径的更新方程。接着，定义预测性能指标函数，结合状态空间方程推导出预测模型向量描述。进而，基于牛顿法二次规划求解最优控制量。最后，通过仿真实验分析算法的可行性，并为了说明该方法的性能，设计移动机器人路径跟踪控制系统实验平台验证算法的实际意义。

本发明的有益效果主要表现在：通过建立运动学误差模型，定义参数化路径与实际机器人状态有关，结合模型预测控制算法，牛顿法二次规划计算最优控制量，解决移动机器人路径跟踪过程中控制量输入速度与机器人实际速度不能快速匹配的问题。

附图说明

图1是移动机器人误差模型坐标建立示意图。

具体实施方式

下面结合附图对本发明作进一步描述。

参照图1，一种基于路径参数化的移动机器人预测跟踪控制方法，包括以下步骤：

1)建立机器人运动学模型，x＝[x,y,θ]^T为机器人实际位姿，(x,y)为机器人实际位置，θ为机器人实际角度，定义r＝[x_r,y_r,θ_r]^T为虚拟机器人参考位姿，(x_r,y_r)为虚拟机器人参考位置，θ_r为虚拟机器人参考角度，则机器人运动学模型为：

其中，v为机器人实际线速度，ω为机器人实际角速度，v_r为虚拟机器人参考线速度，ω_r为机器人参考角速度，跟踪误差模型为：

其中，[x_e,y_e,θ_e]为误差向量，(x_e,y_e)为实际位置与参考位置偏差，θ_e为角度偏差；

2)建立移动机器人线性误差模型，对式(3)求导得：

根据式(4)在平衡点线性化得如下的状态空间方程：

其中，为状态误差向量，为机器人控制输入偏差向量，矩阵矩阵对式(5)进行离散化得：

其中，k为采样时刻，为k时刻机器人状态误差向量，为k时刻控制输入偏差量， T_s为采样周期；

3)定义参数化的期望路径：

P＝{r(k)∈Rⁿ|r(k)＝p(θ_r(k))} (7)

其中，P为参数化期望路径，r(k)为k时刻参考位置，p(θ_r(k))为k时刻的路径，θ_r(k)为k时刻的路径参数，θ_r(k)参数更新方式为:

其中，ω_p(k)为k时刻路径期望角速度，为k时刻与控制输入偏差量相关的线性表达式，关系如下：

其中，λ是增益标量，C＝[c₁ c₂]是与控制输入误差向量有关的增益矩阵；

4)定义如下预测性能指标：

其中，是状态偏差惩罚项，Q是状态加权矩阵，表示k时刻对k+i时刻的状态预测值，是k时刻参考控制量与路径期望控制量的惩罚项，v_r(k+i|k)是k+i时刻参考线速度预测值，ω_r(k+i|k)是k+i时刻参考角速度预测值，v_p(k+i|k)是k+i时刻路径期望线速度预测值，ω_p(k+i|k)是k+i时刻路径期望角速度预测值，N是状态偏差的预测时域，是控制输入偏差惩罚项，R是输入加权矩阵，表示k时刻对k+i时刻的控制输入偏差量的预测值，M是控制输入偏差量的预测时域；

5)定义预测模型向量描述，根据式(6)得预测模型为：

其中，是误差状态预测向量，是控制输入偏差预测向量，根据式(8)求得则Δg(k+i|k)的预测模型为：

其中，G(k)＝[Δg(k) … Δg(k+M-1)]为增益矩阵，R_r为ω_r时刻对应的曲率半径，综上得优化性能指标为：

6)令并根据式(11)、(12)、(13)定义二次规划问题描述：

其中，D＝H^TQH+G^TG+R，E^T＝(Fx(k))^TQH，d＝(Fx(k))^TQFx(k)，牛顿法迭代公式为：

其中，D称为海森矩阵，根据推算并依次后推，而根据牛顿法中的二次终止性求得：

故二次规划问题描述式(14)的极小值点为且中的第一项计算出当前k时刻的控制输入量

Claims (1)

1.一种基于路径参数化的移动机器人预测跟踪控制方法，其特征在于，所述方法包括以下步骤：

1)建立机器人运动学模型，x＝[x,y,θ]^T为机器人实际位姿，(x,y)为机器人实际位置，θ为机器人实际角度，定义r＝[x_r,y_r,θ_r]^T为虚拟机器人参考位姿，(x_r,y_r)为虚拟机器人参考位置，θ_r为虚拟机器人参考角度，则机器人运动学模型为：

其中，v为机器人实际线速度，ω为机器人实际角速度，v_r为虚拟机器人参考线速度，ω_r为机器人参考角速度，跟踪误差模型为：

其中，[x_e,y_e,θ_e]为误差向量，(x_e,y_e)为实际位置与参考位置偏差，θ_e为角度偏差；

2)建立移动机器人线性误差模型，对式(3)求导得：

根据式(4)在平衡点线性化得如下的状态空间方程：

其中，为状态误差向量，为机器人控制输入偏差向量，矩阵矩阵对式(5)进行离散化得：

其中，k为采样时刻，为k时刻机器人状态误差向量，为k时刻控制输入偏差量， T_s为采样周期；

3)定义参数化的期望路径：

P＝{r(k)∈Rⁿ|r(k)＝p(θ_r(k))} (7)

其中，P为参数化期望路径，r(k)为k时刻参考位置，p(θ_r(k))为k时刻的路径，θ_r(k)为k时刻的路径参数，θ_r(k)参数更新方式为:

其中，ω_p(k)为k时刻路径期望角速度，为k时刻与控制输入偏差量相关的线性表达式，关系如下：

其中，λ是增益标量，C＝[c₁ c₂]是与控制输入误差向量有关的增益矩阵；

4)定义如下预测性能指标：

其中，是状态偏差惩罚项，Q是状态加权矩阵，表示k时刻对k+i时刻的状态预测值，是k时刻参考控制量与路径期望控制量的惩罚项，v_r(k+i|k)是k+i时刻参考线速度预测值，ω_r(k+i|k)是k+i时刻参考角速度预测值，v_p(k+i|k)是k+i时刻路径期望线速度预测值，ω_p(k+i|k)是k+i时刻路径期望角速度预测值，N是状态偏差的预测时域，是控制输入偏差惩罚项，R是输入加权矩阵，表示k时刻对k+i时刻的控制输入偏差量的预测值，M是控制输入偏差量的预测时域；

5)定义预测模型向量描述，根据式(6)得预测模型为：

其中，是误差状态预测向量，是控制输入偏差预测向量，根据式(8)求得则Δg(k+i|k)的预测模型为：

其中，G(k)＝[Δg(k) … Δg(k+M-1)]为增益矩阵，R_r为ω_r时刻对应的曲率半径，综上得优化性能指标为：

6)令并根据式(11)、(12)、(13)定义二次规划问题描述：

其中，D＝H^TQH+G^TG+R，E^T＝(Fx(k))^TQH，d＝(Fx(k))^TQFx(k)，牛顿法迭代公式为：

其中，D称为海森矩阵，根据推算并依次后推，而根据牛顿法中的二次终止性求得：

故二次规划问题描述式(14)的极小值点为且中的第一项计算出当前k时刻的控制输入量