基于多传感器信息融合的智能小车路径规划方法及系统

摘要

公开了一种基于多传感器信息融合的智能小车路径规划方法及系统。该方法可以包括：智能小车进入目标区域，根据红外传感器与测距传感器获得目标区域的初始地图与初始障碍物；根据初始地图与初始障碍物，确定智能小车的初始行进路线；设定安全距离，根据红外传感器获取红外信号，根据测距传感器获得距离信息；根据距离信息，计算行进角度；根据深度传感器，确定智能小车前方平面的图像，根据图像上每个像素点计算陷落点的位置；根据陷落点的位置、智能小车的行进路线、行进速度、制动距离、安全距离与行进角度，获取智能小车的路径规划策略。本发明通过多传感器数据融合计算行进角度，能够精细规划路线图，通过深度传感器，实现陷落点规避。

权利要求

1.一种基于多传感器信息融合的智能小车路径规划方法，其特征在于，所述方法包括：智能小车进入目标区域，根据红外传感器与测距传感器获得所述目标区域的初始地图与初始障碍物；

根据所述初始地图与所述初始障碍物，确定所述智能小车的初始行进路线；

设定安全距离，在所述智能小车的初始行进路线上，根据红外传感器，获取处于所述智能小车设定距离的范围内的红外信号，进而根据测距传感器，获得所述测距传感器与对应的前方障碍物或所述红外信号的距离信息；

根据所述距离信息，计算行进角度；

根据深度传感器，确定智能小车前方平面的图像，根据所述图像上每个像素点计算陷落点的位置；

根据所述陷落点的位置、所述智能小车的行进路线、行进速度、制动距离、安全距离与行进角度，获取所述智能小车的路径规划策略；

其中，通过公式(1)计算所述行进角度：

其中，x为左侧测距传感器与左侧障碍物或左侧红外信号的距离，y为右侧测距传感器与右侧障碍物或右侧红外信号的距离，r为前方测距传感器与前方障碍物或前方红外信号的距离，v为行进速度，σ为安全距离，θ为行进角度。

2.根据权利要求1所述的基于多传感器信息融合的智能小车路径规划方法，其中，所述安全距离为0.1-1m。

3.根据权利要求1所述的基于多传感器信息融合的智能小车路径规划方法，其中，还包括：当x大于y时，所述行进角度的方向为向左；当x小于y时，所述行进角度的方向为向右；

当x等于y时，所述智能小车随机选择所述行进角度的方向。

4.根据权利要求1所述的基于多传感器信息融合的智能小车路径规划方法，其中，还包括：设定距离阈值；

若x、y、r中任意一项小于所述距离阈值，则行进速度降为当前行进速度的20％；

若x、y、r中任意两项小于所述距离阈值，则行进速度降为当前行进速度的50％；

若x、y、r均小于所述距离阈值，则所述智能小车停止。

5.根据权利要求1所述的基于多传感器信息融合的智能小车路径规划方法，其中，所述根据所述图像上每个像素点计算陷落点的位置包括：建立深度传感器坐标系与智能小车坐标系；

在所述深度传感器坐标系中，获取每个像素点距离所述深度传感器的扫描区域中心的距离与方向，获得每个像素点在所述深度传感器坐标系中的位置；

根据每个像素点在所述深度传感器坐标系中的位置，通过坐标变换计算每个像素点在所述智能小车坐标系中的位置；

设定当前智能小车所在平面为基准面，将所在平面低于所述基准面的像素点标记为凹陷像素点；

根据所述凹陷像素点的位置，获取所述凹陷像素点在所述基准面的投射点，将距离所述智能小车最近的投射点标记为陷落点，获得所述陷落点的位置。

6.根据权利要求5所述的基于多传感器信息融合的智能小车路径规划方法，其中，所述智能小车的路径规划策略包括：当所述陷落点位于行进路线上，且与智能小车之间的距离小于所述安全距离与所述制动距离之和时，所述智能小车停止行进。

7.根据权利要求5所述的基于多传感器信息融合的智能小车路径规划方法，其中，所述智能小车的路径规划策略还包括：当所述陷落点位于行进路线上，且与智能小车之间的距离大于所述安全距离与所述制动距离之和时，智能小车减速并规划其他行进路径。

8.一种基于多传感器信息融合的智能小车路径规划系统，其特征在于，该系统包括：深度传感器，所述深度摄像头位于所述智能小车的前部，用于获取所述智能小车前方平面的图像；

红外传感器，设置于所述智能小车的侧面一周，用于获取处于所述智能小车设定距离的范围内的红外信号；

测距传感器，分别位于所述智能小车的前侧面、左侧面、右侧面，用于获得所述测距传感器与对应的前方障碍物或所述红外信号的距离信息；

处理器，所述处理器分别与所述深度传感器、所述红外传感器、所述测距传感器电性连接，执行以下步骤：智能小车进入目标区域，根据红外传感器与测距传感器获得所述目标区域的初始地图与初始障碍物；

根据所述初始地图与所述初始障碍物，确定所述智能小车的初始行进路线；

设定安全距离，在所述智能小车的初始行进路线上，根据红外传感器，获取处于所述智能小车设定距离的范围内的红外信号，进而根据测距传感器，获得所述测距传感器与对应的前方障碍物或所述红外信号的距离信息；

根据所述距离信息，计算行进角度；

根据深度传感器，确定智能小车前方平面的图像，根据所述图像上每个像素点计算陷落点的位置；

根据所述陷落点的位置、所述智能小车的行进路线、行进速度、制动距离、安全距离与行进角度，获取所述智能小车的路径规划策略；

其中，通过公式(1)计算所述行进角度：

其中，x为左侧测距传感器与左侧障碍物或左侧红外信号的距离，y为右侧测距传感器与右侧障碍物或右侧红外信号的距离，r为前方测距传感器与前方障碍物或前方红外信号的距离，v为行进速度，σ为安全距离，θ为行进角度。

说明书

基于多传感器信息融合的智能小车路径规划方法及系统

技术领域

[0001] 本发明涉及智能控制领域，更具体地，涉及一种基于多传感器信息融合的智能小车路径规划方法及系统。

背景技术

[0002] 近几年各类智能小车越来越多的进入到人们的生活中，那么，智能小车的路径规划是否合理、应用简便则是大家都关注的问题，目前多通过测距传感器进行防碰撞设计，以实现路径规划，转弯也多为直角转弯，这种简单的路径规划无法适应复杂地形。

[0003] 并且，大部分运用的智能小车采用轮式行驶结构，这类型智能小车一般工作在平面的环境中，只能承受跨越较小地面高度起伏，一般在1-2cm之间。较大的地面(>10cm)陷落会让智能小车失去重心而跌倒，更有甚者，如果遇到楼梯或者台阶，往往会让智能小车摔落损坏。针对这种问题当前市面上智能小车，一般在底盘下面安装了红外测距传感器，检测底盘移动方向下方地面是否虚悬，以此决定是否前进，这种设计需要智能小车在全速运行下能达到很短的刹车距离，一般要小于底盘半径。对于移动速度较快，重心较高的智能小车无法达到这种要求。因此，有必要开发一种适用于复杂地形的基于多传感器信息融合的智能小车路径规划方法及系统。

[0004] 公开于本发明背景技术部分的信息仅仅旨在加深对本发明的一般背景技术的理解，而不应当被视为承认或以任何形式暗示该信息构成已为本领域技术人员所公知的现有技术。

发明内容

[0005] 本发明提出了一种基于多传感器信息融合的智能小车路径规划方法及系统，其能够通过多传感器数据融合计算行进角度，能够精细规划路线图，通过深度传感器，实现陷落点规避，两者结合实现了精确高效的路径规划。

[0006] 根据本发明的一方面，提出了一种基于多传感器信息融合的智能小车路径规划方法。所述方法可以包括：智能小车进入目标区域，根据红外传感器与测距传感器获得所述目标区域的初始地图与初始障碍物；根据所述初始地图与所述初始障碍物，确定所述智能小车的初始行进路线；设定安全距离，在所述智能小车的初始行进路线上，根据红外传感器，获取处于所述智能小车设定距离的范围内的红外信号，进而根据测距传感器，获得所述测距传感器与对应的前方障碍物或所述红外信号的距离信息；根据所述距离信息，计算行进角度；根据深度传感器，确定智能小车前方平面的图像，根据所述图像上每个像素点计算陷落点的位置；根据所述陷落点的位置、所述智能小车的行进路线、行进速度、制动距离、安全距离与行进角度，获取所述智能小车的路径规划策略。

[0007] 在一个示例中，所述安全距离为0.1-1m。

[0008] 在一个示例中，通过公式(1)计算所述行进角度：

[0009]

[0010] 其中，x为左侧测距传感器与左侧障碍物或左侧红外信号的距离，y为右侧测距传感器与右侧障碍物或右侧红外信号的距离，r为前方测距传感器与前方障碍物或前方红外信号的距离，v为行进速度，σ为安全距离，θ为行进角度。

[0011] 在一个示例中，还包括：当x大于y时，所述行进角度的方向为向左；当x小于y时，所述行进角度的方向为向右；当x等于y时，所述智能小车随机选择所述行进角度的方向。

[0012] 在一个示例中，还包括：设定距离阈值；若x、y、r中任意一项小于所述距离阈值，则行进速度降为当前行进速度的20％；若x、y、r中任意两项小于所述距离阈值，则行进速度降为当前行进速度的50％；若x、y、r均小于所述距离阈值，则所述智能小车停止。

[0013] 在一个示例中，所述根据所述图像上每个像素点计算陷落点的位置包括：建立深度传感器坐标系与智能小车坐标系；在所述深度传感器坐标系中，获取每个像素点距离所述深度传感器的扫描区域中心的距离与方向，获得每个像素点在所述深度传感器坐标系中的位置；根据每个像素点在所述深度传感器坐标系中的位置，通过坐标变换计算每个像素点在所述智能小车坐标系中的位置；设定当前智能小车所在平面为基准面，将所在平面低于所述基准面的像素点标记为凹陷像素点；根据所述凹陷像素点的位置，获取所述凹陷像素点在所述基准面的投射点，将距离所述智能小车最近的投射点标记为陷落点，获得所述陷落点的位置。

[0014] 在一个示例中，所述智能小车的路径规划策略包括：当所述陷落点位于行进路线上，且与智能小车之间的距离小于所述安全距离与所述制动距离之和时，所述智能小车停止行进。

[0015] 在一个示例中，所述智能小车的路径规划策略还包括：当所述陷落点位于行进路线上，且与智能小车之间的距离大于所述安全距离与所述制动距离之和时，智能小车减速并规划其他行进路径。

[0016] 根据本发明的另一方面，提出了一种基于多传感器信息融合的智能小车路径规划系统，包括：深度传感器，所述深度摄像头位于所述智能小车的前部，用于获取所述智能小车前方平面的图像；红外传感器，设置于所述智能小车的侧面一周，用于获取处于所述智能小车设定距离的范围内的红外信号；测距传感器，分别位于所述智能小车的前侧面、左侧面、右侧面，用于获得所述测距传感器与对应的前方障碍物或所述红外信号的距离信息；处理器，所述处理器分别与所述深度传感器、所述红外传感器、所述测距传感器电性连接，执行以下步骤：智能小车进入目标区域，根据红外传感器与测距传感器获得所述目标区域的初始地图与初始障碍物；根据所述初始地图与所述初始障碍物，确定所述智能小车的初始行进路线；设定安全距离，在所述智能小车的初始行进路线上，根据红外传感器，获取处于所述智能小车设定距离的范围内的红外信号，进而根据测距传感器，获得所述测距传感器与对应的前方障碍物或所述红外信号的距离信息；根据所述距离信息，计算行进角度；根据深度传感器，确定智能小车前方平面的图像，根据所述图像上每个像素点计算陷落点的位置；根据所述陷落点的位置、所述智能小车的行进路线、行进速度、制动距离、安全距离与行进角度，获取所述智能小车的路径规划策略。

[0017] 在一个示例中，所述安全距离为0.1-1m。

[0018] 在一个示例中，通过公式(1)计算所述行进角度：

[0019]

[0020] 其中，x为左侧测距传感器与左侧障碍物或左侧红外信号的距离，y为右侧测距传感器与右侧障碍物或右侧红外信号的距离，r为前方测距传感器与前方障碍物或前方红外信号的距离，v为行进速度，σ为安全距离，θ为行进角度。

[0021] 在一个示例中，还包括：当x大于y时，所述行进角度的方向为向左；当x小于y时，所述行进角度的方向为向右；当x等于y时，所述智能小车随机选择所述行进角度的方向。

[0022] 在一个示例中，还包括：设定距离阈值；若x、y、r中任意一项小于所述距离阈值，则行进速度降为当前行进速度的20％；若x、y、r中任意两项小于所述距离阈值，则行进速度降为当前行进速度的50％；若x、y、r均小于所述距离阈值，则所述智能小车停止。

[0023] 在一个示例中，所述根据所述图像上每个像素点计算陷落点的位置包括：建立深度传感器坐标系与智能小车坐标系；在所述深度传感器坐标系中，获取每个像素点距离所述深度传感器的扫描区域中心的距离与方向，获得每个像素点在所述深度传感器坐标系中的位置；根据每个像素点在所述深度传感器坐标系中的位置，通过坐标变换计算每个像素点在所述智能小车坐标系中的位置；设定当前智能小车所在平面为基准面，将所在平面低于所述基准面的像素点标记为凹陷像素点；根据所述凹陷像素点的位置，获取所述凹陷像素点在所述基准面的投射点，将距离所述智能小车最近的投射点标记为陷落点，获得所述陷落点的位置。

[0024] 在一个示例中，所述智能小车的路径规划策略包括：当所述陷落点位于行进路线上，且与智能小车之间的距离小于所述安全距离与所述制动距离之和时，所述智能小车停止行进。

[0025] 在一个示例中，所述智能小车的路径规划策略还包括：当所述陷落点位于行进路线上，且与智能小车之间的距离大于所述安全距离与所述制动距离之和时，智能小车减速并规划其他行进路径。

[0026] 本发明具有其它的特性和优点，这些特性和优点从并入本文中的附图和随后的具体实施方式中将是显而易见的，或者将在并入本文中的附图和随后的具体实施方式中进行详细陈述，这些附图和具体实施方式共同用于解释本发明的特定原理。

附图说明

[0027] 通过结合附图对本发明示例性实施例进行更详细的描述，本发明的上述以及其它目的、特征和优势将变得更加明显，其中，在本发明示例性实施例中，相同的参考标号通常代表相同部件。

[0028] 图1示出了根据本发明的基于多传感器信息融合的智能小车路径规划方法的步骤的流程图。

具体实施方式

[0029] 下面将参照附图更详细地描述本发明。虽然附图中显示了本发明的优选实施例，然而应该理解，可以以各种形式实现本发明而不应被这里阐述的实施例所限制。相反，提供这些实施例是为了使本发明更加透彻和完整，并且能够将本发明的范围完整地传达给本领域的技术人员。

[0030] 图1示出了根据本发明的基于多传感器信息融合的智能小车路径规划方法的步骤的流程图。

[0031] 在该实施例中，根据本发明的基于多传感器信息融合的智能小车路径规划方法可以包括：步骤101，智能小车进入目标区域，根据红外传感器与测距传感器获得目标区域的初始地图与初始障碍物；步骤102，根据初始地图与初始障碍物，确定智能小车的初始行进路线；步骤103，设定安全距离，在智能小车的初始行进路线上，根据红外传感器，获取处于智能小车设定距离的范围内的红外信号，进而根据测距传感器，获得测距传感器与对应的前方障碍物或红外信号的距离信息；步骤104，根据距离信息，计算行进角度；步骤105，根据深度传感器，确定智能小车前方平面的图像，根据图像上每个像素点计算陷落点的位置；步骤106，根据陷落点的位置、智能小车的行进路线、行进速度、制动距离、安全距离与行进角度，获取智能小车的路径规划策略。

[0032] 在一个示例中，安全距离为0.1-1m。

[0033] 在一个示例中，通过公式(1)计算行进角度：

[0034]

[0035] 其中，x为左侧测距传感器与左侧障碍物或左侧红外信号的距离，y为右侧测距传感器与右侧障碍物或右侧红外信号的距离，r为前方测距传感器与前方障碍物或前方红外信号的距离，v为行进速度，σ为安全距离，θ为行进角度。

[0036] 在一个示例中，还包括：当x大于y时，行进角度的方向为向左；当x小于y时，行进角度的方向为向右；当x等于y时，智能小车随机选择行进角度的方向。

[0037] 在一个示例中，还包括：设定距离阈值；若x、y、r中任意一项小于距离阈值，则行进速度降为当前行进速度的20％；若x、y、r中任意两项小于距离阈值，则行进速度降为当前行进速度的50％；若x、y、r均小于距离阈值，则智能小车停止。

[0038] 在一个示例中，根据图像上每个像素点计算陷落点的位置包括：建立深度传感器坐标系与智能小车坐标系；在深度传感器坐标系中，获取每个像素点距离深度传感器的扫描区域中心的距离与方向，获得每个像素点在深度传感器坐标系中的位置；根据每个像素点在深度传感器坐标系中的位置，通过坐标变换计算每个像素点在智能小车坐标系中的位置；设定当前智能小车所在平面为基准面，将所在平面低于基准面的像素点标记为凹陷像素点；根据凹陷像素点的位置，获取凹陷像素点在基准面的投射点，将距离智能小车最近的投射点标记为陷落点，获得陷落点的位置。

[0039] 在一个示例中，智能小车的路径规划策略包括：当陷落点位于行进路线上，且与智能小车之间的距离小于安全距离与制动距离之和时，智能小车停止行进。

[0040] 在一个示例中，智能小车的路径规划策略还包括：当陷落点位于行进路线上，且与智能小车之间的距离大于安全距离与制动距离之和时，智能小车减速并规划其他行进路径。

[0041] 具体地，根据本发明的基于多传感器信息融合的智能小车路径规划方法可以包括：

[0042] 智能小车进入目标区域，根据红外传感器与测距传感器获得目标区域的初始地图与初始障碍物；根据初始地图与初始障碍物，确定智能小车的初始行进路线，在初始行进路线行进时，可以根据传感器的实时数据调整行进路线。

[0043] 设定安全距离为0.1-1m，在智能小车的初始行进路线上，根据红外传感器，获取处于智能小车设定距离的范围内的红外信号，进而根据测距传感器，获得测距传感器与对应的前方障碍物或红外信号的距离信息；根据距离信息，通过公式(1)计算行进角度。

[0044] 根据深度传感器，确定智能小车前方平面的图像，根据图像上每个像素点计算陷落点的位置，包括：建立深度传感器坐标系与智能小车坐标系；在深度传感器坐标系中，获取每个像素点距离深度传感器的扫描区域中心的距离与方向，获得每个像素点在深度传感器坐标系中的位置；根据每个像素点在深度传感器坐标系中的位置，通过坐标变换计算每个像素点在智能小车坐标系中的位置；设定当前智能小车所在平面为基准面，将所在平面低于基准面的像素点标记为凹陷像素点；根据凹陷像素点的位置，获取凹陷像素点在基准面的投射点，将距离智能小车最近的投射点标记为陷落点，获得陷落点的位置。

[0045] 根据陷落点的位置、智能小车的行进路线、行进速度、制动距离、安全距离与行进角度，获取智能小车的路径规划策略：

[0046] (1)根据公式(1)实时计算行进角度，当x大于y时，行进角度的方向为向左；当x小于y时，行进角度的方向为向右；当x等于y时，智能小车随机选择行进角度的方向。

[0047] (2)设定距离阈值；若x、y、r中任意一项小于距离阈值，则行进速度降为当前行进速度的20％；若x、y、r中任意两项小于距离阈值，则行进速度降为当前行进速度的50％；若x、y、r均小于距离阈值，则智能小车停止。

[0048] (3)当陷落点位于行进路线上，且与智能小车之间的距离小于安全距离与制动距离之和时，智能小车停止行进；陷落点位于行进路线上，且与智能小车之间的距离大于安全距离与制动距离之和时，智能小车减速并规划其他行进路径。

[0049] 本方法通过多传感器数据融合计算行进角度，能够精细规划路线图，通过深度传感器，实现陷落点规避，两者结合实现了精确高效的路径规划。

[0050] 应用示例

[0051] 为便于理解本发明实施例的方案及其效果，以下给出一个具体应用示例。本领域技术人员应理解，该示例仅为了便于理解本发明，其任何具体细节并非意在以任何方式限制本发明。

[0052] 根据本发明的基于多传感器信息融合的智能小车路径规划方法可以包括：

[0053] 智能小车进入目标区域，根据红外传感器与测距传感器获得目标区域的初始地图与初始障碍物；根据初始地图与初始障碍物，确定智能小车的初始行进路线，智能小车在初始行进路线行进后，可以根据传感器的实时数据调整行进路线。

[0054] 设定安全距离为0.1-1m，距离阈值为0.1m，在智能小车的初始行进路线上，根据红外传感器，获取处于智能小车设定距离的范围内的红外信号，进而根据测距传感器，获得测距传感器与对应的前方障碍物或红外信号的距离信息；根据距离信息，通过公式(1)计算行进角度为30°，由于此时x大于y，则向左30°行进，此时的x、y、r均大于距离阈值，则正常行进。

[0055] 根据深度传感器，确定智能小车前方平面的图像，根据图像上每个像素点计算陷落点的位置，包括：建立深度传感器坐标系与智能小车坐标系；在深度传感器坐标系中，获取每个像素点距离深度传感器的扫描区域中心的距离与方向，获得每个像素点在深度传感器坐标系中的位置；根据每个像素点在深度传感器坐标系中的位置，通过坐标变换计算每个像素点在智能小车坐标系中的位置；设定当前智能小车所在平面为基准面，将所在平面低于基准面的像素点标记为凹陷像素点；根据凹陷像素点的位置，获取凹陷像素点在基准面的投射点，将距离智能小车最近的投射点标记为陷落点，获得陷落点的位置不在行进路线上，则正常行进。

[0056] 综上所述，本发明通过多传感器数据融合计算行进角度，能够精细规划路线图，通过深度传感器，实现陷落点规避，两者结合实现了精确高效的路径规划。

[0057] 本领域技术人员应理解，上面对本发明的实施例的描述的目的仅为了示例性地说明本发明的实施例的有益效果，并不意在将本发明的实施例限制于所给出的任何示例。

[0058] 根据本发明的实施例，提供了一种基于多传感器信息融合的智能小车路径规划系统，其特征在于，该系统包括：深度传感器，深度摄像头位于智能小车的前部，用于获取智能小车前方平面的图像；红外传感器，设置于智能小车的侧面一周，用于获取处于智能小车设定距离的范围内的红外信号；测距传感器，分别位于智能小车的前侧面、左侧面、右侧面，用于获得测距传感器与对应的前方障碍物或红外信号的距离信息；处理器，处理器分别与深度传感器、红外传感器、测距传感器电性连接，执行以下步骤：智能小车进入目标区域，根据红外传感器与测距传感器获得目标区域的初始地图与初始障碍物；根据初始地图与初始障碍物，确定智能小车的初始行进路线；设定安全距离，在智能小车的初始行进路线上，根据红外传感器，获取处于智能小车设定距离的范围内的红外信号，进而根据测距传感器，获得测距传感器与对应的前方障碍物或红外信号的距离信息；根据距离信息，计算行进角度；根据深度传感器，确定智能小车前方平面的图像，根据图像上每个像素点计算陷落点的位置；根据陷落点的位置、智能小车的行进路线、行进速度、制动距离、安全距离与行进角度，获取智能小车的路径规划策略。

[0059] 在一个示例中，安全距离为0.1-1m。

[0060] 在一个示例中，通过公式(1)计算行进角度：

[0061]

[0062] 其中，x为左侧测距传感器与左侧障碍物或左侧红外信号的距离，y为右侧测距传感器与右侧障碍物或右侧红外信号的距离，r为前方测距传感器与前方障碍物或前方红外信号的距离，v为行进速度，σ为安全距离，θ为行进角度。

[0063] 在一个示例中，还包括：当x大于y时，行进角度的方向为向左；当x小于y时，行进角度的方向为向右；当x等于y时，智能小车随机选择行进角度的方向。

[0064] 在一个示例中，还包括：设定距离阈值；若x、y、r中任意一项小于距离阈值，则行进速度降为当前行进速度的20％；若x、y、r中任意两项小于距离阈值，则行进速度降为当前行进速度的50％；若x、y、r均小于距离阈值，则智能小车停止。

[0065] 在一个示例中，根据图像上每个像素点计算陷落点的位置包括：建立深度传感器坐标系与智能小车坐标系；在深度传感器坐标系中，获取每个像素点距离深度传感器的扫描区域中心的距离与方向，获得每个像素点在深度传感器坐标系中的位置；根据每个像素点在深度传感器坐标系中的位置，通过坐标变换计算每个像素点在智能小车坐标系中的位置；设定当前智能小车所在平面为基准面，将所在平面低于基准面的像素点标记为凹陷像素点；根据凹陷像素点的位置，获取凹陷像素点在基准面的投射点，将距离智能小车最近的投射点标记为陷落点，获得陷落点的位置。

[0066] 在一个示例中，智能小车的路径规划策略包括：当陷落点位于行进路线上，且与智能小车之间的距离小于安全距离与制动距离之和时，智能小车停止行进。

[0067] 在一个示例中，智能小车的路径规划策略还包括：当陷落点位于行进路线上，且与智能小车之间的距离大于安全距离与制动距离之和时，智能小车减速并规划其他行进路径。

[0068] 本系统通过多传感器数据融合计算行进角度，能够精细规划路线图，通过深度传感器，实现陷落点规避，两者结合实现了精确高效的路径规划。

[0069] 本领域技术人员应理解，上面对本发明的实施例的描述的目的仅为了示例性地说明本发明的实施例的有益效果，并不意在将本发明的实施例限制于所给出的任何示例。

[0070] 以上已经描述了本发明的各实施例，上述说明是示例性的，并非穷尽性的，并且也不限于所披露的各实施例。在不偏离所说明的各实施例的范围和精神的情况下，对于本技术领域的普通技术人员来说许多修改和变更都是显而易见的。