一种无人车自主跟随方法和装置

本发明涉及自主导航，尤其是涉及一种无人车自主跟随方法和装置。

背景技术：

1、近年来，移动机器人自主导航技术发展迅速，并在人们日常生活和工业领域充分发挥其价值。高精度的自主导航技术能够使移动机器人在没有人工干预的条件下，快速并且准确的实现从起源地到目的地的运动并执行相应的任务，如室内导航机器人、送货机器人等。

2、过去的环卫行业往往需要依靠大量的人力，尽管最近几年机械化环卫车辆已经在推广，但依然需要人工驾驶，人力成本一直是环卫服务公司运营成本的大头。因此，如何控制无人车自主跟随环卫工人，同时完成相应清扫任务是一个亟待解决的问题。

3、实现无人车自主跟随环卫工人属于自主导航技术，与自动驾驶技术不同，自动驾驶技术主要是在汽车驾驶道路上构建高精度地图，在交通规则的基础上衍生的自主驾驶。而无人车自主跟随环卫工人所服务的环卫场景有着可行区域面积大、低速、低安全性等特点，需要在路径规划的过程中进行考虑。

4、移动机器人关键技术分为地图构建和路径规划。

5、在地图构建过程中，由于导航需求不同，因此采用的地图形式也各不相同。目前在移动机器人领域采用的地图形式主要是二维栅格地图、三维栅格地图和高程地图。栅格地图将需要构建的区域划分维相同分辨率大小的正方形或正方体，每个栅格带有已占据、未占据和未知三种结构信息，以描述整个环境的真实情况。二维栅格地图只保留了2维平面的信息，对于地面端的移动机器人运动规划和避障较为方便，但其不能完全地表征环境。3维栅格地图则是对3维世界的完全表征，但是这种地图形式需要消耗巨大的内存以及计算能力，对硬件的要求较高且难以保证实时性的要求。

6、在路径规划过程中，主要分为全局路径规划和局部路径规划。全局路径规划主要分为基于采样类算法和基于搜索类算法，基于搜索类的算法源自dijkstra算法，其巧妙地运用了贪婪思想，以广度优先的策略进行搜索，但其搜索效率不高，因此后续又出现了a*、lpa*、d*等高效的搜索类算法。基于采样类的算法源自rrt算法，其通过在几何空间中不断采样并连接采样点以获得一条从起点到达终点的路径，但是不能保证路径的最优性。因此后续的rrt*、bit*、inform rrt*等算法在路径的最优性以及算法效率上进行了改善。但是诸如以上的算法都是在几何空间中进行路径规划，生成的轨迹都是粗糙的无碰撞轨迹，未考虑车辆的动力学约束。在局部路径规划中，全局路径规划出来的路径往往是粗糙的，无人车无法平滑执行的轨迹，因此需要在局部路径规划中使用多项式曲线作为轨迹表示方式来设计出一条连续、平滑的局部轨迹。然而多项式曲线在分段处难以保证光滑性，需要在分段出施加更多的连续性约束。

7、综上，现有技术存在以下不足：

8、一是现有地图表征方式不能充分且不冗余地表征环卫场景下的环境；二是在全局路径规划阶段规划出的全局路径不包含无人车的动力学约束，导致在局部路径规划阶段出现规划的轨迹不可行的情况；三是在局部路径规划阶段，基于多项式曲线、bezier曲线的轨迹表示方式在优化求解阶段会存在问题。

技术实现思路

1、本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种无人车自主跟随方法和装置，实现了在环卫场景下无人车的自主跟随能力，并且能够很好地进行碰撞避免和目标跟随。

2、本发明的目的可以通过以下技术方案来实现：

3、一种无人车自主跟随方法，包括以下步骤：

4、获取无人车所在区域的局部地图，通过无人车的传感器获取三维点云信息，将三维点云信息映射到局部地图中，构建2.5d的高程地图，该高程地图为二维栅格地图，该二维栅格地图内的各个栅格均包含对应栅格坐标处地形的高度值和方差；

5、在所述高程地图中，以无人车当前所在栅格区域为当前点，并设置目标点和障碍物集，采用包含无人车的动力学约束的全局路径规划算法，获取从当前点到目标点之间的全局路径；

6、基于微分平坦采用四阶b样条拟合所述全局路径，设定目标函数和约束，求解后获取最优的无人车跟随轨迹。

7、进一步地，所述2.5d的高程地图的构建过程包括：

8、将传感器获取的各个测量值转换为相应的高度值；

9、计算传感器测量的关于距离和角度的雅可比矩阵，并融合，获取各个测量点的方差信息，并与对应的高度值组合，获得各个测量点的观测值；

10、将获取的观测值和局部地图对应栅格的估计值进行融合，获取局部地图的高度均值和方差，从而构建2.5d的高程地图。

11、进一步地，所述高度值的转换表达式为：

12、

13、式中，p为高度值，p为投影矩阵，p＝[0,0,1]用于将在地图坐标系m下的三维向量mp降维到一维地形高度值，为传感器坐标系s转化至地图坐标系m的旋转矩阵的逆，为传感器获取的单个测量值，为地图m坐标系下sm的距离信息；

14、所述方差信息的计算表达式为：

15、

16、

17、

18、式中，为测量点的方差信息，js为传感器测量的雅可比矩阵，jφ为传感器测量的旋转矩阵，c(φ)用来描述对应旋转矩阵的映射，σs为传感器测量的关于距离的协方差矩阵，σφ为传感器测量的关于角度的协方差矩阵；

19、所述局部地图的高度均值和方差的计算表达式为：

20、

21、

22、式中，为局部地图的高度值，为局部地图的高度方差，为局部地图对应栅格的高度值估计值，为局部地图对应栅格的高度方差估计值。

23、进一步地，所述障碍物集中障碍物的获取过程包括：

24、记录无人车当前所在栅格区域的高度均值，将高程地图中所有高于无人车所在栅格区域的高度均值，且与邻近栅格高度相差超过设定值的栅格标视为障碍物。

25、进一步地，所述全局路径规划算法为基于hybrid a*的全局路径规划算法，包括以下步骤：

26、s201：设置优先级队列，用于存储等待扩展的节点n，每个节点ni中均包含从起始位置到节点ni的的代价f(n)；设置启发式函数h(n)用来引导全局路径规划算法，该启发式函数h(n)为节点n到目标点的欧氏距离；

27、s202：从优先级队列中获取代价最小的节点n，计算总代价g(n)；

28、s203：判断节点n是否为目标点，若是，则输出全局路径，否则执行步骤s204；

29、s204：选取节点n的邻居节点m，计算该节点n到邻居节点m的移动代价比较该移动节点的代价值g(m)和节点n的代价值g(n)加节点n到邻居节点m的移动代价

30、s205：若则令并将节点m放入所述优先级队列；否则返回步骤s204，重新选择节点n的邻居节点；

31、s206：判断节点n是否存在其它邻居节点，若是，则返回步骤s204，否则返回步骤s202。

32、进一步地，采用四阶b样条拟合所述全局路径的过程中，将时间域[t0,tf]分成n段等长时间，从而构建n段的四阶b样条曲线，对应n组优化变量作为目标函数，从而利用非线性规划器对所述目标函数和约束进行求解，获取最优的无人车跟随轨迹。

33、进一步地，第i段的四阶b样条曲线的表达式为：

34、

35、式中，τ为伪时间，τ∈[0,1]，[b1,4(τ)b2,4(τ)b3,4(τ)b4,4(τ)]为四阶b样条曲线的基函数，pi为空间中的第i个点。

36、进一步地，所述无人车的动力学模型的平坦输出为z＝[x,y]；

37、采用四阶b样条拟合所述全局路径后得到的轨迹参数化形式为：

38、对于节点序列tn

39、所述目标函数的表达式为：

40、

41、

42、所述约束包括状态约束、控制输入约束以及路径约束，所述约束的表达式为：

43、s.t.x(0)＝x0,y(0)＝y0

44、x(tf)＝xf,y(tf)＝yf

45、

46、

47、

48、

49、

50、式中，x(0)为初始时刻的无人车横坐标，y0为初始时刻的无人车纵坐标，全局路径的整体时间为[t0,tf]，xmin为无人车横坐标最小值，xmax为无人车横坐标最大值，ymin为无人车纵坐标最小值，ymax为无人车纵坐标最大值，为轨迹在时刻t下的曲率，ρmax为曲线转弯半径，为轨迹在时刻t下的速度，vmax为车速最大值，为车的加速度，amax为车的加速度最大值。

51、进一步地，所述方法用于环卫场景下的无人车自主跟随。

52、本发明还提供一种无人车自主跟随装置，包括存储器和处理器，所述存储器存储有计算机程序，处理器调用所述计算机程序执行如上所述的方法的步骤。

53、与现有技术相比，本发明具有以下优点：

54、(1)本发明基于高程地图的地图表示方式，将地图维度拓展到2维与3维之间，即2.5d地图，既能充分概括无人车所在任务场景下的环境，同时保证地图更新的实时性，减少了对内存以及计算能力的消耗；

55、基于hybrid a*的全局路径规划算法找到的全局轨迹考虑到了车辆的动力学约束，从而在具体局部路径优化的过程中缓解优化压力，避免了局部路径不可行的情况；

56、采用四阶b样条作为局部轨迹表示方式，得到的轨迹通过微分平坦映射到控制量输入到控制器中；基于四阶b样条的轨迹表示方式可以保证无人车轨迹的平滑性，同时b样条的凸包性质可以方便地通过优化控制点以实现避障和动力学约束。

57、(2)本发明在局部路径优化阶段中，通过将优化时间域n等分的方法，可以保证局部路径优化的求解问题可以得到数值解，能通过非线性规划器来进行求解，从而得到需要的最优小车轨迹。