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

本发明专利技术公开了一种适应未知动态空间的路径跟随控制方法，属于自动驾驶领域。方法步骤包括：判断载具是否到达目标，如果到达则停止，如果没到则继续；基于可采信位姿，在粗路径节点串中找到当前节点和下一节点；基于可采信位姿，根据当前规划用图，采用人工势场法解算预期运动指令；基于载具的运动响应动力学，用预期运动指令解算跟随控制指令。本发明专利技术将局部路径修正和轨迹跟随控制一体解决，规划效率高、路径实现能力强；在路径实现时考虑局部环境和载具能力，避障避撞效果好、适于载具控制。

【技术实现步骤摘要】
适应未知动态空间的路径跟随控制方法、装置及存储介质
本专利技术涉及自动驾驶技术，尤其与一种适应未知动态空间的路径跟随控制方法、装置及存储介质有关。
技术介绍
路径规划完成后，载具实现路径时，由于载具本身运动能力限制，通常要进行路径平滑处理。基于稠密栅格点规划所得路径，经过满足载具运动能力限制的路径平滑后，通常会破坏路径的最优性，甚至引起路径不可行；而在路径规划过程中考虑载具运动能力限制，则大大增加了路径规划难度、降低搜索效率。通过直接规划路径并对规划结果直接进行跟随控制的方法不适用于未知动态空间的路径实现。
技术实现思路
针对上述现状，本专利技术提供一种适应未知动态空间的路径跟随控制方法、装置及存储介质，局部路径修正和轨迹跟随控制一体解决，便于载具实现路径跟随控制，规划效率高、路径实现能力强；在路径实现时考虑局部环境和载具能力，避障避撞效果好、适于载具控制。为了实现本专利技术的目的，拟采用以下方案：一种适应未知动态空间的路径跟随控制方法，其特征在于，包括步骤：判断载具是否到达目标：若到达则停止，若未达到则继续执行；基于可采信位姿，在粗路径节点串中找到当前节点和下一节点；基于可采信位姿，根据当前规划用图，采用人工势场法解算预期运动指令；基于载具的运动响应动力学，用预期运动指令解算跟随控制指令。进一步，判断载具是否到达目标，是判断当前可采信位姿距离粗路径节点串的末节点是否小于预设的目标判断阈值，若是，则表示达到目标；若否，则表示未达到目标。>进一步，基于可采信位姿，在粗路径节点串中找到当前节点和下一节点，包括步骤：顺次计算当前位姿与粗路径节点串中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，则下一节点为粗路径节点串本文档来自技高网...

【技术保护点】
1.一种适应未知动态空间的路径跟随控制方法，其特征在于，包括步骤：/n判断载具是否到达目标：若到达则停止，若未达到则继续执行；/n基于可采信位姿，在粗路径节点串中找到当前节点和下一节点；/n基于可采信位姿，根据当前规划用图，采用人工势场法解算预期运动指令；/n基于载具的运动响应动力学，用预期运动指令解算跟随控制指令。/n

【技术特征摘要】
1.一种适应未知动态空间的路径跟随控制方法，其特征在于，包括步骤：
判断载具是否到达目标：若到达则停止，若未达到则继续执行；
基于可采信位姿，在粗路径节点串中找到当前节点和下一节点；
基于可采信位姿，根据当前规划用图，采用人工势场法解算预期运动指令；
基于载具的运动响应动力学，用预期运动指令解算跟随控制指令。


2.根据权利要求1所述的适应未知动态空间的路径跟随控制方法，其特征在于，判断载具是否到达目标，是判断当前可采信位姿距离粗路径节点串的末节点是否小于预设的目标判断阈值，若是，则表示达到目标；若否，则表示未达到目标。


3.根据权利要求1所述的适应未知动态空间的路径跟随控制方法，其特征在于，基于可采信位姿，在粗路径节点串中找到当前节点和下一节点，包括步骤：
顺次计算当前位姿与粗路径节点串中n个点的距离，其中距离最短的粗路径节点记为当前节点，其节点记号为i，满足1≤i≤n；
若i<n，则下一节点为粗路径节点串中第i+1个路径节点；
若i=n，则下一节点为粗路径节点串中第n个路径节点。


4.根据权利要求1所述的适应未知动态空间的路径跟随控制方法，其特征在于，基于可采信位姿，根据当前规划用图，采用人工势场法解算预期运动指令，包括步骤：
以下一节点为局部运动目标，求取引力；
以当前规划用图中找到的距当前位姿的距离在预定阈值范围内的障碍点计算斥力；
根据引力和斥力，以载具的转弯半径、速度限制作为约束，采用最优控制解算预期运动指令。


5.根据权利要求4所述的适应未知动态空间的路径跟随控制方法，其特征在于，选取预定阈值范围内距离当前位...

【专利技术属性】
技术研发人员：罗冠辰，王鹏涛，刘俊君，王明杰，
申请(专利权)人：北京轩宇空间科技有限公司，
类型：发明
国别省市：北京;11