[发明专利]一种基于路径参数化的移动机器人预测跟踪控制方法

权利要求书

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时刻的控制输入量