一种基于模型预测控制的AGV轨迹修正方法与流程

本发明属于机器人导航控制领域，尤其涉及一种基于模型预测控制的agv轨迹修正方法。

背景技术：

在如今物流产业飞速发展的背景下，机器人正越来越受到先进的物流工厂的注意。自动导引车辆更是成为众多企业工厂的内部物流智能化潮流中不可缺少的一部分。其中自主导航技术对于提升物流的全自动化和智能化有着重要的促进作用，也是成为了近些年学者们研究的热点。

路径规划是求解一条最优的无碰撞轨迹，传统的全局路径规划方法有a*、dijkstra等方法。但是在机器人运行过程中，由于地图局部性和障碍物的动态特性，原有的全局路径可能无法支持机器人的无碰撞运行，因此局部路径规划是不可缺少的。假设全局路径规划器已经生成了可行路径，越来越多的学者更倾向于对这条初始路径进行动态修改，结合传感器数据来实时的处理动态环境，从而在线修改轨迹，进行局部优化。这相比于重新规划全局路线在效率上和实时性上都有着明显的优势。

技术实现要素：

本发明的目的在于利用机器人运动模型预测未来的运动，提供一种基于模型预测控制的agv轨迹修正方法，该策略在初始的给定的路径基础上，通过轨迹优化器，对其进行后续的修正，最终生成一条机器人无碰撞运动轨迹。在轨迹优化问题中考虑到的约束有轨迹运行时间、与动态障碍物的距离、运动学和几何约束等。另外引入模型预测，并在满足约束的同时优化给定目标序列，在每个时间步长里解决优化问题，最后规划出时间最优轨迹。模型预测控制将最佳轨迹的规划与控制回路中的序列状态反馈相结合，从而形成闭环控制系统。考虑到电机的特性，本发明在预测模型中加入一阶惯性环节，使得对电机的速度跟踪的效果更好。在解决优化问题时，考虑到目标的局部性，因为它们只依赖于连续的几个agv的状态，因此采用了g2o框架求解器，最终可以较好的得到优化后的目标轨迹。

本发明的目的是通过以下技术方案来实现的：一种基于模型预测控制的agv轨迹修正方法，该方法包括以下步骤：

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

b:＝{s1,u1,δt1,s2,u2,δt2…sn-1,un-1,δtn-1,sn}

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

将初始轨迹等时间间隔表示为该轨迹上的n个agv位姿si＝[xi,yi,βi]^t组成的状态序列，其中xi,yi是agv的坐标，βi是agv的朝向。轨迹的起始点位姿与目标点位姿分别用ss和se表示。

运动控制输入u初始化为0，δt初始化为默认值δtref。

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

(2.1)在每个采样周期，起始点为当前的agv位姿，即每个周期更新s1。

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

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

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

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

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

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

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

s(t＝0)＝ss

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

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

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

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

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

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

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

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

线速度

角速度

线加速度

角加速度

其中γ(sk+1,sk)表示agv的运动方向(前进为1，倒退为-1)。

给定最大线速度vmax、最大线加速度amax、最大角速度ωmax和最大角加速度则有以下不等式约束：

vk(sk+1,sk,δtk)＝[vmax-|vk|,ωmax-|ωk|]^t＞0

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

将表示为位姿sk与障碍物的距离，l＝1,2…r，r为障碍物的总个数；设定agv与障碍物的最短距离为ρmin，则障碍物的约束ok为：

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

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

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

其中σl,σm表示对应的惩罚项的权重,lk为以上等式约束对应的函数，mk为以上不等式约束对应的函数。

那么，表示为：

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

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

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

进一步地，所述步骤(2)中，目标序列调整是为了动态的更改轨迹的长度，因为当有障碍物出现在运动轨迹上时，每次迭代优化后的轨迹会为了避开障碍物而增加轨迹长度，此时需要动态的增加状态个数n，从而增加轨迹的平滑性。

进一步地，所述步骤(3.1)中，参数t的具体标定方式如下：给定电机参数，在负载x状态下给予电机定加速度的速度变化值将下发的速度值列表记为l1，同时读取电机响应速度值列表l2。然后利用matlab分别模拟l1和l2两条线，对l1引入一阶惯性环节，仿真后得到的线记为l3，改变参数t使得l3与l2这两条线基本重合，即得到该参数t。

进一步地，所述步骤(3.3)中，相邻两个位姿连线成的向量可以表示为dk,k+1＝[xk+1-xk,yk+1-yk,0]^t，第k个位姿的朝向可以表示为qk＝[cosβk,sinβk]^t，那么γ(sk+1,sk)按以下规则计算：

进一步地，所述步骤(4)中，求解的目标函数是最小二乘函数，由于优化目标的局部性，大部分目标只与连续的几个agv状态和控制量有关，具有稀疏性的特点，使用g2o框架求解器可以快速得到目标函数的最优解。对于得到的最优解，增加n次迭代，对其中的时间间隔进行检查，利用步骤(2)中的规则，动态的增加或者删除状态，从而提高轨迹的平滑性。

本发明的有益效果是：本发明提供了一种基于模型预测控制的agv轨迹修正方法，是一种实时在线对初始轨迹进行优化的方法。该方法考虑了移动机器人带有电机特性的运动模型和动力学约束，同时考虑了对动态障碍物的有效避障和速度加速度的限制，生成一条准时间最优的轨迹。优化的目标具有稀疏性的结构，可以使用求解稀疏性结构的开源框架计算。该算法实时运行，直接为底层机器人运动控制器生成命令，并且具有高度的灵活性，容易适应不同机器人运动学和应用要求。

附图说明

图1为本发明方法实现流程图。

具体实现方法

下面结合附图说明进一步详细描述本方法的执行步骤，但本发明的保护范围不局限于以下所述。

本发明提供的一种基于模型预测控制的agv轨迹修正方法，该方法使得运动轨迹能有效地避开动态的障碍物，并在时间上快速的到达目标点，达到自主导航的目的。如图1所示，本方法整体为一个闭环系统，具体步骤如下：

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

b:＝{s1,u1,δt1,s2,u2,δt2…sn-1,un-1,δtn-1,sn}

其中ρ＝np+(n-1)(q+1)；p为状态的维度，q为运动控制输入的维度，本实施例中，p取3，即包括agv的横纵坐标及agv的朝向，q取2，即包括agv的线速度和角速度。

将初始轨迹等时间间隔表示为该轨迹上的n个agv位姿si＝[xi,yi,βi]^t组成的状态序列，其中xi,yi是agv的坐标，βi是agv的朝向。轨迹的起始点位姿与目标点位姿分别用ss和se表示，则有s1＝ss,sn＝se。另外运动控制输入u初始化为0，δt初始化为一个默认值δtref，比如可取0.1s。

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

(2.1)在每个采样周期，起始点就是当前的agv位姿，即每个周期更新s1。

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

遍历目标序列，给定一个较小的值∈(比如可取0.1δtref)，如果δtk＞δtref+∈,则在sk和sk+1之间按时间线性插入一个状态那么sk和snew的时间间隔为0.5δtk，snew和sk+1的时间间隔为0.5δtk；同理如果δtk＜δtref-∈,则删除掉状态sk+1，那么新的时间间隔

目标序列调整是为了动态的更改轨迹的长度，因为当有障碍物出现在运动轨迹上时，每次迭代优化后的轨迹会为了避开障碍物而增加轨迹长度，此时需要动态的增加状态个数n，从而增加轨迹的平滑性。

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

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

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

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

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

其中时间独立的状态对应的运动控制输入

考虑到agv驱动电机的特性，在给定电机一定速度时，电机并不能马上到达该速度，因此下发控制量的时候对agv动力学模型引入一阶惯性环节，使得更精准的得到agv驱动电机的实际控制量，提高模型预测控制的准确性：

其中u(s)表示下发的输入控制量，表示预测的驱动电机实际控制量，参数t与电机参数和实际负载相关，具体标定方式如下：给定电机参数(即电机参数固定)，在负载x(重量)状态下给予电机定加速度的速度变化值将下发的速度值列表记为l1，同时读取电机响应速度值列表l2。然后利用matlab分别模拟l1和l2两条线，对l1引入一阶惯性环节，仿真后得到的线记为l3，改变参数t使得l3与l2这两条线基本重合，即得到该参数t。在引入一阶惯性环节之后建立预测的agv动力学模型：

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

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

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

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

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

首先有如下计算式：

线速度

角速度

线加速度

角加速度

其中γ(sk+1,sk)表示agv的运动方向(前进为1，倒退为-1)。相邻两个位姿连线成的向量可以表示为dk,k+1＝[xk+1-xk,yk+1-yk,0]^t，第k个位姿的朝向可以表示为qk＝[cosβk,sinβk]^t，那么γ(sk+1,sk)按以下规则计算：

agv的行驶速度和加速度是有限制的，避免为了追求时间最优δtk→0导致vk→∞。给定最大线速度vmax、最大线加速度amax、最大角速度ωmax和最大角加速度则有以下不等式约束：

vk(sk+1,sk,δtk)＝[vmax-|vk|,ωmax-|ωk|]^t＞0

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

agv需要与障碍物保持安全的避障距离。将表示为位姿sk与障碍物的距离，l＝1,2…r，r为障碍物的总个数；设定agv与障碍物的最短距离为ρmin，则障碍物的约束ok为：

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

以上为目标函数的四个约束。

考虑到优化问题的目的是对agv合理控制，以便在最小的时间内将agv从起始点位姿ss转移到目标点位姿se，同时需要满足上述四个约束。由于求解带约束的优化问题是非常消耗计算量的，而无约束优化则避免了拉格朗日乘数，因此本策略采用最小二乘优化问题来描述：

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

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

其中σl,σm表示对应的惩罚项的权重,lk为以上等式约束对应的函数，mk为以上不等式约束对应的函数。

那么，可以表示为：

(4)求解目标函数。使用g2o框架求解优化目标函数，得到优化后的修正轨迹，并返回步骤(2)迭代n次(n为外部参数设置，n通常取2或3)。

求解的目标函数是最小二乘函数，由于优化目标的局部性，大部分目标只与连续的几个agv状态和控制量有关，具有稀疏性的特点，因此使用g2o框架求解器可以快速得到目标函数的最优解。对于得到的最优解，增加n次迭代，对其中的时间间隔进行检查，利用步骤(2)中的规则，动态的增加或者删除状态，从而提高轨迹的平滑性。

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

每个采样周期将最优解中的下发给agv，这是基于模型预测控制的基本思想。过程的当前状态作为最优控制问题的初始状态，解得的最优控制序列只实施第一个控制作用，使得最优控制考虑到了未来的agv运动行为约束，提高了系统的性能。

(6)反馈信息，构成闭环系统。agv沿着修正轨迹运动，并获取agv实时速度和位姿，以及障碍物的运动信息，反馈回步骤(3)用于更新下一采样周期的目标函数，增加了预测控制抗扰动和系统不确定性的能力。

本发明融合了电机特性，提供一种动力学模型用于机器人的模型预测控制。该方法能控制机器人在行走路线上有效地避开障碍物，同时不违背运动学规律，快速地到达目标点。优化的目标具有稀疏性的结构，可以使用求解稀疏性结构的开源框架计算。该方法实时运行，直接为底层机器人运动控制器生成命令，并且具有高度的灵活性，容易适应不同机器人运动学和应用要求。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和变型，这些改进和变型也应视为本发明的保护范围。