1.一种基于路径参数化的移动机器人预测跟踪控制方法,其特征在于,所述方法包括以下步骤:
1)建立机器人运动学模型,x=[x,y,θ]T为机器人实际位姿,(x,y)为机器人实际位置,θ为机器人实际角度,定义r=[xr,yr,θr]T为虚拟机器人参考位姿,(xr,yr)为虚拟机器人参考位置,θr为虚拟机器人参考角度,则机器人运动学模型为:其中,v为机器人实际线速度,ω为机器人实际角速度,vr为虚拟机器人参考线速度,ωr为机器人参考角速度,跟踪误差模型为:其中,[xe,ye,θe]为误差向量,(xe,ye)为实际位置与参考位置偏差,θe为角度偏差;
2)建立移动机器人线性误差模型,对式(3)求导得:根据式(4)在平衡点线性化得如下的状态空间方程:其中, 为状态误差向量, 为机器人控制输入偏差向量,矩阵 矩阵 对式(5)进行离散化得:其中,k为采样时刻, 为k时刻机器人状态误差向量,为k时刻控制输入偏差量,
Ts为采样周期;
3)定义参数化的期望路径:
P={r(k)∈Rn|r(k)=p(θr(k))} (7)其中,P为参数化期望路径,r(k)为k时刻参考位置,p(θr(k))为k时刻的路径,θr(k)为k时刻的路径参数,θr(k)参数更新方式为:其中,ωp(k)为k时刻路径期望角速度, 为k时刻与控制输入偏差量相关的线性表达式,关系如下:其中,λ是增益标量,C=[c1 c2]是与控制输入误差向量有关的增益矩阵;
4)定义如下预测性能指标:
其中, 是状态偏差惩罚项,Q是状态加权矩阵, 表示k时刻对k+i时刻的状态预测值, 是k时刻参考控制量与路径期望控制量的惩罚项,vr(k+i|k)是k+i时刻参考线速度预测值,ωr(k+i|k)是k+i时刻参考角速度预测值,vp(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)]为增益矩阵,Rr为ωr时刻对应的曲率半径,综上得优化性能指标为:
6)令 并根据式(11)、(12)、(13)定义二次规划问题描述:其中,D=HTQH+GTG+R,ET=(Fx(k))TQH,d=(Fx(k))TQFx(k),牛顿法迭代公式为:其中, D称为海森矩阵,
根据 推算 并依次后推,而根据牛顿法中的二次终止性求得:
故二次规划问题描述式(14)的极小值点为 且 中的第一项计算出当前k时刻的控制输入量