适用狭窄空间的无人车改进TEB导航方法

摘要

针对狭窄通道通过率较低且无法自主导航的问题，提出一种适用狭窄空间的无人车改进TEB导航方法，所述方法包括：构建含狭窄通道的环境栅格地图；利用改进RRT算法和改进TEB算法分别进行全局路径规划和局部路径规划，完成无人车的自主导航，保证了导航路线的安全性和可行性；通过添加位姿辅助点，在经过狭窄通道时调用定点规划算法，保证了狭窄通道的通过率；利用速度插补控制和S形速度规划分别优化自主导航和定点导航的输出速度，保证了输出曲线的平滑和导航过程的稳定性。通过gazebo仿真实验，验证了改进导航方法的可行性，且无人车在狭窄通道的通过率显著提升，输出自主导航和定点导航的速度曲线，两个阶段的速度曲线波动均小于0.05m/s。

权利要求

1.一种适用狭窄空间的无人车改进TEB导航方法，其特征是：S1、构建含有狭窄通道的环境栅格代价地图；

S2、在狭窄通道两侧添加位姿辅助点，设定距离阈值；

S3、获取无人车位置和目标点，调用全局路径规划器，获取全局路径；

S4、基于全局路径使用改进TEB算法进行局部路径规划；

S5、当无人车到达位姿辅助点时，调用定点规划算法，进行定点导航；

S6、再次获取全局路径，基于改进TEB算法，完成剩余路径的自主导航；

所述S4基于全局路径使用改进TEB算法进行局部路径规划，当无人车和位姿辅助点距离小于距离阈值时，将位姿辅助点作为局部目标点，具体包括如下子步骤：S41、输入全局路径，获取无人车初始位姿s1、位姿辅助点sa1、sa2以及目标点位姿sn；

S42、截取一段全局路径，获取无人车的初始化轨迹L，包括无人车在世界坐标系下的位姿集合S和时间间隔集合τ，具体如下：si＝[xi,yi,θi]    (1)

S＝{si}i＝0,1,2…,nn∈N    (2)τ＝{ΔTi}i＝0,1,2…,n‑1n∈N    (3)L:＝(S,τ)＝{s1,ΔT1,s2,ΔT2,…ΔTn‑1,sn}n∈N    (4)其中，xi,yi,θi分别表示无人车位置坐标(xi,yi)和位姿角θi，si为无人车当前位姿，ΔTi为相邻位姿默认时间间隔；

S43、构建靠近障碍物的约束fobs和远离全局路径的约束fpath，具体如下：其中，dmin,j为全局路径点和障碍物之间的直线最短距离， 为无人车与障碍物之间的距离阈值， 为无人车与全局路径的距离阈值；

S44、进入迭代求解循环，调节局部轨迹的长度，保证局部规划优化的长度不发生变化，剔除无人车经过的固定长度轨迹；

S45、建立超图(hyper‑graph)，构建多目标优化函数来优化超图，通过构建非线性最小*二乘代价函数来获得最优轨迹L，具体如下：

其中，fT为目标函数、fP为惩罚函数、σk为权重值、L/{s1,sn}表示初始位姿，s1和目标点位姿sn是固定的，不受优化；

S46、验证轨迹是否可行；

S47、连用相邻两个位姿间的欧氏距离、方位角变化量和两个位姿的时间隔间有限差分估算无人车的速度，具体如下：其中，vi为无人车的线速度值、ωi为无人车的角速度值、xi为无人车x方向位姿、yi为无人车y方向位姿、θi为无人车方向角变化量、ΔTi为两个位姿的时间间隔；

利用速度插补规划算法对无人车的输出速度进行约束，具体如下：其中，Δvx、Δvω、Δv分别为相邻位姿间线速度、角速度和速度变化量，I、D、W为中间参量，av、jv、aω、jω分别为无人车的线加速度、线急动度、角加速度、角急动度，vpub和ωpub为发送给无人车的线速度和角速度；

S48、更新环境地图中无人车和障碍物的位置信息；

S49、判断当前位置和sa1距离是否小于threshold，threshold为距离阈值，若小于将sa1作为局部目标点，到达sa1结束该阶段的局部路径规划，否则，返回步骤S42；

所述S5当无人车到达位姿辅助点时，调用定点规划算法，进行定点导航，具体包括如下子步骤：S51、获取位姿辅助点sa2的位姿，根据sa2的方向调整无人车车头方向；

S52、获取无人车最大线速度vmax和最大角速度ωmax，采用7段S形加减速算法进行速度规划，将定点规划划分为7个阶段：加加速阶段、匀加速阶段、减加速阶段、匀速阶段、加减速阶段、匀减速阶段、减减速阶段，具体如下：其中，j为急动度、a为加速度、v为加速度、t为时间点、ti为过渡时间点、τi为局部时间点，表示各个阶段的初始时刻；

S53、根据速度规划结果，从sa1直线运动到sa2，完成定点导航。

说明书

适用狭窄空间的无人车改进TEB导航方法

技术领域

[0001] 本发明涉及人工智能与无人系统领域，具体为无人车局部路径规划自主导航技术领域，尤其涉及一种适用狭窄空间的无人车改进TEB导航方法。

背景技术

[0002] 路径规划作为无人车的研究的核心技术，针对这一领域国内外已经进行了众多研究。无人车局部路径规划主要利用DWA算法和TEB算法，其中TEB算法作为一种全局路径的后续优化算法，可以产生一条从起点到局部目标点的弹性带，将路径规划问题转化为图优化问题，适用于完整和非完整约束的无人车。但是，TEB在面对复杂环境，尤其是狭窄通道这种空间不足的场景，无人车无法充分调整位姿，导致通过性和通过率都不理想。

[0003] 针对上述问题，论文《在狭窄通道环境下书库管理机器人的最优路径规划》提出一种定点位置规划的方法，存在如下问题：

[0004] (1)只能实现点对点的定点导航，无法完成机器人的自主导航，容易受环境和人为因素影响；

[0005] (2)未考虑机器人的速度约束，容易出现速度及加速度跳变现象，无法保证速度曲线的平滑输出。

发明内容

[0006] 本发明提供一种适用狭窄空间的无人车改进TEB导航方法，通过添加位姿辅助点，在通过狭窄通道时调用定点规划算法，保证狭窄通道的通过率；利用TEB算法完成机器人的自主导航，保证了导航路线的安全性和可行性，利用速度插补控制保证输出速度曲线的平滑。

[0007] 未达到上述目的，本发明提供一种基于改进TEB针对狭窄空间的无人车路径规划方法，包括以下步骤：

[0008] S1、构建含有狭窄通道的环境栅格代价地图；

[0009] S2、在狭窄通道两侧添加位姿辅助点，设定距离阈值；

[0010] S3、获取无人车位置和目标点，调用全局路径规划器，获取全局路径；

[0011] S4、基于全局路径使用改进TEB算法进行局部路径规划；

[0012] S5、当无人车到达位姿辅助点时，调用定点规划算法，进行定点导航；

[0013] S6、再次获取全局路径，基于改进TEB算法，完成剩余路径的自主导航；

[0014] 所述S2中，分别在狭窄通道的入口和出口位置，分别添加位姿辅助点sa1和sa2，并设定距离阈值threshold；

[0015] 所述S3中，获取无人车位置和目标点，调用全局路径规划器，获取全局路径，具体包括如下子步骤：

[0016] S31、利用cartography纯定位模式获取无人车的具体位置，并标记无人车的目标点位置及无人车姿态；

[0017] S32、利用改进RRT算法进行全局路径规划，将无人车当前位置作为根节点Xstart，在代价地图中随机生成随机点Xrand，继而在已有节点中寻找与Xrand距离最近的节点Xnear，并在此方向上进行碰撞检测，根据碰撞检测的结果以及概率目标偏置思想，重新生成Xrand，具体公式如下：

[0018]

[0019] 其中，Xend为全局目标点、p为0‑1的随机数、ω为权重因子、ρ为概率因子，flag为碰撞检测结果，flag＝0表示未检测到碰撞，flag＝1表示检测到碰撞；

[0020] S33、在Xnear和重构的Xrand的连线方向上基于步长σ获取新生节点Xnew，具体公式如下：

[0021]

[0022] 其中，newNode.x为Xnew的横坐标、newNode.y为Xnew的纵坐标、nearNode.x为Xnear的横坐标、nearNode.y为Xnear的纵坐标、 为Xrand、Xnear和x轴的夹角，并在此方向上进行碰撞检测，检测Xnew是否为有效节点，若未检测到碰撞则将Xnew加入有效节点集Xnodes；否则，返回步骤S32；

[0023] S34、判断Xnew是否满足|Xend‑Xnew|≤radius，是则根据Xnodes各节点关系，反向连接，输出路径；否则，返回步骤S32；

[0024] S35、利用基于最大转角的逆向寻优和3次B样条曲线拟合对输出路径进行优化。

[0025] 所述S4中，基于全局路径使用改进TEB算法进行局部路径规划，具体包括如下子步骤：

[0026] S41、输入全局路径，获取无人车初始位姿s1、位姿辅助点sa1、sa2以及目标点位姿sn；

[0027] S42、截取一段全局路径，获取无人车的初始化轨迹L，包括无人车在世界坐标系下的位姿集合S和时间间隔集合τ，具体如下：

[0028] si＝[xi,yi,θi]  (3)

[0029] S＝{si}i＝0,1,2…,n n∈N  (4)

[0030] τ＝{ΔTi}i＝0,1,2…,n‑1 n∈N  (5)

[0031] L:＝(S,τ)＝{s1,ΔT1,s2,ΔT2,…ΔTn‑1,sn}n∈N  (6)

[0032] 其中，xi,yi,θi分别表示无人车位置坐标(xi,yi)和位姿角θi，si为无人车当前位姿，ΔTi为相邻位姿默认时间间隔；

[0033] S43、构建靠近障碍物的约束fobs和远离全局路径的约束fpath，具体如下：

[0034]

[0035]

[0036] 其中，dmin,j为全局路径点和障碍物之间的直线最短距离， 为无人车与障碍物之间的距离阈值， 为无人车与全局路径的距离阈值；

[0037] S44、进入迭代求解循环，调节局部轨迹的长度，保证局部规划优化的长度不发生变化，剔除无人车经过的固定长度轨迹；

[0038] S45、建立超图(hyper‑graph)，构建多目标优化函数来优化超图，通过构建非线性*最小二乘代价函数来获得最优轨迹L，具体如下：

[0039]

[0040] 其中，fT为目标函数、fP为惩罚函数、σk为权重值、L/{s1,sn}表示初始位姿s1和目标点位姿sn是固定的，不受优化；

[0041] S46、验证轨迹是否可行；

[0042] S47、连用相邻两个位姿间的欧氏距离、方位角变化量和两个位姿的时间隔间有限差分估算无人车的速度，具体如下：

[0043]

[0044] 其中，vi为无人车线速度值、ωi为无人车角速度值、xi为无人车x方向位姿、yi为无人车y方向位姿、θi为无人车方向角变化量、ΔTi为两个位姿时间间隔；

[0045] 利用速度插补规划算法对无人车的输出速度进行约束，具体如下：

[0046]

[0047]

[0048]

[0049]

[0050] 其中，Δvx、Δvω、Δv分别为相邻位姿间线速度、角速度和速度变化量，I、D、W为中间参量，av、jv、aω、jω分别为无人车的线加速度、线急动度、角加速度、角急动度，vpub和ωpub为发送给无人车的线速度和角速度；

[0051] S48、更新环境地图中无人车和障碍物的位置信息；

[0052] S49、判断当前位置和sa1距离是否小于threshold，若小于将sa1作为局部目标点，到达sa1结束该阶段的局部路径规划，否则，返回步骤S42；

[0053] 所述S5中，当无人车到达位姿辅助点时，调用定点规划算法，进行定点导航，具体包括如下子步骤：

[0054] S51、获取位姿辅助点sa2的位姿，根据sa2的方向调整无人车车头方向；

[0055] S52、获取无人车最大线速度vmax和最大角速度ωmax，采用7段S形加减速算法进行速度规划，将定点规划划分为7个阶段：加加速阶段、匀加速阶段、减加速阶段、匀速阶段、加减速阶段、匀减速阶段、减减速阶段，具体如下：

[0056]

[0057]

[0058]

[0059] 其中，j为急动度、a为加速度、v为加速度、t为时间点、ti为过渡时间点、τi为局部时间点，表示各个阶段的初始时刻；

[0060] S53、根据速度规划结果，从sa1直线运动到sa2，完成定点导航。

[0061] 本发明具有如下有益效果：

[0062] 1.本发明所述方法通过添加位姿辅助点，实现了自主导航与定点导航的综合应用，提高了狭窄通道的通过率，仿真结果表明，在狭窄通道场景下，纯自主导航通过的概率较低，改进方法则显著提升了该场景的通行概率；

[0063] 2.本发明所述方法采用的改进TEB算法，采用了速度插补控制，保证了输出速度的平滑性与可行性，仿真结果表明，在行进过程中，速度波动小于0.05m/s且启动与停止阶段未发生速度大范围突变现象；

[0064] 3.本发明所述方法基于7段式S形规划对定点导航进行速度规划，使得启动与停止阶段的不会出现速度突变，保证了整个过程的稳定性；仿真结果表明，整个定点导航过程速度曲线平滑无突变。

附图说明

[0065] 为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图做以简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

[0066] 图1为适用狭窄空间的无人车改进TEB导航方法流程图；

[0067] 图2为无人车自主导航效果图；

[0068] 图3为自主导航输出速度曲线；

[0069] 图4为无人车定点导航效果图；

[0070] 图5为定点导航输出速度曲线。

具体实施方式

[0071] 为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图1，对本发明进行进一步详细说明。

[0072] S1、构建含有狭窄通道的环境栅格代价地图，包含如下子步骤：

[0073] S11、无人车基于2D激光雷达和cartographer SLAM框架完成含有狭窄通道的环境栅格地图的构建；

[0074] S12、利用navigation框架中的costmap_2d部分将环境栅格地图转换为代价地图，并根据传感器数据更新障碍物信息。

[0075] S2、在狭窄通道两侧添加位姿辅助点，设定距离阈值，包括如下子步骤：

[0076] S21、分别在狭窄通道的入口和出口位置，分别添加位姿辅助点sa1和sa2；

[0077] S22、以辅助点sa1和sa2为圆心，分别设定距离阈值threshold。

[0078] S3、获取无人车位置和目标点，调用全局路径规划器，获取全局路径，具体包括如下子步骤：

[0079] S31、利用cartography纯定位模式获取无人车的具体位置，并标记无人车的目标点位置及无人车姿态；

[0080] S32、利用改进RRT算法进行全局路径规划，将无人车当前位置作为根节点Xstart，在代价地图中随机生成随机点Xrand，继而在已有节点中寻找与Xrand距离最近的节点Xnear，并在此方向上进行碰撞检测，根据碰撞检测的结果以及概率目标偏置思想，重新生成Xrand，具体公式如下：

[0081]

[0082] 其中，Xend为全局目标点、p为0‑1的随机数、ω为权重因子、ρ为概率因子，flag为碰撞检测结果，flag＝0表示未检测到碰撞，flag＝1表示检测到碰撞；

[0083] S33、在Xnear和重构的Xrand的连线方向上基于步长σ获取新生节点Xnew，具体公式如下：

[0084]

[0085] 其中，newNode.x为Xnew的横坐标、newNode.y为Xnew的纵坐标、nearNode.x为Xnear的横坐标、nearNode.y为Xnear的纵坐标、为Xrand、Xnear和x轴的夹角，并在此方向上进行碰撞检测，检测Xnew是否为有效节点，若未检测到碰撞则将Xnew加入有效节点集Xnodes；否则，返回步骤S32；

[0086] S34、判断Xnew是否满足|Xend‑Xnew|≤radius，是则根据Xnodes各节点关系，反向连接，输出路径；否则，返回步骤S32；

[0087] S35、利用基于最大转角的逆向寻优和3次B样条曲线拟合对输出路径进行优化。

[0088] S4、基于全局路径使用改进TEB算法进行局部路径规划，具体包括如下子步骤：

[0089] S41、输入全局路径，获取无人车初始位姿s1、位姿辅助点sa1、sa2以及目标点位姿sn；

[0090] S42、截取一段全局路径，获取无人车的初始化轨迹L，包括无人车在世界坐标系下的位姿集合S和时间间隔集合τ，具体如下：

[0091] si＝[xi,yi,θi]  (3)

[0092] S＝{si}i＝0,1,2…,n n∈N  (4)

[0093] τ＝{ΔTi}i＝0,1,2…,n‑1 n∈N  (5)

[0094] L:＝(S,τ)＝{s1,ΔT1,s2,ΔT2,…ΔTn‑1,sn}n∈N  (6)

[0095] 其中，xi,yi,θi分别表示无人车位置坐标(xi,yi)和位姿角θi，si为无人车当前位姿，ΔTi为相邻位姿默认时间间隔；

[0096] S43、构建靠近障碍物的约束fobs和远离全局路径的约束fpath，具体如下：

[0097]

[0098]

[0099] 其中，dmin,j为全局路径点和障碍物之间的直线最短距离， 为无人车与障碍物之间的距离阈值， 为无人车与全局路径的距离阈值；

[0100] S44、进入迭代求解循环，调节局部轨迹的长度。保证局部规矩优化的长度不发生变化，剔除无人车经过的固定长度轨迹；

[0101] S45、建立超图(hyper‑graph)，构建多目标优化函数来优化超图，通过构建非线性*最小二乘代价函数来获得最优轨迹L，具体如下：

[0102]

[0103] 其中，fT为目标函数、fP为惩罚函数、σk为权重值、L/{s1,sn}表示初始位姿s1和目标点位姿sn是固定的，不受优化；

[0104] S46、验证轨迹是否可行；

[0105] S47、连用相邻两个位姿间的欧氏距离、方位角变化量和两个位姿的时间隔间有限差分估算无人车的速度，具体如下：

[0106]

[0107] 其中，vi为无人车线速度值、ωi为无人车角速度值、xi为无人车x方向位姿、yi为无人车y方向位姿、θi为无人车方向角变化量、ΔTi为两个位姿时间间隔；

[0108] 利用速度插补规划算法对无人车的输出速度进行约束，具体如下：

[0109]

[0110]

[0111]

[0112]

[0113] 其中，Δvx、Δvω、Δv分别为相邻位姿间线速度、角速度和速度变化量，I、D、W为中间参量，av、jv、aω、jω分别为无人车的线加速度、线急动度、角加速度、角急动度，vpub和ωpub为发送给无人车的线速度和角速度；

[0114] S48、更新环境地图中无人车和障碍物的位置信息；

[0115] S49、判断当前位置和sa1距离是否小于threshold，若小于将sa1作为局部目标点，到达sa1结束该阶段的局部路径规划；否则，返回步骤S42。

[0116] S5、当无人车到达位姿辅助点时，调用定点规划算法，进行定点导航，具体包括如下子步骤：

[0117] S51、获取位姿辅助点sa2的位姿，根据sa2的方向调整无人车车头方向；

[0118] S52、获取无人车最大线速度vmax和最大角速度ωmax，采用7段S形加减速算法进行速度规划，将定点规划划分为7个阶段：加加速阶段、匀加速阶段、减加速阶段、匀速阶段、加减速阶段、匀减速阶段、减减速阶段，具体如下：

[0119]

[0120]

[0121]

[0122] 其中，j为急动度、a为加速度、v为加速度、t为时间点、ti为过渡时间点、τi为局部时间点，表示各个阶段的初始时刻；

[0123] S53、根据速度规划结果，从sa1直线运动到sa2，完成定点导航。

[0124] S6、再次获取全局路径，基于改进TEB算法，完成剩余路径的自主导航，具体包括如下子步骤：

[0125] S61、以sa2为起始点，sn为目标点，再次利用改进RRT算法进行全局路径规划，获取全局路径，具体步骤同S3；

[0126] S62、输入全局路径，获取无人车位姿辅助点sa2以及目标点位姿sn，利用改进TEB完成剩余路径的局部路径规划和自主导航，直至到达目标点sn。

[0127] 自主导航效果如图2所示，图中坐标系为cartography建图时所用的局部坐标系，每次相隔固定时间会设立坐标系用于无人车定位。例如(0,16)表示第一次建图第十六次定位，狭窄通道两侧设有位姿辅助点，实线为全局规划器规划的全局路径，箭头为局部规划器规划的局部路径，无人车按照规划的轨迹行进，狭窄通道两侧自主导航速度曲线如图3所示，可以看出速度曲线较为平滑，在启动和停止阶段速度值出现了波动，但波动均小于0.05m/s，未发生大范围突变现象；定点导航效果如图4所示，其中无人车宽度为30cm，通道宽度为40cm，无人车添加位姿辅助点后可以无碰撞通过，定点导航速度曲线如图5所示，将整个过程划分为7段，实现了速度的平滑过渡。

[0128] 以上所述，仅是本发明的较佳实施例而已，并非对本发明作任何形式上的限制，任何熟悉本专业的技术人员在不脱离本发明技术方案范围内，当可利用上述揭示的技术内容做出些许更动或修饰为等同变化的等效实施例，但凡是未脱离本发明技术方案的内容，依据本发明的技术实质对以上实施例所做的任何简单修改、等同变化与修饰，均仍属于本发明技术方案的范围内。