基于轨迹拓扑地图和避障的车辆导航方法、系统及介质与流程

本发明涉及机器人导航技术领域，具体地，涉及。尤其地，涉及基于SLAM轨迹拓扑地图和球形VFH避障的自主导航小车系统。

背景技术：

目前，小车自主导航是机器人基本功能之一。它的其中一个目的是解决如何表达记录已知环境，如何在已知环境里搜索路径以及在导航中躲避地图中没有记录的障碍物的问题。SLAM是当前最受研究人员青睐的机器人定位与建图方式，但是由于一些SLAM构建的地图点云稀疏并有较多错误点，往往无法直接用于机器人导航，或网格地图直接导航效率较低。避障方面，传统VFH计算量小，相对势场法不易陷入局部最小点，但是对距离阈值敏感，过大阈值会导致忽视近距离的通道，过小阈值会导致难以更早发现远处的通道；另外，VFH只适用单线激光，较难推广到三维点云上的应用。

专利文献CN109002046A(申请号：201811119696.4)公开了一种移动机器人导航系统及导航方法，所述系统包括移动机器人、导航二维码以及导航光带，所述移动机器人包括工控机、高速相机以及光电传感器；其中，所述导航光带按照预设线路进行铺设，所述导航二维码固定设置在所述导航光带预设位置上；所述高速相机安装于所述移动机器人底部，用于获取导航二维码信息；所述光电传感器安装于所述移动机器人底部的前端位置，用于向所述导航光带发射光信号、以及接受经所述导航光带反射后的光信号；所述高速相机以及光电传感器与所述工控机连接，所述工控机获取所述高速相机以及光电传感器发送出的信号，并根据所述信号调整所述移动机器人的运动姿态。

技术实现要素：

针对现有技术中的缺陷，本发明的目的是提供一种基于轨迹拓扑地图和避障的车辆导航方法、系统及介质。

根据本发明提供的一种基于轨迹拓扑地图和避障的车辆导航方法，包括：

建图步骤：构建环境地图，根据构建的环境地图生成导航用轨迹拓扑地图；

全局规划步骤：利用路径搜索算法A*在生成的轨迹拓扑地图上搜索连接起点与终点的全局最短路径；

局部避障步骤：利用获得的全局最短路径实时生成局部目标点，结合局部目标点和激光传感器实时探测的环境信息生成车辆控制信号控制车辆躲避障碍物并到达目的地。

优选地，所述建图步骤：

通过SLAM构建环境地图，保存从SLAM获得的地图点云和构建地图时走过的轨迹；

根据从SLAM获得的地图点云和构建地图时走过的轨迹，清除轨迹点周围一定距离阈值内的地图点云；

用八叉树把轨迹点进行分割，计算每个八叉树叶节点内所有轨迹点的重心，若某两个重心之间的距离小于第一预设阈值，则剔除其中一个重心点，把余下的重心点作为拓扑地图的节点；

获取两两节点间的连通性，根据两两节点间的连通性生成拓扑地图的边，并去除冗余边，获得轨迹拓扑地图。

优选地，所述获取两两节点间的连通性：

判断两节点间距离大于第二预设阈值：若两节点距离大于第二预设阈值，则判定两节点之间无连通性；否则，则判断两节点之间是否被地图点云阻挡：在两节点间的连线上紧密排布与车辆宽度一致的球体，球心间隔与球体直径相同，若存在某个球体内包含地图点数量大于第一预设数量，则认为两节点被阻挡；否则，则认为两节点连通，两节点之间不被地图点云阻挡；

所述去除冗余边：

遍历所有边，若存在某三条边组成一个回环，则删除最长的边。

优选地，所述全局规划步骤：

分别找到离起点和终点最近的轨迹拓扑地图节点，然后使用A*在轨迹拓扑地图上搜索连接这两个拓扑地图节点的最短路径。

优选地，所述局部避障步骤：

首先更新局部目标点在最短路径上的位置，然后根据激光传感器实时获取的点云使用球形VFH寻找安全通道，再把局部目标点和安全通道结合求得新局部目标点，最后根据新局部目标点计算车辆控制信号；

所述更新局部目标点在最短路径上的位置：

导航开始时，局部目标点在最短路径的起点节点出发，以大于车辆最大速度的预设速度沿最短路径向终点节点前进，若局部目标点与车辆所在的位置之间被地图点云阻挡，则局部目标点停止前进直至不再被地图点云阻挡；

所述使用球形VFH寻找安全通道：

首先在激光扫过的区域按一定角度的分辨率分成若干方向，计算每个方向上离最近的障碍物有多少个与车辆等宽的球体，然后判断球体数量是否大于第二预设数量或是否存在若干连续方向上的球体数量比扫描区域边界方向的球体数量大：若是，则判定所述方向为安全通道，划出安全通道的角度，获得安全通道；否则，所述方向不认为是安全通道；

所述用局部目标点和安全通道结合求新局部目标点：

判断是否局部目标点在安全通道内：若局部目标点在安全通道内，则新局部目标点与局部目标点相同；否则，则找到与局部目标点角度差最小的安全通道，以安全通道最靠近车辆的边界点为新局部目标点；

所述计算车辆控制信号：

根据新局部目标点与车辆的距离计算车辆的前进速度控制信号，根据新局部目标点与车辆前进方向的角度差和局部目标点前进方向与车辆前进方向的角度差，计算车辆速度控制信号。

优选地，所述更新局部目标点在最短路径上的位置：

设局部目标点为plt(j)，在导航过程中，局部目标点发起于起点plt(0)＝Ns，终止于终点plt(0)＝Ne，按照以下条件沿最短路径P(V，E)行进：

plt∈Ep(i)

式中，

plt表示局部目标点位置；

Ep(i)表示第i段路径；

表示局部目标点的移动速度；

α表示常量参数，大于1，一般可取2；

vmax_robot表示机器人移动的最大速度；

定示第i段路径的终点；

表示第i段路径的起点；

clear(plt，probot)表示两点之间没有被地图点阻挡。

公式(1)表示当局部目标点和小车自身位置点没有被地图点阻挡时，局部目标点以大于小车最大速度的速度沿最短路径P(V，E)行进，否则停止等待；

所述计算车辆控制信号：

若局部目标点在某个安全通道的朝向内，则用下式计算小车的控制信号：

ωc＝kωαθlt_robot-kωβ(θlt-θrobot) (2)

其中，

vc表示小车控制输入线速度；

kv表示常量参数；

表示局部目标点位置；

表示小车当前位置；

ωc表示小车控制输入角速度；

kωα表示常量参数；

θlt_robot表示局部目标点相对小车的角度差；

kωβ表示常量参数；

θlt表示局部目标点移动的方向角度；

θrobot表示小车当前移动方向角度；

若局部目标点不在某个安全通道的朝向内，则选取与局部目标点方向最近的安全通道，把被选取的安全通道离小车最近的边缘点作为新局部目标点，用下式计算小车的控制信号：

ωc＝kωαθnbcp_robot-kωβ(θlt-θrobot) (3)其中，

dball表示球体的直径；

wfront表示正前方的扫描值；

θnbcp_robot表示被选择的安全通道离小车最近的边缘点相对小车的角度差。

根据本发明提供的一种基于轨迹拓扑地图和避障的自主车辆导航系统，包括：

建图模块：构建环境地图，根据构建的环境地图生成导航用轨迹拓扑地图；

全局规划模块：利用路径搜索算法A*在生成的轨迹拓扑地图上搜索连接起点与终点的全局最短路径；

局部避障模块：利用获得的全局最短路径实时生成局部目标点，结合局部目标点和激光传感器实时探测的环境信息生成车辆控制信号控制车辆躲避障碍物并到达目的地。

优选地，所述建图模块：

通过SLAM构建环境地图，保存从SLAM获得的地图点云和构建地图时走过的轨迹；

根据从SLAM获得的地图点云和构建地图时走过的轨迹，清除轨迹点周围一定距离阈值内的地图点云；

用八叉树把轨迹点进行分割，计算每个八叉树叶节点内所有轨迹点的重心，若某两个重心之间的距离小于第一预设阈值，则剔除其中一个重心点，把余下的重心点作为拓扑地图的节点；

获取两两节点间的连通性，根据两两节点间的连通性生成拓扑地图的边，并去除冗余边，获得轨迹拓扑地图；

所述获取两两节点间的连通性：

判断两节点间距离大于第二预设阈值：若两节点距离大于第二预设阈值，则判定两节点之间无连通性；否则，则判断两节点之间是否被地图点云阻挡：在两节点间的连线上紧密排布与车辆宽度一致的球体，球心间隔与球体直径相同，若存在某个球体内包含地图点数量大于第一预设数量，则认为两节点被阻挡；否则，则认为两节点连通，两节点之间不被地图点云阻挡；

所述去除冗余边：

遍历所有边，若存在某三条边组成一个回环，则删除最长的边。

优选地，所述全局规划模块：

分别找到离起点和终点最近的轨迹拓扑地图节点，然后使用A*在轨迹拓扑地图上搜索连接这两个拓扑地图节点的最短路径；

所述局部避障模块：

首先更新局部目标点在最短路径上的位置，然后根据激光传感器实时获取的点云使用球形VFH寻找安全通道，再把局部目标点和安全通道结合求得新局部目标点，最后根据新局部目标点计算车辆控制信号；

所述更新局部目标点在最短路径上的位置：

导航开始时，局部目标点在最短路径的起点节点出发，以大于车辆最大速度的预设速度沿最短路径向终点节点前进，若局部目标点与车辆所在的位置之间被地图点云阻挡，则局部目标点停止前进直至不再被地图点云阻挡；

所述使用球形VFH寻找安全通道：

首先在激光扫过的区域按一定角度的分辨率分成若干方向，计算每个方向上离最近的障碍物有多少个与车辆等宽的球体，然后判断球体数量是否大于第二预设数量或是否存在若干连续方向上的球体数量比扫描区域边界方向的球体数量大：若是，则判定所述方向为安全通道，划出安全通道的角度，获得安全通道；否则，所述方向不认为是安全通道；

所述用局部目标点和安全通道结合求新局部目标点：

判断是否局部目标点在安全通道内：若局部目标点在安全通道内，则新局部目标点与局部目标点相同；否则，则找到与局部目标点角度差最小的安全通道，以安全通道最靠近车辆的边界点为新局部目标点；

所述计算车辆控制信号：

根据新局部目标点与车辆的距离计算车辆的前进速度控制信号，根据新局部目标点与车辆前进方向的角度差和局部目标点前进方向与车辆前进方向的角度差，计算车辆速度控制信号；

所述更新局部目标点在最短路径上的位置：

设局部目标点为plt(j)，在导航过程中，局部目标点发起于起点plt(0)＝Ns，终止于终点plt(0)＝Ne，按照以下条件沿最短路径P(V，E)行进：

plt∈Ep(i)

式中，

plt表示局部目标点位置；

Ep(i)表示第i段路径；

表示局部目标点的移动速度；

α表示常量参数，大于1，一般可取2；

vmax_robot表示机器人移动的最大速度；

表示第i段路径的终点；

定示第i段路径的起点；

clear(plt，probot)表示两点之间没有被地图点阻挡。

公式(1)表示当局部目标点和小车自身位置点没有被地图点阻挡时，局部目标点以大于小车最大速度的速度沿最短路径P(V，E)行进，否则停止等待；

所述计算车辆控制信号：

若局部目标点在某个安全通道的朝向内，则用下式计算小车的控制信号：

ωc＝kωαθlt_robot-kωβ(θlt-θrobot) (2)

其中，

vc表示小车控制输入线速度；

kv表示常量参数；

表示局部目标点位置；

表示小车当前位置；

ωc表示小车控制输入角速度；

kωα表示常量参数；

θlt_robot表示局部目标点相对小车的角度差；

kωβ表示常量参数；

θlt表示局部目标点移动的方向角度；

θrobot表示小车当前移动方向角度；

若局部目标点不在某个安全通道的朝向内，则选取与局部目标点方向最近的安全通道，把被选取的安全通道离小车最近的边缘点作为新局部目标点，用下式计算小车的控制信号：

ωc＝kωαθnbcp_robot-kωβ(θlt-θrobot) (3)

其中，

dball表示球体的直径；

wfront表示正前方的扫描值；

θnbcp_robot表示被选择的安全通道离小车最近的边缘点相对小车的角度差。

根据本发明提供的一种存储有计算机程序的计算机可读存储介质，其特征在于，所述计算机程序被处理器执行时实现上述中任一项所述的基于轨迹拓扑地图和避障的车辆导航方法的步骤。

与现有技术相比，本发明具有如下的有益效果：

1、本发明的SLAM建图的轨迹可以去除大部分影响导航的错误地图点，大大提高利用SLAM地图构建拓扑地图的可靠性。

2、本发明用球体探测所有方向的安全通道，保证了找到的安全通道必然可以通过小车。本发明创造性地构造结合全局路径的局部目标点和安全通道的控制器，可以在不预先规划轨迹的情况下控制小车躲避障碍物并安全到达目的地。

3、本发明导航方式对SLAM的地图点敏感度低，能一定程度上避免把地图点云稀疏或没有处误识别为可通行区域，同时避免把错误的地图点误识别为不可通行区域；

4、本发明的避障方法可有效找到实时环境点云中可通行的安全区域；

5、本发明的避障控制方法可在不预先生成多项式轨迹的情况下安全躲避障碍物，有较低的计算量。

附图说明

通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：

图1为本发明一实施例中的仿真实验场景示意图；

图2为本发明一实施例中用激光SLAM得到的地图示意图；

图3为本发明一实施例中使用激光SLAM地图生成的拓扑地图示意图；

图4为本发明一实施例中用视觉SLAM得到的地图示意图；

图5为本发明一实施例中使用视觉SLAM地图生成的拓扑地图示意图；

图6为本发明二实施例中的真实实验场景示意图；

图7为本发明二实施例中用激光SLAM得到的地图示意图；

图8为本发明二实施例中使用激光SLAM地图生成的拓扑地图示意图；

图9为本发明二实施例中用视觉SLAM得到的地图示意图；

图10为本发明二实施例中使用视觉SLAM地图生成的拓扑地图示意图；

图11为本发明实施例中安全区判别示意图；

图12为本发明实施例中控制角度定义示意图；

图13为本发明一实施例中使用激光SLAM导航实验的结果示意图；

图14为本发明一实施例中使用视觉SLAM导航实验的结果示意图；

图15为本发明二实施例中使用激光SLAM导航实验的结果示意图；

图16为本发明二实施例中使用视觉SLAM导航实验的结果示意图。

具体实施方式

下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。

根据本发明提供的一种基于轨迹拓扑地图和避障的车辆导航方法，包括：

建图步骤：构建环境地图，根据构建的环境地图生成导航用轨迹拓扑地图；

全局规划步骤：利用路径搜索算法A*在生成的轨迹拓扑地图上搜索连接起点与终点的全局最短路径；

局部避障步骤：利用获得的全局最短路径实时生成局部目标点，结合局部目标点和激光传感器实时探测的环境信息生成车辆控制信号控制车辆躲避障碍物并到达目的地。

具体地，所述建图步骤：

通过SLAM构建环境地图，保存从SLAM获得的地图点云和构建地图时走过的轨迹；

根据从SLAM获得的地图点云和构建地图时走过的轨迹，清除轨迹点周围一定距离阈值内的地图点云；

用八叉树把轨迹点进行分割，计算每个八叉树叶节点内所有轨迹点的重心，若某两个重心之间的距离小于第一预设阈值，则剔除其中一个重心点，把余下的重心点作为拓扑地图的节点；

获取两两节点间的连通性，根据两两节点间的连通性生成拓扑地图的边，并去除冗余边，获得轨迹拓扑地图。

具体地，所述获取两两节点间的连通性：

判断两节点间距离大于第二预设阈值：若两节点距离大于第二预设阈值，则判定两节点之间无连通性；否则，则判断两节点之间是否被地图点云阻挡：在两节点间的连线上紧密排布与车辆宽度一致的球体，球心间隔与球体直径相同，若存在某个球体内包含地图点数量大于第一预设数量，则认为两节点被阻挡；否则，则认为两节点连通，两节点之间不被地图点云阻挡；

所述去除冗余边：

遍历所有边，若存在某三条边组成一个回环，则删除最长的边。

具体地，所述全局规划步骤：

分别找到离起点和终点最近的轨迹拓扑地图节点，然后使用A*在轨迹拓扑地图上搜索连接这两个拓扑地图节点的最短路径。

具体地，所述局部避障步骤：

首先更新局部目标点在最短路径上的位置，然后根据激光传感器实时获取的点云使用球形VFH寻找安全通道，再把局部目标点和安全通道结合求得新局部目标点，最后根据新局部目标点计算车辆控制信号；

所述更新局部目标点在最短路径上的位置：

导航开始时，局部目标点在最短路径的起点节点出发，以大于车辆最大速度的预设速度沿最短路径向终点节点前进，若局部目标点与车辆所在的位置之间被地图点云阻挡，则局部目标点停止前进直至不再被地图点云阻挡；

所述使用球形VFH寻找安全通道：

首先在激光扫过的区域按一定角度的分辨率分成若干方向，计算每个方向上离最近的障碍物有多少个与车辆等宽的球体，然后判断球体数量是否大于第二预设数量或是否存在若干连续方向上的球体数量比扫描区域边界方向的球体数量大：若是，则判定所述方向为安全通道，划出安全通道的角度，获得安全通道；否则，所述方向不认为是安全通道；

进一步地，根据附图11所示，划出安全通道的一个条件是球体数量是否超过一个阈值，另一个条件是这一连续方向上的球体数量是否大于其边界方向上的球体数量，两个符合一个即可认为是安全通道。

所述用局部目标点和安全通道结合求新局部目标点：

判断是否局部目标点在安全通道内：若局部目标点在安全通道内，则新局部目标点与局部目标点相同；否则，则找到与局部目标点角度差最小的安全通道，以安全通道最靠近车辆的边界点为新局部目标点；

所述计算车辆控制信号：

根据新局部目标点与车辆的距离计算车辆的前进速度控制信号，根据新局部目标点与车辆前进方向的角度差和局部目标点前进方向与车辆前进方向的角度差，计算车辆速度控制信号。

具体地，所述更新局部目标点在最短路径上的位置：

设局部目标点为plt(j)，在导航过程中，局部目标点发起于起点plt(0)＝Ns，终止于终点plt(0)＝Ne，按照以下条件沿最短路径P(V，E)行进：

plt∈Ep(i)

式中，

plt表示局部目标点位置；

Ep(i)表示第i段路径；

表示局部目标点的移动速度；

α表示常量参数，大于1，一般可取2；

vmax_robot表示机器人移动的最大速度；

表示第i段路径的终点；

表示第i段路径的起点；

clear(plt，probot)表示两点之间没有被地图点阻挡。

公式(1)表示当局部目标点和小车自身位置点没有被地图点阻挡时，局部目标点以大于小车最大速度的速度沿最短路径P(V，E)行进，否则停止等待；

所述计算车辆控制信号：

若局部目标点在某个安全通道的朝向内，则用下式计算小车的控制信号：

ωc＝kωαθlt_robot-kωβ(θlt-θrobot) (2)

其中，

vc表示小车控制输入线速度；

kv表示常量参数；

表示局部目标点位置；

表示小车当前位置；

ωc表示小车控制输入角速度；

kωα表示常量参数；

θlt_robot表示局部目标点相对小车的角度差；

kωβ表示常量参数；

θlt表示局部目标点移动的方向角度；

θrobot表示小车当前移动方向角度；

若局部目标点不在某个安全通道的朝向内，则选取与局部目标点方向最近的安全通道，把被选取的安全通道离小车最近的边缘点作为新局部目标点，用下式计算小车的控制信号：

ωc＝kωαθnbcp_robot-kωβ(θlt-θrobot) (3)

其中，

dball表示球体的直径；

wfront表示正前方的扫描值；

θnbcp_robot表示被选择的安全通道离小车最近的边缘点相对小车的角度差。

本发明提供的基于轨迹拓扑地图和避障的自主车辆导航系统，可以通过本发明给的基于轨迹拓扑地图和避障的车辆导航方法的步骤流程实现。本领域技术人员可以将所述基于轨迹拓扑地图和避障的车辆导航方法，理解为所述基于轨迹拓扑地图和避障的自主车辆导航系统的一个优选例。

根据本发明提供的一种基于轨迹拓扑地图和避障的自主车辆导航系统，包括：

建图模块：构建环境地图，根据构建的环境地图生成导航用轨迹拓扑地图；

全局规划模块：利用路径搜索算法A*在生成的轨迹拓扑地图上搜索连接起点与终点的全局最短路径；

局部避障模块：利用获得的全局最短路径实时生成局部目标点，结合局部目标点和激光传感器实时探测的环境信息生成车辆控制信号控制车辆躲避障碍物并到达目的地。

具体地，所述建图模块：

通过SLAM构建环境地图，保存从SLAM获得的地图点云和构建地图时走过的轨迹；

根据从SLAM获得的地图点云和构建地图时走过的轨迹，清除轨迹点周围一定距离阈值内的地图点云；

用八叉树把轨迹点进行分割，计算每个八叉树叶节点内所有轨迹点的重心，若某两个重心之间的距离小于第一预设阈值，则剔除其中一个重心点，把余下的重心点作为拓扑地图的节点；

获取两两节点间的连通性，根据两两节点间的连通性生成拓扑地图的边，并去除冗余边，获得轨迹拓扑地图；

所述获取两两节点间的连通性：

判断两节点间距离大于第二预设阈值：若两节点距离大于第二预设阈值，则判定两节点之间无连通性；否则，则判断两节点之间是否被地图点云阻挡：在两节点间的连线上紧密排布与车辆宽度一致的球体，球心间隔与球体直径相同，若存在某个球体内包含地图点数量大于第一预设数量，则认为两节点被阻挡；否则，则认为两节点连通，两节点之间不被地图点云阻挡；

所述去除冗余边：

遍历所有边，若存在某三条边组成一个回环，则删除最长的边。

具体地，所述全局规划模块：

分别找到离起点和终点最近的轨迹拓扑地图节点，然后使用A*在轨迹拓扑地图上搜索连接这两个拓扑地图节点的最短路径；

所述局部避障模块：

首先更新局部目标点在最短路径上的位置，然后根据激光传感器实时获取的点云使用球形VFH寻找安全通道，再把局部目标点和安全通道结合求得新局部目标点，最后根据新局部目标点计算车辆控制信号；

所述更新局部目标点在最短路径上的位置：

导航开始时，局部目标点在最短路径的起点节点出发，以大于车辆最大速度的预设速度沿最短路径向终点节点前进，若局部目标点与车辆所在的位置之间被地图点云阻挡，则局部目标点停止前进直至不再被地图点云阻挡；

所述使用球形VFH寻找安全通道：

首先在激光扫过的区域按一定角度的分辨率分成若干方向，计算每个方向上离最近的障碍物有多少个与车辆等宽的球体，然后判断球体数量是否大于第二预设数量或是否存在若干连续方向上的球体数量比扫描区域边界方向的球体数量大：若是，则判定所述方向为安全通道，划出安全通道的角度，获得安全通道；否则，所述方向不认为是安全通道；

所述用局部目标点和安全通道结合求新局部目标点：

判断是否局部目标点在安全通道内：若局部目标点在安全通道内，则新局部目标点与局部目标点相同；否则，则找到与局部目标点角度差最小的安全通道，以安全通道最靠近车辆的边界点为新局部目标点；

所述计算车辆控制信号：

根据新局部目标点与车辆的距离计算车辆的前进速度控制信号，根据新局部目标点与车辆前进方向的角度差和局部目标点前进方向与车辆前进方向的角度差，计算车辆速度控制信号；

所述更新局部目标点在最短路径上的位置：

设局部目标点为plt(j)，在导航过程中，局部目标点发起于起点plt(0)＝Ns，终止于终点plt(0)＝Ne，按照以下条件沿最短路径P(V，E)行进：

plt∈Ep(i)

式中，

plt表示局部目标点位置；

Ep(i)表示第i段路径；

表示局部目标点的移动速度；

α表示常量参数，大于1，一般可取2；

vmax_robot表示机器人移动的最大速度；

表示第i段路径的终点；

表示第i段路径的起点；

clear(plt，probot)表示两点之间没有被地图点阻挡。

公式(1)表示当局部目标点和小车自身位置点没有被地图点阻挡时，局部目标点以大于小车最大速度的速度沿最短路径P(V，E)行进，否则停止等待；

所述计算车辆控制信号：

若局部目标点在某个安全通道的朝向内，则用下式计算小车的控制信号：

ωc＝kωαθtt_robot-kωβ(θlt-θrobot) (2)

其中，

vc表示小车控制输入线速度；

kv表示常量参数；

表示局部目标点位置；

表示小车当前位置；

ωc表示小车控制输入角速度；

kωα表示常量参数；

θlt_robot表示局部目标点相对小车的角度差；

kωβ表示常量参数；

θlt表示局部目标点移动的方向角度；

θrobot表示小车当前移动方向角度；

若局部目标点不在某个安全通道的朝向内，则选取与局部目标点方向最近的安全通道，把被选取的安全通道离小车最近的边缘点作为新局部目标点，用下式计算小车的控制信号：

ωc＝kωαθnbcp_robot-kωβ(θlt-θrobot) (3)

其中，

dball表示球体的直径；

wfront表示正前方的扫描值；

θnbcp_robot表示被选择的安全通道离小车最近的边缘点相对小车的角度差。

根据本发明提供的一种存储有计算机程序的计算机可读存储介质，其特征在于，所述计算机程序被处理器执行时实现上述中任一项所述的基于轨迹拓扑地图和避障的车辆导航方法的步骤。

下面通过优选例，对本发明进行更为具体地说明。

优选例1：

本发明要解决的技术问题是利用稀疏不均匀的SLAM地图点云进行路径规划，同时改进VFH若干方面，把两者结合构成一个小车自主导航系统。

为了解决这一技术问题，本发明提供了一种基于SLAM轨迹拓扑地图和球形VFH避障的自主导航小车系统，其中提供了：利用激光或视觉SLAM的信息生成导航用轨迹拓扑地图的方法；基于轨迹拓扑地图进行全局导航的方法；基于球形VFH的避障方法。

该系统包含如下硬件：地面小车，双面相机和激光雷达，惯性传感器。

该方法包括如下步骤：

建图阶段：使用SLAM构建环境地图，使用SLAM的地图生成导航用轨迹拓扑地图；

全局规划阶段：利用A*在轨迹拓扑地图搜索连接起点与终点的全局最短路径；

局部避障阶段：利用全局路径实时生成局部目标点，结合局部目标点和激光传感器实时探测的环境信息生成小车控制信号控制小车躲避障碍物并到达目的地。

在建图阶段，首先使用SLAM把环境地图构建完全，保存从SLAM获得的地图点云和构建地图时走过的轨迹。其次，清除轨迹点周围一定距离阈值内的地图点云。再次，用八叉树把轨迹点进行分割，计算每个八叉树叶节点内所有轨迹点的重心，若某两个重心之间的距离小于设定阈值，则剔除其中一个重心点，把余下的重心点作为拓扑地图的节点。最后，根据两两节点间的连通性生成拓扑地图的边，并去除冗余边。

在判断两两节点连通性时，使用如下条件：两节点距离小于一定阈值；两节点之间不被地图点云阻挡。

在判断两节点是否被地图点云阻挡时，在两节点间的连线上紧密排布与小车宽度一致的球体，球心间隔与球体直径相同，若存在某个球体内包含地图点数量大于设定阈值，则认为两节点被阻挡，否则认为两节点连通。

在去除冗余边时，遍历所有边，若存在某三条边组成一个回环，则删除最长的边。

在全局规划阶段，首先分别找到离起点和终点最近的拓扑地图节点，然后使用A*在拓扑地图上搜索连接这两个拓扑地图节点的最短路径。

在局部避障阶段，首先更新局部目标点在最短路径上的位置，然后根据激光传感器实时获取的点云使用球形VFH寻找安全通道，再把局部目标点和安全通道结合求得新局部目标点，最后根据新局部目标点计算小车控制信号。

在更新局部目标点在最短路径上的位置时，导航开始时，局部目标点在最短路径的起点节点出发，以大于小车最大速度的速度沿最短路径向终点节点前进，若局部目标点与小车所在的位置之间被地图点云阻挡，则局部目标点停止前进直至不再被地图点云阻挡。

在使用球形VFH寻找安全通道时，首先在激光扫过的区域按一定角度的分辨率分成若干方向，计算每个方向上离最近的障碍物有多少个与小车等宽的球体，然后根据球体数目是否大于阈值或是否存在比两端大的若干连续方向，划出安全通道的角度。

在用局部目标点和安全通道结合求新局部目标点时，若局部目标点在某个安全通道内，则新局部目标点与局部目标点相同；否则找到与局部目标点角度差最小的安全通道，以安全通道最靠近小车的边界点为新局部目标点。

在计算小车控制信号时，根据新局部目标点与小车的距离计算小车的前进速度控制信号，根据新局部目标点与小车前进方向的角度差和局部目标点前进方向与小车前进方向的角度差，计算小车选择速度的控制信号。

优选例2：

以下将结合图至图对本发明提供的基于SLAM轨迹拓扑地图和球形VFH避障的自主导航小车系统进行详细的描述，其为本发明一可选的实施例，可以认为，本领域的技术人员在不改变本发明精神和内容的范围内能够对其进行修改和润色。

本实施例提供了基于SLAM轨迹拓扑地图和球形VFH避障的自主导航小车系统，其中提供了：利用激光或视觉SLAM的信息生成导航用轨迹拓扑地图的方法；基于轨迹拓扑地图进行全局导航的方法；基于球形VFH的避障方法。

主要过程通过C++来实现，该方法包括如下步骤：

建图阶段：使用SLAM构建环境地图，使用SLAM的地图生成导航用轨迹拓扑地图；

全局规划阶段：利用A*在轨迹拓扑地图搜索连接起点与终点的全局最短路径；

局部避障阶段：利用全局路径实时生成局部目标点，结合局部目标点和激光传感器实时探测的环境信息生成小车控制信号控制小车躲避障碍物并到达目的地。

在建图阶段，首先使用SLAM把环境地图构建完全，保存从SLAM获得的地图点云和构建地图时走过的轨迹。其次，清除轨迹点周围一定距离阈值内的地图点云。再次，用八叉树把轨迹点进行分割，计算每个八叉树叶节点内所有轨迹点的重心，若某两个重心之间的距离小于设定阈值，则剔除其中一个重心点，把余下的重心点作为拓扑地图的节点。最后，根据两两节点间的连通性生成拓扑地图的边，并去除冗余边。

在判断两两节点连通性时，使用如下条件：两节点距离小于一定阈值；两节点之间不被地图点云阻挡。

在判断两节点是否被地图点云阻挡时，在两节点间的连线上紧密排布与小车宽度一致的球体，球心间隔与球体直径相同，若存在某个球体内包含地图点数量大于设定阈值，则认为两节点被阻挡，否则认为两节点连通。

在去除冗余边时，遍历所有边，若存在某三条边组成一个回环，则删除最长的边。

在全局规划阶段，首先分别找到离起点和终点最近的拓扑地图节点，然后使用A*在拓扑地图上搜索连接这两个拓扑地图节点的最短路径。

在局部避障阶段，首先更新局部目标点在最短路径上的位置，然后根据激光传感器实时获取的点云使用球形VFH寻找安全通道，再把局部目标点和安全通道结合求得新局部目标点，最后根据新局部目标点计算小车控制信号。

在更新局部目标点在最短路径上的位置时，导航开始时，局部目标点在最短路径的起点节点出发，以大于小车最大速度的速度沿最短路径向终点节点前进，若局部目标点与小车所在的位置之间被地图点云阻挡，则局部目标点停止前进直至不再被地图点云阻挡。

在使用球形VFH寻找安全通道时，首先在激光扫过的区域按一定角度的分辨率分成若干方向，计算每个方向上离最近的障碍物有多少个与小车等宽的球体，然后根据球体数目是否大于阈值或是否存在比两端大的若干连续方向，划出安全通道的角度。

在用局部目标点和安全通道结合求新局部目标点时，若局部目标点在某个安全通道内，则新局部目标点与局部目标点相同；否则找到与局部目标点角度差最小的安全通道，以安全通道最靠近小车的边界点为新局部目标点。

在计算小车控制信号时，根据新局部目标点与小车的距离计算小车的前进速度控制信号，根据新局部目标点与小车前进方向的角度差和局部目标点前进方向与小车前进方向的角度差，计算小车选择速度的控制信号。

本发明创造性地发现到，SLAM建图的轨迹可以去除大部分影响导航的错误地图点，大大提高利用SLAM地图构建拓扑地图的可靠性。本发明创造性地用球体探测所有方向的安全通道，保证了找到的安全通道必然可以通过小车。本发明创造性地构造结合全局路径的局部目标点和安全通道的控制器，可以在不预先规划轨迹的情况下控制小车躲避障碍物并安全到达目的地。

以下将对其中内容进行进一步展开，其中只详细描述本方法提出算法的部分内容。

步骤一、轨迹拓扑地图生成

本发明的两个实施例的实验场景见图1和图6。首先在实验场景内使用激光SLAM或视觉SLAM把完整的环境地图构建好，如图2、图4、图7和图9，保存SLAM地图的地图点云和轨迹点。其次，清除轨迹点周围一定距离阈值内的地图点云。再次，用八叉树把轨迹点进行分割，计算每个八叉树叶节点内所有轨迹点的重心，若某两个重心之间的距离小于设定阈值，则剔除其中一个重心点，把余下的重心点作为拓扑地图的节点。最后，根据两两节点间的连通性生成拓扑地图的边，并去除冗余边。至此即可生成轨迹拓扑地图G(V，E)，如图3、图5、图8和图10。

判断两节点的连通性使用的方法是个递归的过程。首先计算两点的中点，以中点为球心小车宽度为直径，找出球内的所有地图点，若地图点数目大于设定阈值，则认为两点被阻挡，否则，判断中点与其中一个点的距离是否大于小车宽度，若大于则分别以中点与两个点重复上述过程，否则，认为两点连通。

步骤二、全局规划

分别找到离起点Ns与终点Ne最近的拓扑节点为起始节点Vs与终止节点Ve，分别连接Ns与Vs以及Ne与Ve，直接在拓扑地图上使用A*找到连接Ns与Ne的最短路径P(V，E)。

步骤三、局部目标点更新

局部目标点是带领小车沿最短拓扑路径到达目的地的重要保证，也为避障算法提供了重要的参考局部目的地。

设局部目标点为plt(j)，在导航过程中，局部目标点发起于起点plt(0)＝Ns，终止于终点plt(0)＝Ne，按照以下条件沿最短路径P(V，E)行进：

plt∈Ep(i)

式中，

plt表示局部目标点位置；

Ep(i)表示第i段路径；

表示局部目标点的移动速度；

α表示常量参数，大于1，一般可取2；

vmax_robot表示机器人移动的最大速度；

表示第i段路径的终点；

表示第i段路径的起点；

clear(plt，probot)表示两点之间没有被地图点阻挡。

式(1)表示当局部目标点和小车自身位置点没有被地图点阻挡时，局部目标点以大于小车最大速度的速度沿最短路径P(V，E)行进，否则停止等待。

步骤四、搜索安全区域

在获得激光输入的实时障碍物点云后，本发明算法首先绕z轴(竖直向上)把的角度按一定分辨率分成若干方向，依次朝每个方向进行如下扫描操作：从离小车最近的一点开始沿这个方向排布球体，每添加一个球，计算球内障碍物点的数量，若数量超过阈值，则放弃此球体并停止添加球体，把此方向添加的总球体数作为此方向的扫描值。图11的scanj为j方向的一次扫描操作示例。

获得所有扫描值的同时，若本次扫描值比上一个朝向的扫描值大/小，则认为本次扫描可能获得一个安全通道的右/左边缘，如图11的scanj和scani；若本次扫描值大于阈值，则认为本次扫描为一个安全方向，如图11的scanm和scank之间的扫描。根据上述条件可搜索得到之间所有的安全通道，由于球体扫描的保证，这些通道均可通过小车，如图11的两个黄色扫描安全通道。

步骤五、避障与控制

这一步的输入是前面获得的局部目标点和所有安全通道。小车控制信号输入是vc和ωc。若局部目标点在某个安全通道的朝向内，则用下式计算小车的控制信号：

ωc＝kωαθlt_robot-kωβ(θlt-θrobot) (2)

其中，

vc表示小车控制输入线速度；

kv表示常量参数；

表示局部目标点位置；

定示小车当前位置；

ωc表示小车控制输入角速度；

kωα表示常量参数；

θlt_robot表示局部目标点相对小车的角度差；

kωβ表示常量参数；

θlt表示局部目标点移动的方向角度；

θrobot表示小车当前移动方向角度；

否则，选取与局部目标点方向最近的安全通道，把被选取的安全通道离小车最近的边缘点作为新局部目标点，用下式计算小车的控制信号：

ωc＝kωαθnbcp_robot-kωβ(θlt-θrobot) (3)

其中，

dball表示球体的直径

wfront表示正前方的扫描值

θnbcp_robot表示被选择的安全通道离小车最近的边缘点相对小车的角度差

式(2)和式(3)中角度的定义相似，所有的k都是常量，dball是一个球体的直径，Wfront是正前方的扫描值，图12是式(2)的角度定义。

我们利用以上步骤分别在仿真和真实场景下分别使用激光和视觉SLAM各进行了两组导航实验，所有实验均能安全导航至目的地，小车导航的轨迹结果见图13-图16。从拓扑地图生成的结果看，本发明对SLAM的地图点疏密度的敏感度较低，在特征少或错误地图点多的场景均能正确生成拓扑地图。实验结果表明，本发明可有效使用不同SLAM在已知环境内进行导航。

优选例3：

本发明提供了一种基于SLAM轨迹拓扑地图和球形VFH避障的自主导航小车系统。本发明提供了：利用激光或视觉SLAM的信息生成导航用轨迹拓扑地图的方法；基于轨迹拓扑地图进行全局导航的方法；基于球形VFH的避障方法。

该方法包括如下步骤：

建图阶段：使用SLAM构建环境地图，使用SLAM的地图生成导航用轨迹拓扑地图；

全局规划阶段：利用A*在轨迹拓扑地图搜索连接起点与终点的全局最短路径；

局部避障阶段：利用全局路径实时生成局部目标点，结合局部目标点和激光传感器实时探测的环境信息生成小车控制信号控制小车躲避障碍物并到达目的地。

轨迹拓扑地图把SLAM的轨迹和点云地图信息转变为便于导航的拓扑地图信息。

在生成局部目标点时，设置目标点在最短路径上以高于小车的速度行进，并在小车与局部目标点之间被地图记录的障碍物阻挡时停下等候小车，确保局部目标点在小车前方带领小车，初步躲避地图记录的障碍物，辅助局部避障。

小车与局部目标点之间被障碍物阻挡的判断方法，是在小车与局部目标点之间的连线上排满与小车宽度等大的球体，计算球体内的地图点的数量，当地图点数量超过一定阈值时认为两者被阻挡。

球形VFH方法，是在传统VFH方法上，把原方法一个方向的距离用单点距离表示，改为用球体区域探测，把不包含障碍物点的球体个数替代单点距离。

在得到所有方向上的障碍物距离后，把距离变大的连续方向或距离大于阈值的连续方向认为是安全通道。

在无额外障碍物时，控制器直接用局部目标点与小车的距离和朝向差计算控制量；在有额外障碍物时，控制器把VFH得到的安全通道的边界点作为新局部目标点计算控制量，无须规划轨迹。

在本申请的描述中，需要理解的是，术语“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本申请和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本申请的限制。

本领域技术人员知道，除了以纯计算机可读程序代码方式实现本发明提供的系统、装置及其各个模块以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统、装置及其各个模块以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同程序。所以，本发明提供的系统、装置及其各个模块可以被认为是一种硬件部件，而对其内包括的用于实现各种程序的模块也可以视为硬件部件内的结构；也可以将用于实现各种功能的模块视为既可以是实现方法的软件程序又可以是硬件部件内的结构。

以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本申请的实施例和实施例中的特征可以任意相互组合。