[发明专利]基于全向移动机器人运动学建模的误差模型预测控制方法

权利要求书

1.基于全向移动机器人运动学建模的误差模型预测控制方法，其特征在于，所述方法包括以下步骤：

S11、建立FM-OMR四个麦克纳姆轮之间的速度约束运动学模型，具体包括：

S111、首先对FM-OMR的结构进行描述；

S112、其次对FM-OMR的速度约束进行描述：

将车轮的转速表示为车轮的平移速度表示为而V_ir，i＝1，2，3，4，是自由移动辊的切向速度，将机器人框架中轮子的合成速度表示为：

V_1x＝V_1m+V_1rcosα₁；V_1y＝V1_rsinα₁

V_2x＝V_2m+V_2rcosα₂；V_2y＝V_2rsinα₂

V_3x＝V_3m+V_3rcosα₃；V_3y＝V_3rsinα₃

V_4x＝V_4m+V_4rcosα₄；V_4y＝V_4rsinα₄

其中α₁＝α₃＝45，α₂＝α₄＝-45，V_1m、V_2m、V_3m、V_4m分别表示四个麦克纳姆轮的平移速度，V_1y、V_2y、V_3y、V_4y表示分别表示四个麦克纳姆轮沿y轴的合计速度，此外，由于轮速与机器人本体的速度是刚性连接的，四个轮子速度V_1x、V_2x、V_3x、V_4x与机器人本体的速度关系建立为：

V_1x＝v_x-l₁w；V_1y＝v_y+l₂w

V_2x＝v_x+l₁w；V_2y＝v_y+l₂w

V_3x＝v_x-l₁w；V_3y＝v_y-l₂w

V_4x＝v_x+l₁w；V_4y＝v_y-l₂w

因此，获得FM-OMR机体的速度以及四个轮子的角速度的关系如下：

其中v_x、v_y是机体沿着x、y方向的速度，ω是机体沿着z方向的角速度，其中R是如下雅可比矩阵：

其中l₁、l₂表示车轮到质心的纵向和横向距离；R_w表示麦克纳姆轮的半径；

其中矩阵R不是方阵，因此没有逆矩阵，然而，广义逆矩阵表示为：

因此得到：

由上面获得：

从上式得到以下等式：

ω₁+ω₂＝ω₃+ω₄

调整四个麦克纳姆轮各自的速度，实现在给定的平面内期望的全向运动，每个轮子的约束产生作用于整个机体，四个麦克纳姆轮之间的速度约束关系由ω₁+ω₂＝ω₃+ω₄表示，车身速度与四个轮子角速度之间的关系已经得出，机器人本体的速度表示如下：

其中，

S113、最后构建FM-OMR的运动学模型：

从上述式子，得出非完整FM-OMR的运动学模型如下：

S12、建立FM-OMR的跟踪误差运动学模型，具体过程为：

S121、确定领导者R_r以及跟随者R的物理参数，其中对于领导者小车，有：

其中，l₃、l₄表示车轮到质心的纵向和横向距离；

S122、根据领导者以及跟随者在世界坐标系下的位置和角度计算R_r在R坐标系下的相对位置和角度(x_e，y_e，θ_e)：

其中，(x，y，θ)以及(x_r，y_r，θ_r)是分别都是对应于R和R_r的惯性协同坐标系；

S123、根据所得的(x_e，y_e，θ_e)计算其导数，建立其导数与输入[u₁，u₂，u₃]之间的状态方程，所述导数表示为：

其中，

a＝w_1r+w_2r+w_3r+w_4r

b＝w_1r-w_2r-w_3r+w_4r

c＝w₁+w₂+w₃+w₄

d＝w₁-w₂-w₃+w₄

其中，a、b是R_r四个车轮角速度的线性组合，c、d是R四个车轮角速度的线性组合；

则综上四个麦克纳姆轮之间具有速度约束的轨迹跟踪误差运动学模型为：

其中，

u₃＝w_r-w；

S124、通过控制输入[u₁，u₂，u₃]，误差状态量[x_e，y_e，θ_e]收敛到原点；

S13、针对FM-OMR的轨迹跟踪问题，针对跟踪误差运动学模型，设计结合速度约束方程的误差模型预测控制器；设计结合速度约束方程的误差模型预测控制器时，连续时变系统用离散时间模型的形式重新表示，每个特征点的离散时间状态空间模型表示为：

z_e(k+1)＝L(k)z_e(k)+M(k)u(k)

其中，表示输入向量，表示状态量，参数n和m表示状态和输入变量的数量；根据采样定理，给定连续时间系统用采样周期T离散化，因此，上式写成如下形式：

所述状态空间模型中，为了导出给定预测层N_p的最优控制序列，损失函数表示为：

其中，Δu(k)表示输入向量的增量，有Δu(k)＝u(k)-u(k-1)，z_e(k+j|k)和Δu(k+j|k)表示基于当前时间k，在时间k+j，上的预测变量，此外，N_c代表控制层，N_p代表预测层，最后，Q和P表示适当的加权矩阵，T为采样周期；

从以上式子得出：

定义如下形式的预测向量：

其中，N_p＝3，N_c＝2，且对于所有k≥0时刻，约束表示如下：

其中状态量z_e的最大值和最小值输入向量的最大值和最小值输入向量的增量Δu(k)的最小值和最大值均为某一给定的常数；

预测状态表示为如下形式：

最终，基于约束条件下代价函数的EMPC算法通过解以下约束来对问题进行优化：

这取决于：