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

摘要

本发明公开了一种基于模型预测控制的AGV轨迹修正方法。在初始的全局规划路径的基础上，通过轨迹优化器对其进行后续的修正，从而达到生成一条AGV无碰撞运动轨迹的目的。在轨迹优化问题中考虑轨迹运行时间、与动态障碍物的距离、运动学和几何约束等。采用模型预测控制，使优化器在模型的帮助下能预测AGV未来姿态，并在满足约束的同时优化给定目标序列，在每个时间步长里解决优化问题，最后规划出时间最优轨迹。模型预测控制将最佳轨迹的规划与控制回路中的序列状态反馈相结合，从而形成闭环控制。考虑到电机的特性，在预测模型中加入一阶惯性环节来模拟。在解决优化问题时，使用g2o框架求解器，最终能够较好的得到优化后的目标轨迹。

主权项

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)用于更新下一采样周期的目标函数。