适应未知动态空间的路径跟随控制方法、装置及存储介质与流程

本发明涉及自动驾驶技术，尤其与一种适应未知动态空间的路径跟随控制方法、装置及存储介质有关。

背景技术：

路径规划完成后，载具实现路径时，由于载具本身运动能力限制，通常要进行路径平滑处理。基于稠密栅格点规划所得路径，经过满足载具运动能力限制的路径平滑后，通常会破坏路径的最优性，甚至引起路径不可行；而在路径规划过程中考虑载具运动能力限制，则大大增加了路径规划难度、降低搜索效率。通过直接规划路径并对规划结果直接进行跟随控制的方法不适用于未知动态空间的路径实现。

技术实现要素：

针对上述现状，本发明提供一种适应未知动态空间的路径跟随控制方法、装置及存储介质，局部路径修正和轨迹跟随控制一体解决，便于载具实现路径跟随控制，规划效率高、路径实现能力强；在路径实现时考虑局部环境和载具能力，避障避撞效果好、适于载具控制。

为了实现本发明的目的，拟采用以下方案：

一种适应未知动态空间的路径跟随控制方法，其特征在于，包括步骤：

判断载具是否到达目标：若到达则停止，若未达到则继续执行；

基于可采信位姿，在粗路径节点串中找到当前节点和下一节点；

基于可采信位姿，根据当前规划用图，采用人工势场法解算预期运动指令；

基于载具的运动响应动力学，用预期运动指令解算跟随控制指令。

进一步，判断载具是否到达目标，是判断当前可采信位姿距离粗路径节点串的末节点是否小于预设的目标判断阈值，若是，则表示达到目标；若否，则表示未达到目标。

进一步，基于可采信位姿，在粗路径节点串中找到当前节点和下一节点，包括步骤：

顺次计算当前位姿与粗路径节点串中n个点的距离，其中距离最短的粗路径节点记为当前节点，其节点记号为i，满足1≤i≤n；

若i<n，则下一节点为粗路径节点串中第i+1个路径节点；

若i=n，则下一节点仍为粗路径节点串中第n个路径节点。

进一步，基于可采信位姿，根据当前规划用图，采用人工势场法解算预期运动指令，包括步骤：

以下一节点为局部运动目标，求取引力；

以当前规划用图中找到的距当前位姿的距离在预定阈值范围内的障碍点计算斥力；

根据引力和斥力，以载具的转弯半径、速度限制作为约束，采用最优控制解算预期运动指令。

进一步，选取预定阈值范围内距离当前位姿最近的障碍点计算斥力，若阈值范围内没有障碍则斥力为0。

进一步，基于载具的运动响应动力学，设计载具运动控制器，运动控制器的控制目标是令载具实际运动追踪预期运动指令，从而用预期运动指令解算跟随控制指令，运动控制器采用的控制算法包括但不限于pid控制、鲁棒控制、滑模控制、预测控制、回步控制，可以是其中一种或多种结合。

进一步，当前规划用图是指：实时在线更新，并用于记录当前环境，且逻辑在于表达当前该点的障碍概率的局部地图。

进一步，粗路径节点串，是指以凸顶点为节点，搜索得到的一系列有序路径节点序列，凸定点是从拓展地图中根据不可通行区域获得的，拓展地图是根据载具的外形尺寸约束，在栅格地图基础上获得的。

一种适应未知动态空间的路径跟随控制装置，其特征在于，包括：

判断模块，用于判断载具是否到达目标：若到达则停止，若未达到则继续执行；

节点模块，用于在判断模块判断结果为未达到目标时，基于可采信位姿，在粗路径节点串中找到当前节点和下一节点；

预期模块，用于基于可采信位姿，根据当前规划用图，采用人工势场法解算预期运动指令；及

控制模块，用于基于载具的运动响应动力学，用预期运动指令解算跟随控制指令。

一种计算机可读存储介质，其上存储有计算机程序，其特征在于，在所述计算机程序被处理器运行时控制所述存储介质所在设备执行所述的适应未知动态空间的路径跟随控制方法。

本发明的有益效果在于：

1、本发明将局部路径修正和轨迹跟随控制一体解决，便于载具实现路径跟随控制，路径规划时只需解算粗路径节点串，极大降低规划难度，规划效率高、路径实现能力强；

2、本发明在路径实现时考虑局部环境和载具能力，避障避撞效果好、确保载具控制能力和效果。

附图说明

本文描述的附图只是为了说明所选实施例，而不是所有可能的实施方案，更不是意图限制本发明的范围。

图1为本申请实施例的路径跟随控制方法流程图。

图2为本申请实施例的数据信号流图。

图3为本申请实施例的找寻下一节点流程图。

图4为本申请实施例的求解预期运动指令流程图。

图5为本申请实施例的路径跟随控制装置结构框图。

图6为本申请实施例的路径跟随控制装置判断模块结构框图。

图7为本申请实施例的路径跟随控制装置预期模块结构框图。

图8为本申请实施例的路径跟随控制装置控制模块结构框图。

具体实施方式

为了使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明做进一步的详细描述。

实施例一

本申请实施例的路径跟随控制方法主要包括步骤：

（1）判断载具是否到达目标，如果到达则停止，如果没到则继续。设定到达目标判断阈值d_end，若当前可采信位姿距离粗路径节点串的末节点小于阈值，则认为到达目标；否则认为没有到达目标。末节点也就是路径规划的目标点。

（2）基于可采信位姿，在粗路径节点串中找到当前节点和下一节点。其中，可采信位姿可以是各类传感器直接获取后经过一定运算的结果，也可以是位姿融合处理后得到的综合结果。当前节点是粗路径节点串中，距离可采信位姿最近的节点。根据当前节点可以找到粗路径节点串中的下一节点。

（3）基于可采信位姿，根据当前规划用图，采用人工势场法解算预期运动指令。当前规划用图是快速更新的实时在线局部地图，地图中记录当前的环境，逻辑在于表达当前该点的障碍概率。提供“引力”的仅有局部运动目标点一个点，该点为粗路径节点串中基于可采信位姿找到的下一节点；提供“斥力”的是当前规划用图中与可采信位姿距离在阈值范围内的障碍点，阈值范围外的点不起作用。

（4）基于载具的运动响应动力学，设计载具运动控制器，控制目标是令载具实际运动追踪预期运动指令，从而用预期运动指令解算跟随控制指令。可选地，可采用的控制算法，包括但不限于pid控制、鲁棒控制、滑模控制、预测控制、回步控制，可以是其中一种或多种结合。

如图1~2所示的流程，具体说明在本实例中，作进一步对上述步骤进行详细说明：

步骤s100，判断载具是否到达目标，如果到达则停止，如果没到则继续。设定到达目标判断阈值d_end，若当前可采信位姿距离粗路径节点串的末节点小于阈值，则认为到达目标；否则认为没有到达目标，继续下一步。

步骤s200，图3为找寻“下一节点”流程图。基于可采信位姿，在粗路径节点串中找到当前节点和下一节点，具体方法为：

s201、顺次计算当前位姿与粗路径节点串中n个点的距离，其中距离最短的粗路径节点记为当前节点，其节点记号为i，满足1≤i≤n；

s202、若i<n，则下一节点为粗路径节点串中第i+1个路径节点；

s203、若i=n，则下一节点仍为粗路径节点串中第n个路径节点。

步骤s300，图4为求解预期运动指令流程图。基于可采信位姿，根据当前规划用图，采用人工势场法解算预期运动指令，具体方法为：

s301、以“下一节点”为局部运动目标，求取“引力”；

s302、设定阈值，在当前规划用图中找到距当前位姿的距离在阈值范围内的障碍点，用这些障碍点计算“斥力”，优选地，选取阈值范围内最近障碍点计算斥力可以简化计算，若阈值范围内没有障碍则斥力为0；

s303、考虑载具的机动能力，包括转弯半径、速度限制等作为约束，采用最优控制解算预期运动指令。

步骤s400，基于载具的运动响应动力学，设计载具运动控制器，控制目标是令载具实际运动追踪预期运动指令，从而用预期运动指令解算跟随控制指令。

实施例二

结合附图5所述的结构框图，对本申请实施例的适应未知动态空间的路径跟随控制装置做进一步说明。

本实例的路径跟随控制装置，包括依次连接的判断模块、节点模块、预期模块、控制模块。

具体的，判断模块，判断载具是否到达目标：若到达则停止，若未达到则继续执行；在判断模块判断结果为未达到目标时，节点模块基于可采信位姿，在粗路径节点串中找到当前节点和下一节点；预期模块基于可采信位姿，根据当前规划用图，采用人工势场法解算预期运动指令；控制模块基于载具的运动响应动力学，用预期运动指令解算跟随控制指令。

具体的，判断模块，如图6所示，包括目标阈值单元和目标判断单元，目标阈值单元用于设定目标判断阈值d_end，目标判断单元对当前可采信位姿距离粗路径节点串的末节点是否小于阈值d_end，若小于，则表示到达目标，停止；否则表示没有到达，继续节点模块的执行。

具体的，节点模块，如图7所示，包括计算单元、第一标记单元和第二标记单元，计算单元顺次计算当前位姿与粗路径节点串中n个点的距离，第一标记单元用于将计算单元计算的距离中中距离最短的粗路径节点记为当前节点，其节点记号为i，满足1≤i≤n，第二标记单元用于在i<n时将粗路径节点串中第i+1个路径节点记为下一节点；在i=n时将粗路径节点串中第n个路径节点记为下一节点。

具体的，预期模块，如图8所示，包括引力单元、斥力单元和解算单元。引力单元以下一节点为局部运动目标，求取引力。斥力单元通过设定阈值，在当前规划用图中找到距当前位姿的距离在阈值范围内的障碍点，用这些障碍点计算斥力。优选地，选取阈值范围内最近障碍点计算斥力可以简化计算，若阈值范围内没有障碍则斥力为0。解算单元根据引力和斥力，考虑载具的机动能力，包括转弯半径、速度限制等作为约束，采用最优控制解算预期运动指令。

实施例三

本实例提供一种计算机可读存储介质，其上存储有计算机程序，在计算机程序被处理器运行时控制存储介质所在设备执行实施例一的适应未知动态空间的路径跟随控制方法。

以上仅为本发明的优选实施例，并不表示是唯一的或是限制本发明。本领域技术人员应理解，在不脱离本发明的范围情况下，对本发明进行的各种改变或同等替换，均属于本发明保护的范围。