一种路径规划方法及相关装置与流程

本技术涉及路径规划，尤其涉及一种路径规划方法及相关装置。

背景技术：

1、路径规划技术，是指行驶装置(例如机器人)根据自身传感器对行驶环境进行感知，从而自动规划一段从当前点到目标点的安全的运动路径。除了定位和建图，路径规划是任何自主导航系统的关键功能，主要解决三大问题：1、连通性，即路径必须能连接起点和终点，并保证全程安全避障；2、动力学和运动学可行性，即路径满足行驶装置自身运动的约束和动力学的约束，如行驶装置的速度不应超过其速度上限值；3、最优性，即在达成前两者的前提下，路径的长度、行驶时间以及行驶装置的能耗等应被尽可能优化。

2、由于地面上的行驶装置不能离地运动，目前的路径规划技术通常是将行驶装置视为一个质点，将路径规划的目标视为质点在二维空间上的避障通行。因此，目前的路径规划技术的应用场景通常被限制为相对简单、结构化的场景。

3、随着机器人功能形态的发展和应用场景的深入，目前的路径规划技术逐渐无法满足对复杂环境的丰富信息适配。具体来说，非结构化的环境挑战日益增强，而目前基于二维空间的路径规划技术不能完整地还原三维空间内的复杂拓扑结构，因此无法很好地处理针对于悬空类障碍物的碰撞检测。

4、基于此，目前亟需一种能够有效应对三维空间内复杂拓扑结构的路径规划方法。

技术实现思路

1、本技术提供了一种路径规划方法，能够在具有复杂拓扑结构的三维空间下实现路径规划，提高路径规划方法的场景适应能力。

2、本技术第一方面提供一种路径规划方法，该方法应用于行驶装置上。该方法具体包括：首先，获取行驶环境的点云数据，点云数据用于表征行驶环境中的地面及障碍物。其中，点云数据实际上是多个点的集合，每个点分别用于表示行驶环境中的地面或障碍物表面的坐标。因此，基于点云数据中的多个点，能够确定行驶环境中的地面以及障碍物表面的各个点的坐标。

3、然后，基于点云数据、行驶装置的越障高度以及行驶装置的高度，确定行驶环境中的多个区域的通行状态。其中，在通行状态为可通行的区域中，与地面接触的障碍物部分的高度不大于越障高度且可通行高度不小于行驶装置的高度，可通行高度为区域中供行驶装置通行的高度。

4、具体来说，影响行驶装置通行的因素主要是地面上的障碍物。地面上的障碍物通常又包括几种类型的障碍物。对于与地面接触且无悬空部分的障碍物而言，需要考虑行驶装置是否能够跨越该障碍物，以确定行驶装置是否能够在该障碍物所处的区域通行；对于与地面接触且具有悬空部分的障碍物而言，则需要考虑行驶装置是否能够跨越该障碍物，或者是否能够从该障碍物的悬空部分下钻行通过，以确定行驶装置是否能够在该障碍物所处的区域通行。

5、最后，基于多个区域的通行状态，确定第一路径，第一路径用于供行驶装置行驶，且第一路径所经过的区域均为多个区域中通行状态为可通行的区域。

6、本方案中，采用点云数据来表征行驶环境，并基于行驶装置跨越障碍的高度以及行驶装置本身的高度，确定行驶环境中能够供行驶装置通行的区域，即行驶装置能够跨越障碍物通行的区域或在悬空的障碍物下钻行通过的区域，进而在具有复杂拓扑结构的三维空间下实现路径规划，提高路径规划方法的场景适应能力。

7、在一种可能的实现方式中，行驶环境的多个区域包括第一区域，第一区域的可通行高度为第一区域中的第一障碍物部分与第二障碍物部分在高度方向之间的距离，第一障碍物部分为与地面接触的障碍物部分，第二障碍物部分为悬空的障碍物部分。

8、也就是说，在区域中同时存在障碍物的悬空部分以及障碍物与地面接触的部分时，区域的可通行高度即为障碍物的悬空部分的最低点与障碍物与地面接触部分的最高点之间的距离。

9、在一种可能的实现方式中，第一区域的可通行高度是根据第一点和第二点之间的高度差来确定的，第一点和第二点为点云数据在第一区域中高度值相邻且高度差最大的两个点。

10、也就是说，在基于点云数据获取到第一区域中的所有点之后，可以按照各个点的高度值来对所有点进行依次排序，并且计算高度值相邻的两个点之间高度差，进而确定所有相邻点之间的高度差。最后，选择高度差最大的两个相邻点(即第一点和第二点)之间的高度差作为第一区域的可通行高度。

11、本方案中，通过计算点云数据中高度值相邻且高度差最大的两个点之间的高度来确定区域的可通行高度，能够有效地在三维空间中确定区域内空间的形状，以便于后续判断该空间是否能够供行驶装置通行。

12、在一种可能的实现方式中，行驶装置为站立高度可变化的机器人。在通行状态为可通行的区域中，可通行高度不小于机器人的最小站立高度。

13、在行驶装置为站立高度可变化的机器人的情况下，机器人能够调整自身的站立高度，因此在通行状态为可通行的区域中，区域的可通行高度可以是不小于机器人的最小站立高度。也就是说，在区域的可通行高度不小于机器人的最小站立高度的情况下，机器人能够以最小站立高度钻行通过该区域。

14、本方案中，通过考虑机器人本身在高度方向上的高度调节能力，能够结合机器人本身的构造特性，更加灵活地实现机器人在三维空间中的避障，提高机器人的避障能力。

15、在一种可能的实现方式中，该方法还包括：基于目标惩罚函数，对第一路径进行修正，得到第二路径。其中，对第一路径进行修正的目标为降低目标惩罚函数的值，目标惩罚函数与目标距离具有负相关的关系，目标距离为行驶装置在行驶过程中与行驶环境中的障碍物的距离。也就是说，对于通过修正第一路径所得到的一条路径而言，当行驶装置在该路径的行驶过程中与行驶环境中的障碍物的距离越大，则目标惩罚函数越小；当行驶装置在该路径的行驶过程中与行驶环境中的障碍物的距离越小，则目标惩罚函数越大。

16、本方案中，通过设置目标惩罚函数，可以是以行驶装置与障碍物之间的距离作为路径优化的约束条件，在修正第一路径的过程中尽可能地使得行驶装置与障碍物之间保持一定的安全距离，从而提高最终所得到的路径的安全性，保证行驶装置在行驶过程中的安全性。

17、在一种可能的实现方式中，目标惩罚函数包括第一惩罚函数和第二惩罚函数；其中，第一惩罚函数与第一距离相关，第一距离为行驶装置在水平方向上与行驶环境中的障碍物的距离；第二惩罚函数与第二距离相关，第二距离为行驶装置在高度方向上与行驶环境中的障碍物的距离。

18、简单来说，第一距离可以理解为行驶装置在前后左右等方向上与障碍物的距离，第二距离则可以理解为行驶装置的顶部与悬空的障碍物的距离，以及行驶装置的底部与地面上障碍物的距离。

19、本方案中，通过同时考虑行驶装置在水平方向以及高度方向上与障碍物的距离来构建目标惩罚函数，能够保证行驶装置在各个方向上均与障碍物保持安全的距离，进而提高行驶装置在采用调整位姿避开障碍物、采用骑行跨越地面障碍物、钻行通过悬空障碍物等避障方式时的安全性，保证了修正得到的第二路径的安全性。

20、在一种可能的实现方式中，目标距离与行驶装置的表面轮廓形状相关。

21、也就是说，针对于具有不同轮廓形状的行驶装置，即便这些行驶装置具有相同的位姿，这些行驶装置与障碍物之间的距离也是不一样的。

22、具体而言，对于行驶装置在行驶过程中的任意一个障碍物而言，在求取行驶装置与该障碍物之间的距离时，具体可以先计算行驶装置的表面轮廓上的每个点与障碍物的距离，进而选出与障碍物距离最近的一个点所对应的距离来作为行驶装置与障碍物之间的距离。

23、本方案中，在确定行驶装置与障碍物的距离时，通过基于行驶装置的表面轮廓形状来计算其与障碍物之间的距离，能够有效地考虑到行驶装置本身的形状特性对其与障碍物之间的距离的影响，进而保证灵活的通过性。

24、在一种可能的实现方式中，目标惩罚函数包括第三惩罚函数，第三惩罚函数与行驶环境中的地面的坡度以及崎岖度相关，其中地面的坡度以及崎岖度是基于点云数据确定的。

25、具体而言，行驶环境中的地面的坡度和崎岖度越大，则代表地面的可通行度越低，越容易影响行驶装置的正常行驶，因此第三惩罚函数的值也越大；行驶环境中的地面的坡度和崎岖度越小，则代表地面的可通行度越大，行驶装置越容易进行平稳的行驶，因此第三惩罚函数的值也越小。

26、本方案中，通过增加与地面的坡度及崎岖度相关的第三惩罚函数，能够使得在修正第一路径时，尽量避开地面坡度及崎岖度较大的区域，从而保证行驶装置在行驶过程中的平稳性。

27、在一种可能的实现方式中，目标惩罚函数还包括第四惩罚函数、第五惩罚函数和第六惩罚函数。第四惩罚函数与行驶装置在行驶过程中的加速度变化情况相关；第五惩罚函数与行驶装置的行驶总时间相关；第六惩罚函数与第一差异值以及第二差异值相关，第一差异值为行驶装置在行驶过程中的速度与行驶装置的速度上限值之间的差异值，第二差异值为行驶装置在行驶过程中的加速度与行驶装置的加速度上限值之间的差异值。

28、在一种可能的实现方式中，基于点云数据、行驶装置的越障高度以及行驶装置的高度，确定行驶环境中的多个区域的通行状态，具体包括：基于点云数据，确定位于栅格地图的每个栅格中的点集合，其中栅格地图包括多个栅格，多个栅格中的每个栅格对应于行驶环境中的一个区域；基于每个栅格中的点集合、行驶装置的越障高度以及行驶装置的高度，确定栅格地图中的每个栅格的通行状态，以得到行驶环境中的多个区域的通行状态。

29、具体来说，栅格地图就是用一个个栅格所组成的网格来代表地图，栅格里可以存储不同的数值,代表这个栅格的不同含义。在确定每个栅格的通信状态之后，可以在可通行的栅格里存储数值1，代表该栅格的通信状态为可通行；以及，在不可通行的栅格里存储数值0，代表该栅格的通信状态为不可通行。这样一来，基于栅格地图中栅格所存储的值，能够便于后续快速确定可通行的栅格以及不可通行的栅格。

30、在一种可能的实现方式中，基于多个区域的通行状态，确定第一路径，包括：基于多个区域的通行状态，在多个区域中搜索连通起点和终点的多个目标区域，得到路径长度最小的第一路径；其中，第一路径经过多个目标区域，且多个目标区域的通行状态均为可通行。

31、在一种可能的实现方式中，行驶装置包括机器人或车辆。

32、在一种可能的实现方式中，该方法还包括：获取行驶装置在行驶过程中的位姿变化信息以及行驶装置所行驶的地面的点云数据；根据位姿变化信息确定地面的第一凹凸值；根据点云数据计算地面的第二凹凸值；基于第一凹凸值和第二凹凸值，确定地面的目标凹凸值，目标凹凸值用于指示地面的凹陷值或凸起值，地面的凹陷值或凸起值用于确定多个区域的通行状态。

33、本方案中，通过位姿变化信息和点云数据结合，能够准确地计算得到地面的凹凸情况，即地面上凸起的障碍物的高度以及地面上凹陷的坑的深度。这样，在计算各个区域的可通行状态时，可以基于行驶装置的越障高度以及地面的凹凸情况来判断行驶装置是否能够从地面凸起的障碍物上越过，或者是从地面上凹陷的坑行驶过，进而在各种恶劣环境下准确地确定每个区域的可通行状态。

34、在一种可能的实现方式中，该方法还包括：获取第一误差值以及第二误差值，第一误差值用于指示第一凹凸值的计算误差，第二误差值用于指示第二凹凸值的计算误差；基于第一凹凸值和第二凹凸值，确定地面的目标凹凸值，包括：通过对第一凹凸值和第二凹凸值进行加权求和，得到目标凹凸值；其中，第一凹凸值的权重系数和第二凹凸值的权重系数均与第一误差值以及第二误差值相关。

35、本方案中，通过基于凹凸值的误差值来执行两个凹凸值的融合，能够有效地根据凹凸值的误差情况来调整其在融合过程中所占的比例，从而提高地面凹凸值的融合结果的准确性。

36、本技术第二方面提供一种行驶装置，包括：获取模块，用于获取行驶环境的点云数据，点云数据用于表征行驶环境中的地面及障碍物；处理模块，用于基于点云数据、行驶装置的越障高度以及行驶装置的高度，确定行驶环境中的多个区域的通行状态，其中，在通行状态为可通行的区域中，与地面接触的障碍物部分的高度不大于越障高度且可通行高度不小于行驶装置的高度，可通行高度为区域中供行驶装置通行的高度；处理模块，还用于基于多个区域的通行状态，确定第一路径，第一路径用于供行驶装置行驶，且第一路径所经过的区域均为多个区域中通行状态为可通行的区域。

37、在一种可能的实现方式中，多个区域包括第一区域，第一区域的可通行高度为第一区域中的第一障碍物部分与第二障碍物部分在高度方向之间的距离，第一障碍物部分为与地面接触的障碍物部分，第二障碍物部分为悬空的障碍物部分。

38、在一种可能的实现方式中，第一区域的可通行高度是根据第一点和第二点之间的高度差来确定的，第一点和第二点为点云数据在第一区域中高度值相邻且高度差最大的两个点。

39、在一种可能的实现方式中，行驶装置为站立高度可变化的机器人；在通行状态为可通行的区域中，可通行高度不小于机器人的最小站立高度。

40、在一种可能的实现方式中，处理模块，还用于基于目标惩罚函数，对第一路径进行修正，得到第二路径；其中，对第一路径进行修正的目标为降低目标惩罚函数的值，目标惩罚函数与目标距离具有负相关的关系，目标距离为行驶装置在行驶过程中与行驶环境中的障碍物的距离。

41、在一种可能的实现方式中，目标惩罚函数包括第一惩罚函数和第二惩罚函数；其中，第一惩罚函数与第一距离相关，第一距离为行驶装置在水平方向上与行驶环境中的障碍物的距离；第二惩罚函数与第二距离相关，第二距离为行驶装置在高度方向上与行驶环境中的障碍物的距离。

42、在一种可能的实现方式中，目标距离与行驶装置的表面轮廓形状相关。

43、在一种可能的实现方式中，目标惩罚函数包括第三惩罚函数，第三惩罚函数与行驶环境中的地面的坡度以及崎岖度相关，其中地面的坡度以及崎岖度是基于点云数据确定的。

44、在一种可能的实现方式中，目标惩罚函数还包括第四惩罚函数、第五惩罚函数和第六惩罚函数；第四惩罚函数与行驶装置在行驶过程中的加速度变化情况相关；第五惩罚函数与行驶装置的行驶总时间相关；第六惩罚函数与第一差异值以及第二差异值相关，第一差异值为行驶装置在行驶过程中的速度与行驶装置的速度上限值之间的差异值，第二差异值为行驶装置在行驶过程中的加速度与行驶装置的加速度上限值之间的差异值。

45、在一种可能的实现方式中，处理模块，还用于：基于点云数据，确定位于栅格地图的每个栅格中的点集合，其中栅格地图包括多个栅格，多个栅格中的每个栅格对应于行驶环境中的一个区域；基于每个栅格中的点集合、行驶装置的越障高度以及行驶装置的高度，确定栅格地图中的每个栅格的通行状态，以得到行驶环境中的多个区域的通行状态。

46、在一种可能的实现方式中，处理模块，还用于：基于多个区域的通行状态，在多个区域中搜索连通起点和终点的多个目标区域，得到路径长度最小的第一路径；其中，第一路径经过多个目标区域，且多个目标区域的通行状态均为可通行。

47、在一种可能的实现方式中，行驶装置包括机器人或车辆。

48、在一种可能的实现方式中，获取模块，还用于获取行驶装置在行驶过程中的位姿变化信息以及行驶装置所行驶的地面的点云数据；处理模块，还用于：根据位姿变化信息确定地面的第一凹凸值；根据点云数据计算地面的第二凹凸值；基于第一凹凸值和第二凹凸值，确定地面的目标凹凸值，目标凹凸值用于指示地面的凹陷值或凸起值，地面的凹陷值或凸起值用于确定多个区域的通行状态。

49、在一种可能的实现方式中，处理模块，还用于：获取第一误差值以及第二误差值，第一误差值用于指示第一凹凸值的计算误差，第二误差值用于指示第二凹凸值的计算误差；通过对第一凹凸值和第二凹凸值进行加权求和，得到目标凹凸值；其中，第一凹凸值的权重系数和第二凹凸值的权重系数均与第一误差值以及第二误差值相关。

50、本技术第三方面提供一种行驶装置，可以包括处理器，处理器和存储器耦合，存储器存储有程序指令，当存储器存储的程序指令被处理器执行时实现上述第一方面或第一方面任一实现方式的方法。对于处理器执行第一方面的各个可能实现方式中的步骤，具体均可以参阅第一方面，此处不再赘述。

51、本技术第四方面提供了一种计算机可读存储介质，计算机可读存储介质中存储有计算机程序，当其在计算机上运行时，使得计算机执行上述第一方面或第一方面任一实现方式的方法。

52、本技术第五方面提供了一种电路系统，电路系统包括处理电路，处理电路配置为执行上述第一方面或第一方面任一实现方式的方法。

53、本技术第六方面提供了一种计算机程序产品，当其在计算机上运行时，使得计算机执行上述第一方面或第一方面任一实现方式的方法。

54、本技术第七方面提供了一种芯片系统，该芯片系统包括处理器，用于支持服务器或门限值获取装置实现上述第一方面或第一方面任一实现方式中所涉及的功能，例如，发送或处理上述方法中所涉及的数据和/或信息。在一种可能的设计中，芯片系统还包括存储器，存储器，用于保存服务器或通信设备必要的程序指令和数据。该芯片系统，可以由芯片构成，也可以包括芯片和其他分立器件。

55、上述第二方面至第七方面的有益效果可以参考上述第一方面的介绍，在此不再赘述。