CN114355923B - 一种a*引导下基于mpc的轨迹规划及跟踪方法 - Google Patents
一种a*引导下基于mpc的轨迹规划及跟踪方法 Download PDFInfo
- Publication number
- CN114355923B CN114355923B CN202111624520.6A CN202111624520A CN114355923B CN 114355923 B CN114355923 B CN 114355923B CN 202111624520 A CN202111624520 A CN 202111624520A CN 114355923 B CN114355923 B CN 114355923B
- Authority
- CN
- China
- Prior art keywords
- node
- cost
- point
- planning
- openlist
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 27
- 238000011084 recovery Methods 0.000 claims abstract description 13
- 230000003068 static effect Effects 0.000 claims abstract description 13
- 238000011156 evaluation Methods 0.000 claims abstract description 7
- 239000011159 matrix material Substances 0.000 claims description 32
- 239000010931 gold Substances 0.000 claims description 9
- 229910052737 gold Inorganic materials 0.000 claims description 9
- 230000004888 barrier function Effects 0.000 claims description 8
- 238000004364 calculation method Methods 0.000 claims description 8
- PCHJSUWPFVWCPO-UHFFFAOYSA-N gold Chemical compound [Au] PCHJSUWPFVWCPO-UHFFFAOYSA-N 0.000 claims description 6
- 230000008569 process Effects 0.000 claims description 6
- 230000009471 action Effects 0.000 claims description 4
- 238000005457 optimization Methods 0.000 claims description 3
- 239000003086 colorant Substances 0.000 claims description 2
- 230000006870 function Effects 0.000 abstract description 31
- 238000011161 development Methods 0.000 description 4
- 241000282414 Homo sapiens Species 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 3
- 230000000694 effects Effects 0.000 description 2
- 230000003993 interaction Effects 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 235000006629 Prosopis spicigera Nutrition 0.000 description 1
- 240000000037 Prosopis spicigera Species 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000009776 industrial production Methods 0.000 description 1
- 238000011835 investigation Methods 0.000 description 1
- 238000010801 machine learning Methods 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000003058 natural language processing Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
- 238000011144 upstream manufacturing Methods 0.000 description 1
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明涉及一种A*引导下基于MPC的轨迹规划及跟踪方法。本发明通过改进的A*算法依据静态栅格化地图进行全局路径规划，依据在栅格地图上移动规则的不同，确定不同的启发函数，更够能快更准确地寻找到最短路径，并且在代价评估函数中加入偏移代价，能够最大程度上避免出现同代价随机选择的不确定情况，提高了全局路径规划的准确性；通过改进的MPC算法依靠代价地图进行轨迹规划和轨迹跟踪，这使得轨迹规划和轨迹跟踪能够同时进行，提高移动效率；在AGV自主导航失败不移动的情况下，提供自恢复策略，使移动机器人能够最大可能的从避障失败中脱离并重新运转起来，保证机器人能够继续向着目标点移动。
Description
技术领域
本发明属于自主导航机器人导航技术领域，涉及一种A*引导下基于MPC的轨迹规划及跟踪方法。
背景技术
21世纪以来，随着仓储物流、医疗护理、公共服务、探险救援等行业的迅速发展，当今社会对移动机器人的需求逐渐增长，使得移动机器人技术得到了可持续的科学研究与技术突破。在军事上，移动机器人可以代替人类在危险的地带完成排雷，侦查等危险任务；在民用上，有从事货物搬运的自主导航机器人(Autonomous Guided Vehicle，AGV)。
作为一种协助人类完成任务的自主移动设备，移动机器人通常由五个主要的功能单元构成：机械结构、运动学控制、周围环境感知、运算器控制和人机交互，并涉及机械结构设计、自动化控制理论、计算机科学等多个学科。移动机器人作为机器人领域的重要组成部分，近年来，随着计算机视觉、机器学习、自然语言处理和语音识别等人工智能核心技术的快速发展，以及相关上下游技术产业链的打通，使移动机器人技术得到了前所未有的发展，尤其是在人机交互方面，有了更人性化和智能化的进步，对人类生活方式的改善、工业生产的技术变革均起到了重大推动作用。
自主移动作为移动机器人最重要的智能化体现，路径和轨迹规划则是其中的关键支撑技术。面对复杂多变的现实环境时，如何有效地规划出合理路径和轨迹，使机器人快速安全地越过各种障碍物，顺利到达目标位置并完成规定任务，是考量移动机器人的移动规划是否科学合理，避障性能是否优越的重要参考指标。目前市面上的移动机器人在静态工作环境下的路径规划效果一般，存在待优化的空间，而且在复杂多变的动态环境下导航效果更差，如无法越过障碍物或容易陷入局部最小值等。因此，针对室内复杂多变的不确定性环境，尤其是生产车间内的复杂环境，机器人能够综合避障性能要求、环境特征和机器人自身特性等多方需求，进行科学合理的移动规划，使机器人更智能化的避开动态和静态障碍物，这一需求显得尤为迫切。
发明内容
本发明的目的就是提供一种A*引导下基于MPC的轨迹规划及跟踪方法，具体为基于A*全局路径规划算法和MPC轨迹跟踪算法的AGV动态避障方法。
具体包括如下步骤：车载雷达先行进行静态地图的感知与搭建，搭建完成后进行栅格化处理；依靠车辆自身的定位器与传感器获取当前车身位姿，而后利用改进的A*算法进行全局路径规划，全局路径规划完成后确定运动轨迹参考点，依据参考点计算各类参考值；确定好起始参考值后进行基于MPC算法的轨迹规划和轨迹跟踪，若AGV避障失败陷入死区，即MPC轨迹无效，采取三级自恢复行为策略，按照等级重新规划轨迹；若MPC轨迹有效则输出运动控制参数，AGV朝规划点移动，并判断是否达到目标点位姿要求，达到要求则结束导航避障，否则进行新一轮的MPC轨迹规划和轨迹跟踪；
本发明通过改进的A*算法依据静态栅格化地图进行全局路径规划，依据在栅格地图上移动规则的不同，确定不同的启发函数，更够能快更准确地寻找到最短路径，并且在代价评估函数中加入偏移代价，能够最大程度上避免出现同代价随机选择的不确定情况，提高了全局路径规划的准确性；改进的MPC算法依靠代价地图进行轨迹规划和轨迹跟踪，这使得轨迹规划和轨迹跟踪能够同时进行，提高移动效率；自恢复策略在AGV自主导航失败不移动的情况下，提供一种自主恢复行为，使移动机器人能够最大可能的从避障失败中脱离并重新运转起来，保证机器人能够继续向着目标点移动。
所述的改进的A*全局路径规划算法包括以下步骤：
步骤A1：依靠AGV所携带的雷达感知地图，此地图为静态地图G，并将地图进行栅格化处理，用不同颜色标注可通行区域和不可通行区域。给定全局目标与终点位姿，并获取机器人当前位姿。
步骤A2：栅格化静态地图G后，创建两个空表OpenList和CloseList，将给定的路径起点(父节点)添加到CloseList中，并且将起点附近的m个子节点放到OpenList表中。
步骤A3：判断OpenList表中是否为空，空则结束，非空则继续执行。首先遍历OpenList表里的所有子节点，根据代价评估函数f(n)查找出代价最小的子节点nbest，并将其视为新的父节点，最后把nbest从OpenList中移除并添加至CloseList中。
其中评估函数f(n)＝g(n)+h(n)+p(n)，f(n)是从初始点到子节点n，再到目标节点的代价估计函数；g(n)表示从初始点到子节点n的实际成本函数；h(n)是启发函数，是从子节点n到目标节点goal的移动代价函数；p(n)是偏移代价，防止出现只有前两项g(n)与h(n)相加，使得f(n)相等，加上偏移代价能够使得A*算法扩展更少的节点得到最短全局路径。
实际成本函数g(n)，采取曼哈顿计算成本，把横向和纵向移动一个节点的代价定义为c1。斜向(对角线)移动代价c2。
h(n)启发函数可根据在栅格上的移动方式作如下选择：
当前OpenList中的子节点到目标节点的移动方式只有横向和竖向时，采用曼哈顿距离h1(n)＝c1(|n.x-goal.x|+|n.y-goal.y|)；当前OpenList中的子节点到目标节点的移动方式允许横向、纵向和对角线方向移动，并且移动代价都是c1时，采用欧拉距离：
当前OpenList中的子节点到目标节点的移动方式允许横向、纵向和对角线方向移动，但横向纵向移动代价是c1，对角线方向移动代价是c2时，采用对角线距离：
其中h1(n)是曼哈顿距离启发函数，h2(n)是欧拉距离启发函数，h3(n)是对角线距离启发函数；n.x，n.y是子节点的横纵坐标；goal.x，goal.y是目标点的横纵坐标；hdiagonal(n)是沿着对角线移动的步数；hstraight(n)是曼哈顿距离。
计算偏移代价p(n)：
其中start.x，start.y是路径起始点的横纵坐标，dx1是子节点横坐标跟目标点横坐标差值，dy1是子节点纵坐标跟目标点纵坐标差值，dx2是路径起始点跟目标点横坐标差值，dy2是路径起始点跟目标点纵坐标差值，D是起始节点到目标节点的向量和当前n节点到目标节点的向量的内积，h(n)是启发函数，μ是调节系数。
步骤A4：选择nbest节点后，将当前nbest节点相邻的8个新子节点逐个进行新的遍历，若某个相邻子节点已经在CloseList中或该节点是不通过的即为障碍物，则丢弃之，不做处理。否则，对该相邻节点继续执行下面的流程：(i)如果该相邻节点不在OpenList中，则将其添加到OpenList，当前nbest节点为它的父节点。(ii)如果该相邻节点已在OpenList中，则通过f(n)值判断这条路径是否为最近路径。如果是最近的，就把该子节点纳入CloseList表中作为新的父节点。
步骤A5：重复上述A3、A4两步，直至CloseList表中出现目标节点goal，规划结束，并把CloseList表中所有节点按序列用线段相连即得到了最短全局路径。
所述的改进的MPC轨迹规划和跟踪算法包括以下步骤：
步骤B1：确定状态量和控制量建立车辆模型并写成状态方程的形式，模型可以为运动学模型或者动力学模型。建立模型后将其离散化，离散后的模型总：
其中是k+1时刻的状态量偏差矩阵，/>是k时刻的状态量偏差矩阵，是k时刻的控制量偏差矩阵，y(k+1)是输出矩阵，A矩阵与B矩阵与是跟模型相关的系数矩阵，C′矩阵是根据输出需要设计的输出矩阵。
步骤B2：在k时刻依据全局路径计算车辆的参考位置。计算方法有以下两种：(i)选择车辆后轮轴距中心，做两条平行于x轴和y轴的两条直线与全局路径相交得到两个相交点，分别计算两个相交点与后轮轴距中心的距离，选择距离较短的点作为参考点；(ii)选择车辆后轮轴距中心，计算后轮轴距中心到全局路径的垂直距离，并选择垂直距离与全局路径上的交点作为参考点。若此时k时刻是车辆的起始时刻，那么该参考点就是轨迹的起始点。在选择好参考点和模型离散化后，计算各类参考值。
步骤B3：基于 进行优化求解(N为预测步长)，在k时刻基于输入偏差/>以及系统模型预测输出y(k+1)，k+1时刻基于输入偏差及模型预测输出y(k+2)，k+2时刻基于输入偏差/>及模型预测输出y(k+3)......k+N时刻基于输入偏差/>及模型预测输出y(k+N)。y(k+1)、y(k+2)、y(k+3)......y(k+N)组成预测步长，/>组成控制步长。
通过构建代价函数进行最小值求解，
确定
代价函数
式中Q、R、F均为对角矩阵，e(k+i|k)表示在k时刻对k+i时刻做出的预测误差，x(k+i|k)表示在k时刻对k+i时刻做出的预测状态量，xr(k)表示k时刻的状态参考值，表示在k时刻对k+i时刻做出的预测输入控制量偏差值，e(k+N|k)表示在k时刻对k+N时刻做出的终端预测误差，Val(k)是k时刻是车辆雷达感知与障碍物距离得到的栅格值，η表示调节系数，栅格值大小为设定值。
用来表示e(k+i|k)，使得式中只有/>一个变量，使得代价函数J化为标准的二次规划型。
步骤B4：根据计算出的得到u(k)，并施加u(k)进入系统，得到系统的真实输出yreal(k+1)，作为k+1时刻的初始状态，并把预测区间和控制区间往左移动一个单位重新进行预测，求出此时使得代价函数J最小的/>和u(k+1)，多次迭代直到AGV达到目标点位姿要求。
本发明中三级自恢复行为策略具体过程为：采取“自旋-障碍物清除-再规划”的三级自恢复行为策略。第一级策略是原地自选一周将周围障碍物清除并重新感知，再调用局部规划器重新路径规划；如果仍然失败，则调用第二级策略，先移动一小段安全距离，再调用局部规划器重新规划路径；如果经过上述过程仍未奏效，则调用第三级策略，在地图上将动态障碍物更新到全局地图上，重新调用全局路径规划并检测路径是否可通行。如果路径不可行，则直接放弃本次任务，等待下一次任务。
附图说明
图1是本发明实施实时避障的流程图。
具体实施方式
下面结合附图说明本避障方法具体实施流程。如图1所示，一种A*引导下基于MPC的轨迹规划及跟踪方法，具体包括如下步骤：车载雷达先行进行静态地图的感知与搭建，搭建完成后进行栅格化处理。并且依靠车辆自身的定位器与传感器获取当前车身位姿，而后利用改进的A*算法进行全局路径规划，全局路径规划完成后确定运动轨迹参考点，并依据参考点计算各类参考值。确定好起始参考值后便进行基于MPC算法的轨迹规划和轨迹跟踪，若AGV避障失败陷入死区，即MPC轨迹无效，便采取三级自恢复行为策略，按照等级以不同方法重新规划轨迹。若MPC轨迹有效则输出运动(控制)参数，AGV朝规划点移动，并判断是否达到目标点位姿要求，达到要求则结束导航避障，否则进行新一轮的MPC轨迹规划和轨迹跟踪。
以下说明改进的A*全局路径规划算法、改进的MPC轨迹规划和跟踪算法以及移动规划失败的自恢复策略的具体实施方式。
本实施例中改进的A*全局路径规划算法具体实施方式包括以下步骤：
步骤A1：依靠AGV所携带的雷达感知地图，此地图为静态地图G，并将地图进行栅格化处理，白色为可以通行的区域，黑色为不可通行的区域即为障碍物。给定全局目标与终点位姿，并获取机器人当前位姿。
步骤A2：栅格化静态地图G后，创建两个空表OpenList和CloseList，将给定的路径起点(父节点)添加到CloseList中，并且将起点附近的8个子节点放到OpenList表中。
步骤A3：判断OpenList表中是否为空，空则结束，非空则继续执行。首先遍历OpenList表里的所有子节点，根据代价评估函数f(n)查找出代价最小的子节点nbest，并将其视为新的父节点，最后把nbest从OpenList中移除并添加至CloseList中。
其中评估函数f(n)＝g(n)+h(n)+p(n)；f(n)是从初始点到子节点n，再到目标节点的代价估计函数；g(n)表示从初始点到子节点n的实际成本函数；h(n)是启发函数，是从子节点n到目标节点goal的移动代价函数；p(n)是偏移代价，是为了防止出现只有前两项g(n)与h(n)相加，使得f(n)相等的情况，加上偏移代价能够使得A*算法扩展更少的节点得到最短全局路径。
g(n)实际成本函数，成本采取曼哈顿计算方式，即把横向和纵向移动一个节点的代价定义为c1＝10。斜向(对角线)移动代价参考等腰三角形计算斜边的方式，代价为
当前OpenList中的子节点到目标节点的移动方式允许横向、纵向和对角线方向移动，横向纵向移动代价是c1，对角线方向移动代价是c2，h(n)启发函数采用对角线距离，
h3(n)是对角线距离启发函数；n.x，n.y是子节点的横纵坐标；goal.x，goal.y是目标点的横纵坐标；hdiagonal(n)是沿着对角线移动的步数；hstraight(n)是曼哈顿距离。
计算偏移代价p(n)：
其中start.x，start.y是路径起始点的横纵坐标，dx1是子节点横坐标跟目标点横坐标差值，dy1是子节点纵坐标跟目标点纵坐标差值，dx2是路径起始点跟目标点横坐标差值，dy2是路径起始点跟目标点纵坐标差值，D是起始节点到目标节点的向量和当前n节点到目标节点的向量的内积，h(n)是启发函数，μ是调节系数，μ＝0.0001。
步骤A4：选择nbest节点后，将当前nbest节点相邻的8个新子节点逐个进行新的遍历，若某个相邻子节点已经在CloseList中或该节点是不通过的即为障碍物，则丢弃之，不做处理。否则，对该相邻节点继续执行下面的流程：(i)如果该相邻节点不在OpenList中，则将其添加到OpenList，当前nbest节点为它的父节点。(ii)如果该相邻节点已在OpenList中，则通过f(n)值判断这条路径是否为最近路径。如果是最近的，就把该子节点纳入CloseList表中作为新的父节点。
步骤A5：重复上述A3、A4两步，直至CloseList表中出现目标节点goal，规划结束，并把CloseList表中所有节点按序列用线段相连即得到了最短全局路径。
本实施例中改进的MPC轨迹规划和跟踪算法具体实施方式包括以下步骤：
步骤B1：确定状态量和控制量建立车辆模型并写成状态方程的形式。状态量和控制量：其中/>表示状态量偏差矩阵，x表示车辆横轴坐标，xr表示车辆横轴坐标参考值，y表示车辆纵轴坐标，yr表示车辆纵轴坐标参考值，/>表示车辆航向角，/>表示车辆航向角参考值，/>表示控制量偏差矩阵，v表示车速，δ表示前轮转角，vr车速参考值，δr前轮转角参考值。
模型选择为阿克曼车辆结构运动学模型。建立模型后将其离散化，离散后的模型总为：
其中是k+1时刻的状态量偏差矩阵，/>是k时刻的状态量偏差矩阵，是k时刻的控制量偏差矩阵，y(k+1)是输出矩阵，A矩阵与B矩阵与是跟模型相关的系数矩阵，C′矩阵是根据输出需要设计的输出矩阵。此时：
其中T(T＝1s)表示采样时间，l(l＝0.8m)表示前后轮间的轴距。
步骤B2：在k时刻依据全局路径计算车辆的参考位置。计算方法有以下两种：(i)选择车辆后轮轴距中心，做两条平行于x轴和y轴的两条直线与全局路径相交得到两个相交点，分别计算两个相交点与后轮轴距中心的距离，选择距离较短的点作为参考点；(ii)选择车辆后轮轴距中心，计算后轮轴距中心到全局路径的垂直距离，并选择垂直距离与全局路径上的交点作为参考点(xr(k)，yr(k))。若此时k时刻是车辆的起始时刻，那么该参考点就是轨迹的起始点。
在选择好参考点后和模型离散化后，计算各类参考值。车速参考值vr可以设置为一个常数。计算k时刻参考点的曲率ρ，并且依据曲率计算该参考点上前轮转向角的参考值δr(k)：δr(k)＝arctan(l·ρ)；车辆航向角参考值计算公式为：/>yr(k-1)是k-1时刻的车辆纵坐标参考值，xr(k-1)是k-1时刻的车辆横坐标参考值，yr(k)是k时刻纵坐标参考值，xr(k)k时刻横坐标参考值。
步骤B3：基于 来进行优化求解(N为预测步长)，在k时刻基于输入偏差/>以及系统模型预测输出y(k+1)，k+1时刻基于输入偏差及模型预测输出y(k+2)，k+2时刻基于输入偏差/>及模型预测输出y(k+3)......k+N时刻基于输入偏差/>及模型预测输出y(k+N)。那么y(k+1)、y(k+2)、y(k+3)......y(k+N)组成预测步长，/>组成控制步长。
通过构建代价函数并进行最小值求解，确定
代价函数
式中Q、R、F均为对角矩阵，e(k+i|k)表示在k时刻对k+i时刻做出的预测误差，x(k+i|k)表示在k时刻对k+i时刻做出的预测状态量，xr(k)表示k时刻的状态参考值，表示在k时刻对k+i时刻做出的预测输入控制量偏差值，e(k+N|k)表示在k时刻对k+N时刻做出的终端预测误差，Val(k)是k时刻是车辆雷达感知与障碍物距离得到的栅格值，调节系数η取0.1。
栅格值定义如下所述：(i)栅格值255，雷达未探明区域标为255；(ii)栅格值254，雷达感知障碍物与机器人中心重叠，表示必然发生碰撞；(iii)栅格值253，雷达感知障碍物处于机器人内切圆内，表示必然发生碰撞；(iv)栅格值[128，252]，雷达感知障碍物处于机器人外切圆内，处于碰撞临界，表示不一定发生碰撞；(v)栅格值(0，127]，雷达感知障碍物机器人处于机器人附近，属于危险警戒区，表示可能会发生碰撞；(vi)栅格值0，雷达感知无障碍物，表示机器人可以自由通行。
用来表示e(k+i|k)，使得式中只有/>一个变量，使得代价函数J化为标准的二次规划型：
其中为初始状态，求解代价函数J最小值时可当作常数去掉。Val(k)是k时刻是车辆雷达感知与障碍物距离得到的栅格值。这样代价函数J中仅有一个变量便于求解。G，E，H，M，C，/>是计算得到的稀疏矩阵；/>和/>是由基于k时刻的状态偏差量和控制偏差量组成的矩阵。
步骤B4：根据计算出的得到u(k)，/> 其中u(k)表示在k时刻的控制量，即为k时刻车速υ(k)和前轮转角δ(k)；
施加u(k)进入系统，得到系统的真实输出yreal(k+1)，作为k+1时刻的初始状态，并把预测区间和控制区间往左移动一个单位重新进行预测，求出此时使得代价函数J最小的和u(k+1)，多次迭代直到AGV达到目标点位姿要求。
本发明中三级自恢复行为策略具体过程为：采取“自旋-障碍物清除-再规划”的三级自恢复行为策略。第一级策略是原地自选一周将周围障碍物清除并重新感知，再调用局部规划器重新路径规划；如果仍然失败，则调用第二级策略，先移动一小段安全距离，再调用局部规划器重新规划路径；如果经过上述过程仍未奏效，则调用第三级策略，在地图上将动态障碍物更新到全局地图上，重新调用全局路径规划并检测路径是否可通行。如果路径不可行，则直接放弃本次任务，等待下一次任务。
最后，本发明实例中所列参数取值只作参考，对于参数的改动依旧在专利保护范围内。
Claims (1)
1.一种A*引导下基于MPC的轨迹规划及跟踪方法，其特征在于：具体包括如下步骤：车载雷达先行进行静态地图的感知与搭建，搭建完成后进行栅格化处理；依靠车辆自身的定位器与传感器获取当前车身位姿，而后利用改进的A*算法进行全局路径规划，全局路径规划完成后确定运动轨迹参考点，依据参考点计算各类参考值；确定好起始参考值后进行基于MPC算法的轨迹规划和轨迹跟踪，若AGV避障失败陷入死区，即MPC轨迹无效，采取三级自恢复行为策略，按照等级重新规划轨迹；若MPC轨迹有效则输出运动控制参数，AGV朝规划点移动，并判断是否达到终点位姿要求，达到要求则结束导航避障，否则进行新一轮的MPC轨迹规划和轨迹跟踪；
所述的改进的A*算法进行全局路径规划，具体包括如下步骤：
步骤A1：依靠AGV所携带的雷达感知地图，此地图为静态地图G，并将地图进行栅格化处理，用不同颜色标注可通行区域和不可通行区域；给定全局目标与终点位姿，并获取机器人当前位姿；
步骤A2：栅格化静态地图G后，创建两个空表OpenList和CloseList，将给定的路径起点即父节点添加到CloseList中，并且将起点附近的m个子节点放到OpenList表中；
步骤A3：判断OpenList表中是否为空，空则结束，非空则继续执行；首先遍历OpenList表里的所有子节点，根据代价评估函数f(n)查找出代价最小的子节点nbest，并将其视为新的父节点，最后把nbest从OpenList中移除并添加至CloseList中；
其中代价评估函数f(n)＝g(n)+h(n)+p(n)，其中f(n)是从初始点到子节点n，再到目标节点的代价估计函数；g(n)表示从初始点到子节点n的实际成本函数；h(n)是启发函数，是从子节点n到目标节点goal的移动代价函数；p(n)是偏移代价，防止出现只有前两项g(n)与h(n)相加，使得f(n)相等，加上偏移代价使得A*算法扩展更少的节点得到最短全局路径；
实际成本函数g(n)，采取曼哈顿计算成本，把横向和纵向移动一个节点的代价定义为c1，斜向(对角线)移动代价c2；
启发函数h(n)根据在栅格上的移动方式作如下选择：
当前OpenList中的子节点到目标节点的移动方式只有横向和竖向时，采用曼哈顿距离：h1(n)＝c1(|n.x-goal.x|+|n.y-goal.y|)；当前OpenList中的子节点到目标节点的移动方式允许横向、纵向和对角线方向移动，并且移动代价都是c1时，采用欧拉距离：当前OpenList中的子节点到目标节点的移动方式允许横向、纵向和对角线方向移动，但横向纵向移动代价是c1，对角线方向移动代价是c2时，采用对角线距离：/>其中h1(n)是曼哈顿距离启发函数，h2(n)是欧拉距离启发函数，h3(n)是对角线距离启发函数；n.x，n.y是子节点的横纵坐标；goal.x，goal.y是目标点的横纵坐标；hdiagonal(n)是沿着对角线移动的步数；hstraight(n)是曼哈顿距离；
计算偏移代价p(n)：其中start.x，start.y是路径起始点的横纵坐标，dx1是子节点横坐标跟目标点横坐标差值，dy1是子节点纵坐标跟目标点纵坐标差值，dx2是路径起始点跟目标点横坐标差值，dy2是路径起始点跟目标点纵坐标差值，D是起始节点到目标节点的向量和当前n节点到目标节点的向量的内积，h(n)是启发函数，μ是调节系数；
步骤A4：选择nbest节点后，将当前nbest节点相邻的8个新子节点逐个进行新的遍历，若某个相邻子节点已经在CloseList中或该节点是不通过的即为障碍物，则丢弃之，不做处理；否则，对该相邻节点继续执行下面的流程：(i)如果该相邻节点不在OpenList中，则将其添加到OpenList，当前nbest节点为它的父节点；(ii)如果该相邻节点已在OpenList中，则通过f(n)值判断这条路径是否为最近路径；如果是最近的，就把该子节点纳入CloseList表中作为新的父节点；
步骤A5：重复上述A3、A4两步，直至CloseList表中出现目标节点goal，规划结束，并把CloseList表中所有节点按序列用线段相连即得到了最短全局路径；
所述的改进的MPC轨迹规划和跟踪算法包括以下步骤：
步骤B1：确定状态量和控制量建立车辆模型并写成状态方程的形式，模型为运动学模型或者动力学模型；
建立模型后将其离散化，离散后的模型为：其中/>是k+1时刻的状态量偏差矩阵，/>是k时刻的状态量偏差矩阵，/>是k时刻的控制量偏差矩阵，y(k+1)是输出矩阵，A矩阵与B矩阵与是跟模型相关的系数矩阵，C′矩阵是根据输出需要设计的输出矩阵；
步骤B2：在k时刻依据全局路径计算车辆的参考位置；计算方法有以下两种：(i)选择车辆后轮轴距中心，做两条平行于x轴和y轴的两条直线与全局路径相交得到两个相交点，分别计算两个相交点与后轮轴距中心的距离，选择距离较短的点作为参考点；(ii)选择车辆后轮轴距中心，计算后轮轴距中心到全局路径的垂直距离，并选择垂直距离与全局路径上的交点作为参考点；若此时k时刻是车辆的起始时刻，该参考点就是轨迹的起始点；在选择好参考点和模型离散化后，计算各类参考值；
步骤B3：基于来进行优化求解，N为预测步长，在k时刻基于输入偏差/>以及系统模型预测输出y(k+1)，k+1时刻基于输入偏差/>及模型预测输出y(k+2)，k+2时刻基于输入偏差/>及模型预测输出y(k+3)......k+N时刻基于输入偏差/>及模型预测输出y(k+N)；y(k+1)、y(k+2)、y(k+3)......y(k+N)组成预测步长，/> 组成控制步长；
通过构建代价函数并进行最小值求解，确定
代价函数 式中Q、R、F均为对角矩阵，e(k+i|k)表示在k时刻对k+i时刻做出的预测误差，x(k+i|k)表示在k时刻对k+i时刻做出的预测状态量，xr(k)表示k时刻的状态参考值，/>表示在k时刻对k+i时刻做出的预测输入控制量偏差值，e(k+N|k)表示在k时刻对k+N时刻做出的终端预测误差，Val(k)是k时刻是车辆雷达感知与障碍物距离得到的栅格值，η表示调节系数，栅格值大小可以自行定义；
用来表示e(k+i|k)，使得式中只有/>一个变量，使得代价函数J化为标准的二次规划型；
步骤B4：根据计算出的得到u(k)，并施加u(k)进入系统，得到系统的真实输出yreal(k+1)，作为k+1时刻的初始状态，并把预测区间和控制区间往左移动一个单位重新进行预测，求出此时使得代价函数J最小的/>和u(k+1)，多次迭代直到AGV达到目标点位姿要求；
所述的三级自恢复行为策略具体过程为：采取“自旋-障碍物清除-再规划”的三级自恢复行为策略；第一级策略是原地自选一周将周围障碍物清除并重新感知，再调用局部规划器重新路径规划；如果仍然失败，则调用第二级策略，先移动一小段安全距离，再调用局部规划器重新规划路径；如果经过上述过程仍未奏效，则调用第三级策略，在地图上将动态障碍物更新到全局地图上，重新调用全局路径规划并检测路径是否可通行；如果路径不可行，则直接放弃本次任务，等待下一次任务。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111624520.6A CN114355923B (zh) | 2021-12-28 | 2021-12-28 | 一种a*引导下基于mpc的轨迹规划及跟踪方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111624520.6A CN114355923B (zh) | 2021-12-28 | 2021-12-28 | 一种a*引导下基于mpc的轨迹规划及跟踪方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114355923A CN114355923A (zh) | 2022-04-15 |
| CN114355923B true CN114355923B (zh) | 2024-04-02 |
Family
ID=81102605
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111624520.6A Active CN114355923B (zh) | 2021-12-28 | 2021-12-28 | 一种a*引导下基于mpc的轨迹规划及跟踪方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114355923B (zh) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114879704B (zh) * | 2022-07-11 | 2022-11-25 | 山东大学 | 一种机器人绕障控制方法及系统 |
| CN116560241B (zh) * | 2023-07-10 | 2023-09-15 | 北京科技大学 | 面向铰接车的显式循环模型预测控制轨迹跟踪方法和装置 |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2017173990A1 (zh) * | 2016-04-07 | 2017-10-12 | 北京进化者机器人科技有限公司 | 一种机器人避障中的最短路径规划方法 |
| CN108334086A (zh) * | 2018-01-25 | 2018-07-27 | 江苏大学 | 一种基于软约束二次规划mpc的无人驾驶车辆路径跟踪控制方法 |
| WO2019076044A1 (zh) * | 2017-10-20 | 2019-04-25 | 纳恩博（北京）科技有限公司 | 移动机器人局部运动规划方法、装置及计算机存储介质 |
| CN112378408A (zh) * | 2020-11-26 | 2021-02-19 | 重庆大学 | 一种实现轮式移动机器人实时避障的路径规划方法 |
| CN113108796A (zh) * | 2021-04-19 | 2021-07-13 | 北京有竹居网络技术有限公司 | 导航方法、装置、存储介质及设备 |
| CN113156886A (zh) * | 2021-04-30 | 2021-07-23 | 南京理工大学 | 一种智能物流路径规划方法及系统 |
| CN113359757A (zh) * | 2021-06-30 | 2021-09-07 | 湖北汽车工业学院 | 一种改进型混合a*算法的无人驾驶车辆路径规划与轨迹跟踪方法 |
| CN113433942A (zh) * | 2021-06-30 | 2021-09-24 | 南京理工大学 | 一种基于最优航向角的长轴车辆路径跟踪控制方法 |
-
2021
- 2021-12-28 CN CN202111624520.6A patent/CN114355923B/zh active Active
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2017173990A1 (zh) * | 2016-04-07 | 2017-10-12 | 北京进化者机器人科技有限公司 | 一种机器人避障中的最短路径规划方法 |
| WO2019076044A1 (zh) * | 2017-10-20 | 2019-04-25 | 纳恩博（北京）科技有限公司 | 移动机器人局部运动规划方法、装置及计算机存储介质 |
| CN108334086A (zh) * | 2018-01-25 | 2018-07-27 | 江苏大学 | 一种基于软约束二次规划mpc的无人驾驶车辆路径跟踪控制方法 |
| CN112378408A (zh) * | 2020-11-26 | 2021-02-19 | 重庆大学 | 一种实现轮式移动机器人实时避障的路径规划方法 |
| CN113108796A (zh) * | 2021-04-19 | 2021-07-13 | 北京有竹居网络技术有限公司 | 导航方法、装置、存储介质及设备 |
| CN113156886A (zh) * | 2021-04-30 | 2021-07-23 | 南京理工大学 | 一种智能物流路径规划方法及系统 |
| CN113359757A (zh) * | 2021-06-30 | 2021-09-07 | 湖北汽车工业学院 | 一种改进型混合a*算法的无人驾驶车辆路径规划与轨迹跟踪方法 |
| CN113433942A (zh) * | 2021-06-30 | 2021-09-24 | 南京理工大学 | 一种基于最优航向角的长轴车辆路径跟踪控制方法 |
Non-Patent Citations (2)
| Title |
|---|
| 基于改进A*算法机器人路径规划研究;王小红;叶涛;;计算机测量与控制;20180725(第07期);全文 * |
| 复杂环境下移动机器人全局路径规划与跟踪;周滔;赵津;胡秋霞;席阿行;刘东杰;;计算机工程;20180116(第12期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114355923A (zh) | 2022-04-15 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Zhang et al. | Hybrid trajectory planning for autonomous driving in highly constrained environments | |
| Hubmann et al. | A pomdp maneuver planner for occlusions in urban scenarios | |
| CN114355923B (zh) | 一种a*引导下基于mpc的轨迹规划及跟踪方法 | |
| CN113710431B (zh) | 使用基于采样的最优树的路径规划方法以及记录介质 | |
| CN109557912A (zh) | 一种自动驾驶特种作业车辆的决策规划方法 | |
| Ding et al. | New multiple-target tracking strategy using domain knowledge and optimization | |
| Babinec et al. | VFH* TDT (VFH* with Time Dependent Tree): A new laser rangefinder based obstacle avoidance method designed for environment with non-static obstacles | |
| CN105116902A (zh) | 一种移动机器人避障导航的方法和系统 | |
| Laghmara et al. | Obstacle avoidance, path planning and control for autonomous vehicles | |
| Fulgenzi et al. | Probabilistic motion planning among moving obstacles following typical motion patterns | |
| CN112284393B (zh) | 一种智能移动机器人全局路径规划方法和系统 | |
| Zhou et al. | Autonomous vehicles’ turning motion planning for conflict areas at mixed-flow intersections | |
| KR101990878B1 (ko) | 시간상태 영역에서의 온라인 양방향 경로 계획 방법, 이를 구현하기 위한 프로그램이 저장된 기록매체 및 이를 구현하기 위해 매체에 저장된 컴퓨터프로그램 | |
| Gu | Improved trajectory planning for on-road self-driving vehicles via combined graph search, optimization & topology analysis | |
| Claussmann et al. | A path planner for autonomous driving on highways using a human mimicry approach with binary decision diagrams | |
| CN113485346B (zh) | 一种移动机器人在核事故复杂环境中的自主导航方法 | |
| WO2021073781A9 (en) | Prediction and planning for mobile robots | |
| Sun et al. | Safe and smooth motion planning for mecanum-wheeled robot using improved RRT and cubic spline | |
| Li et al. | Adaptive sampling-based motion planning with a non-conservatively defensive strategy for autonomous driving | |
| Huy et al. | A practical and optimal path planning for autonomous parking using fast marching algorithm and support vector machine | |
| KR102097715B1 (ko) | 실시간 웨이포인트 경로 개선 방법, 이를 구현하기 위한 프로그램이 저장된 기록매체 및 이를 구현하기 위해 매체에 저장된 컴퓨터프로그램 | |
| Bai et al. | An online approach for intersection navigation of autonomous vehicle | |
| CN112363498B (zh) | 一种基于激光雷达的水下机器人智能运动控制方法 | |
| Fulgenzi | Autonomous navigation in dynamic uncertain environment using probabilistic models of perception and collision risk prediction. | |
| Chung et al. | Robot motion planning in dynamic uncertain environments |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |