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

摘要

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

主权项

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