[发明专利]一种基于模型预测控制的AGV轨迹修正方法

权利要求书

1.一种基于模型预测控制的AGV轨迹修正方法，其特征在于，该方法包括以下步骤：

(1)轨迹初始化；定义初始目标序列B，B是一个包含n个状态(即AGV位姿)n-1个运动控制输入和相邻两个状态的时间间隔的序列：

B:＝{s₁,u₁,ΔT₁,s₂,u₂,ΔT₂…s_n-1,u_n-1,ΔT_n-1,s_n}

其中ρ＝np+(n-1)(q+1)；p为状态的维度，q为运动控制输入的维度；

将初始轨迹等时间间隔表示为该轨迹上的n个AGV位姿s_i＝[x_i,y_i,β_i]^T组成的状态序列，其中x_i,y_i是AGV的坐标，β_i是AGV的朝向；轨迹的起始点位姿与目标点位姿分别用s_s和s_e表示；

运动控制输入u初始化为0，ΔT初始化为默认值ΔT_ref；

(2)调整目标序列；该步骤包括以下两个部分：

(2.1)在每个采样周期，起始点为当前的AGV位姿，即每个周期更新s₁；

(2.2)在每次迭代优化目标序列之后，微调目标序列，遵行以下规则：

遍历目标序列，给定一个较小的值∈，如果ΔT_k＞ΔT_ref+∈，则在s_k和s_k+1之间按时间线性插入一个状态那么s_k和s_new的时间间隔为0.5ΔT_k，s_new和s_k+1的时间间隔为0.5ΔT_k；如果ΔT_k＜ΔT_ref-∈则，删除掉状态s_k+1，那么新的时间间隔；

(3)对AGV目标序列表示的轨迹进行基于模型预测控制的优化；优化目标函数为：

其中由约束对应的惩罚项组成，B^*为最优序列；

优化目标函数的约束如下：

(3.1)AGV动力学模型约束：

首先，AGV动力学模型如下：

下发控制量的时候对AGV动力学模型引入一阶惯性环节：

其中u(s)表示下发的输入控制量，表示预测的AGV驱动电机实际控制量，参数T与电机参数和实际负载相关；

在引入一阶惯性环节之后建立预测的AGV动力学模型：

将该预测的AGV动力学模型离散化，记为：

u_f(k)表示当前时刻的AGV运动线速度和角速度反馈，；

那么AGV动力学模型约束h定义如下：

(3.2)AGV行驶时间约束g定义如下：

(3.3)AGV的速度与加速度约束：

线速度

角速度

线加速度

角加速度

其中γ(s_k+1,s_k)表示AGV的运动方向；

给定最大线速度v_max、最大线加速度a_max、最大角速度ω_max和最大角加速度则有以下不等式约束：

v_k(s_k+1,s_k,ΔT_k)＝[v_max-|v_k|,ω_max-|ω_k|]^T＞0

(3.4)AGV与障碍物的距离约束：

将表示为位姿s_k与障碍物的距离，l＝1,2…R，R为障碍物的总个数；设定AGV与障碍物的最短距离为ρ_min，则障碍物的约束o_k为：

其中对于未来位姿s_k+1到s_n观察到的障碍物，以障碍物的当前速度v_ob作为参考，观测到的障碍物当前位置记为s_ob，用点状模型预测障碍物的k时刻位置；

将约束进行简化，将以上等式约束对应的惩罚项φ记为：

将以上不等式约束对应的惩罚项χ记为：

其中σ_L,σ_M表示对应的惩罚项的权重,L_k为以上等式约束对应的函数，M_k为以上不等式约束对应的函数；

那么，表示为：

(4)求解目标函数；使用g2o框架求解优化目标函数，得到优化后的修正轨迹，并返回步骤(2)迭代N次；

(5)下发控制量；迭代完成后，下发最优序列B^*中的运动控制输入量给AGV驱动电机，控制AGV运动；

(6)反馈信息，构成闭环系统；AGV沿着修正轨迹运动，并获取AGV实时速度和位姿，以及障碍物的运动信息，反馈回步骤(3)用于更新下一采样周期的目标函数。