CN116678394A - 基于多传感器信息融合实时动态智能路径规划方法及系统 - Google Patents

基于多传感器信息融合实时动态智能路径规划方法及系统 Download PDF

Info

Publication number
CN116678394A
CN116678394A CN202310525420.0A CN202310525420A CN116678394A CN 116678394 A CN116678394 A CN 116678394A CN 202310525420 A CN202310525420 A CN 202310525420A CN 116678394 A CN116678394 A CN 116678394A
Authority
CN
China
Prior art keywords
path
navigation
cost
target
information
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202310525420.0A
Other languages
English (en)
Inventor
孙哲
黄哲
杨辉
孟雅婷
王凌风
安晓亚
金澄
徐鑫朋
陈景
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
CETC 32 Research Institute
Original Assignee
CETC 32 Research Institute
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by CETC 32 Research Institute filed Critical CETC 32 Research Institute
Priority to CN202310525420.0A priority Critical patent/CN116678394A/zh
Publication of CN116678394A publication Critical patent/CN116678394A/zh
Pending legal-status Critical Current

Links

Classifications

    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Navigation (AREA)

Abstract

本发明提供了一种基于多传感器信息融合实时动态智能路径规划方法及系统，包括：步骤S1：基于多传感器信息融合感知环境；步骤S2：优化全局可行路径搜索与轨迹；步骤S3：基于多传感器实时感知的情况，动态调整规划路径；步骤S4：根据规划路径进行实时导航和机动引导。本发明能够快速准确提取环境中的实时天气、地质等自然灾害和突发事件等动态实时信息，并且能够快速处理这些动态信息已获取地图中的未知静态障碍物以及未知动态障碍物。通过视觉、激光、惯性、毫米波雷达、卫星信号等多模态传感器获取到的环境中的动态信息之间的学习映射，实现全天候的未知障碍物间检测能力。

Description

基于多传感器信息融合实时动态智能路径规划方法及系统
技术领域
本发明涉及移动机器人路径规划领域，具体地，涉及一种基于多传感器信息融合实时动态智能路径规划方法及系统。
背景技术
定位导航技术从指南针、天文信息、卫星、惯性等单一的信源开始，随着惯性与卫星导航技术的成熟，这两种技术的松组合、紧组合等组合导航技术迅速发展。考虑到卫星导航的脆弱性以及惯性导航的漂移问题，其它传感器开始被引入到组合导航系统中，形成多源融合的导航系统。伴随着传感器技术的迅速发展，导航场景的日益复杂，导航平台的算力提高，能够实现快速场景切换的多种全源融合导航开始被研究与关注，实现信息的互补与增强，完成对于导航场景的更多维度、更加全面、更加深入的描述。
路径规划指的是在给定的环境中，提供和一条或者多条从起始点到终点的路径。现实环境中实时天气、地质等自然灾害和突发事件对路径规划的影响很大，并且这些因素具有实效性。但是目前存在的路径规划算法大都为静态算法，基于静态路网数据和其它环境数据进行路径规划，无法处理实时动态信息，导致算法最终得到的路径不能规避实时天气、地质等自然灾害和突发事件等因素产生的障碍物区域。目前也有部分算法考虑了动态实时信息对路径规划的影响，但是仍然存在不足，如未考虑如何获取动态信息；获取动态信息的方式单一，得到的动态信息不全面或者不充足；以及对动态信息处理速度慢未能满足实时性的需求。
目前已有的路径规划技术的不足主要包括如下几点：
(1)未考虑地图中的未知障碍物。目前大多数路径规划算法假定地图中的障碍物是静止的并且能够知道所有障碍物的位置，但是在实际环境中，受到实时天气、地质等自然灾害和突发事件的影响，地图中可能产生某些未知的障碍物(包括静态以及动态两类)，此时使用这些路径规划算法并不能规避这些位置的障碍物。另外许多路径规划算法虽然号称动态路径规划算法，但是仅能处理静态未知障碍物，缺少对动态障碍物的规避；还有一些路径规划算法虽能处理动态障碍物，但是处理速度慢，无法满足实时性的需求。
(2)获取动态信息的方式单一。大多数的动态路径规划算法获取动态信息只通过激光传感器或者相机等来获取实时信息，但在复杂环境中(如复杂越野环境)，二者难以捕捉环境中的所有动态信息。
(3)规划的路径不够平滑，无法满足车辆动力学的要求。
张振,张华良,邓永胜,白士宇.融合改进A～*算法与DWA算法的机器人实时路径规划[J].无线电工程,2022,52(11):1984-1993.公开了：对于利用传统A＊算法规划的路径存在搜索效率低下、节点冗余且不平滑及转弯易靠近障碍物等缺点,基于全向移动机器人提出了一种改进A＊算法与动态窗口法(Dynamic Window Approach,DWA)相融合的实时路径规划方法。在传统A＊算法的评价函数中加入环境中障碍物信息和父节点到目标点的代价信息,提高路径搜索效率；对当前节点扩展时进行安全检测,优化节点扩展方向；基于安全阈值提取路径关键点,优化搜索路径；将优化后的关键点作为DWA算法的临时目标点,将2种算法融合规划出一条基于全局最优的圆滑曲线路径。
但是，该技术方案只能够规避静态未知障碍物，没有给出当沿着全局路径搜索时遇到动态障碍物时的处理方法，因此无法有效规避动态未知障碍物；获取动态实时信息的方式单一，仅通过视觉传感器来提取信息；只提供距离优先的路径规划服务。
王翼虎,王思明.基于改进人工势场法的机器人实时路径规划[J].兰州交通大学学报,2020,39(03):60-66.公开了：针对传统人工势场法在机器人实时路径规划中存在目标不可达和局部极值点等问题，提出了一种改进的人工势场法.首先，在斥力函数中引入距离影响因子，解决目标点旁存在障碍物时的目标不可达问题；其次，引入动态法向力，消除由单个障碍物形成的局部极值点振荡或停滞；然后采用一种新的虚拟目标点设置方法，解决复杂槽形障碍形成的局部极值点问题；最后根据机器人受到的斥力，设置自适应调节步长。
但是，该技术方案规划得到的路径相较于真实最优路径仍具有一定的差距；在路径规划时默认已经得到了所有的未知障碍物的信息，缺少获取静态未知障碍物以及动态未知障碍物的提取方法；只提供距离优先的路径规划服务
胡睿婷,刁明光.面向室内实时路径规划的最短路径缓存算法[J].电子技术与软件工程,2019(22):137-139.公开了：通过分析室内实时路径规划中，以人的行走状态与习惯导致相邻两次路径规划结果在短时间内具有的相关性，分析了现有最短路径缓存查询方法在该情景下应用的缺陷，建立了路径相关性缓存代价模型，并提出了一种面向室内路网中实时路径规划的最短路径缓存构建的LPR算法，通过动态更新缓存结构，提高该情景下的最短路径缓存查询效率。
但是，该技术方案只能够解决室内或者广场等小范围内的路径搜索问题；无对动态未知障碍物的处理，无法规避动态未知障碍物，只能够规避静态障碍物；只提供距离优先的路径规划服务；在路径规划时默认已经得到了所有的未知障碍物的信息，缺少对动态实时信息的处理，无法通过本文所提算法获取静态未知障碍物以及动态未知障碍物的提取方法。
发明内容
针对现有技术中的缺陷，本发明的目的是提供一种基于多传感器信息融合实时动态智能路径规划方法及系统。
根据本发明提供的一种基于多传感器信息融合实时动态智能路径规划方法，包括：
步骤S1：基于多传感器信息融合感知环境；
步骤S2：优化全局能够通行路径搜索与轨迹；
步骤S3：基于多传感器实时感知的情况，动态调整规划路径；
步骤S4：根据规划路径进行实时导航和机动引导。
优选地，在所述步骤S1中：
基于深度学习环境感知方法，检测环境中的预设目标，根据导航环境特性，建立场景分类标准；分析多模态信号特性，研究基于多模态融合特征的场景空间及环境变量识别方法，对环境进行三维重构，实现导航环境下的环境信息智能感知。
步骤S1.1：基于多模态传感器数据的越野环境地形重构：
在预设环境下，车辆的导航构建局部地形的高程地形栅格图，为车辆在预设环境下进行路径规划提供感知基础；
系统基于视差图特征和稠密点云进行预设环境地形重构，通过视差方法提取平面，去除无需重构的地形部分，确定地形有效重构区域；融合惯性传感器和里程计信息，对高程栅格地形图进行修正，并进行实时地更新管理；通过高程栅格滤波方法对高程地形栅格进行滤波处理，获得局部高程栅格地形图；
步骤S1.2：基于多模态数据的目标检测与识别：
目标检测与识别获取环境中影响车辆通行的动态障碍物以及静态障碍物，采用基于师生网络的多模态融合目标障碍物检测算法；对于特征表示近似程度符合预设标准，具有预设成像能力或者点云生成能力的模态，采用师生网络的方式对数据进行目标级标注；
教师网络采用二维图像目标识别网络Mask RCNN得到目标识别结果；在特征提取部分，多模态数据的特征被整合为相同大小的二维张量，学生网络采用其他模态的数据特征作为输入，输入至同样结构的二维卷积网络中进行目标识别；跨模态监督部分根据教师网络的结果指导学生网络的训练，其中包括跨模态的目标识别结果对齐以及对较差检测结果的滤除；
步骤S1.3：多模态融合导航特征地图生成：
通过多传感器采集光、电、磁数据，对多模态特征进行编码和抽取之后形成认知快照，由多源传感器融合定位技术中给出的机器人自身位姿数据，将前一时刻的导航特征地图旋转到当前坐标系下，将认知快照与上一时刻的导航特征地图结合形成新时刻的导航特征地图；导航特征地图是分层级的，每个层级分别代表了不同模态数据所抽取的特征以及目标识别的结果。
优选地，在所述步骤S2中：
智能路径规划系统通过多传感器单元采集到的环境信息和系统中已存储的栅格地图，实现导航单元在环境地图定位，搜索出能够到达目标地点的路径；进行通行点规划与点间最优轨迹生成并进行线段轨迹间的转接，包括：
依据车辆导航任务要求，结合已构建的场景地图信息，通过基于图论算法求取出全局最快通行点；路径搜索在栅格地图中搜寻能够到达目标区域的无障碍连通区域，在所有连通区域内搜寻通行路径，计算寻优并生成通行点，对通行点连线并做平滑处理，提供满足不同的路径需求的服务，分别设置不同的代价函数满足要求；
步骤S2.1：基于多模态导航特征地图的位置识别
基于多模态感知地图的自适应位置识别方法将采集的多模态传感器数据按照多模态特征的提取方法进行处理，将生成的多模态特征与事先生成的多模态导航特征地图进行匹配，输出最近邻的定位点位位置信息，完成全天候、全时域位置识别的要求；基于多模态导航特征地图的位置识别包括两部分：多模态特征融合和位置识别；
步骤S2.2：全局通行轨迹点计算：
基于图论进行双向启发式搜索，以起点到目标点的最短代价路径为目标，搜索路径时对每一个搜索位置进行评估，获取最优通行点，从所得通行点进行搜索直到目标位置为止；输入基于离线或在线多传感器信息所构建的栅格地图信息及执行任务的评估结果，输出离散的导航单元参考轨迹点；
步骤S2.3：不同路径规划需求服务：
设置不同的代价函数：
步骤S2.3.1：速度优先：
设置节点之间的代价函数为距离、节点间的速度，即相邻节点所用时间；相邻节点代价函数为
其中Cost(i，j)为节点i到节点j所用时间，Dij为节点i与节点j之间的距离，Vij为节点i到节点j之间的平均速度；
步骤S2.3.2：距离优先：
设置节点之间的代价函数为距离；
步骤S2.3.3：道路优先
将符合条件的路段进行代价缩小设置；设置节点之间的代价函数为距离，相邻节点代价函数为：
若ij路段非优先考虑路段，代价函数：
Cost(i，j)＝Dij
若ij路段优先考虑路段，代价函数：
Cost(i，j)＝coef*Dij
其中，coef为区间[0，1)内的值；
步骤S2.3.4：油耗优先：
设置节点之间的代价函数为两个节点之间的耗油量评估值，算法根据耗油量规划出一条耗油最少的路线：
Cost(i，j)＝Fij
其中Fij表示ij节点之间的耗油量。
步骤S2.3.5：安全优先：
构建如下代价函数：
该代价函数用来计算起始栅格到目标栅格的代价值，sclear(n)为终点栅格维诺圆的面积，能够从维诺路网中保存的栅格离最近障碍物的距离值计算出来；σuneven(n)为终点栅格的维诺不平度值，能够从维诺不平度地图中查询得到；fn为安全优先的代价值，α为路径可行宽度系数，β为路径平稳性系数；
栅格通过代价函数计算出的代价值将与该栅格的能够通行的区域的面积成反比，而与该栅格的不平度值成正比，栅格的能够通行的区域面积越大，其代价值越小；而不平度值越大，其代价值越大；安全优先表示路线中保持与障碍物的距离，设置罚函数，使得当靠近障碍物时的代价大；此时设置节点之间的代价函数为距离，距离障碍物近的区域增加罚函数代价，相邻节点代价函数为
Cost(i，j)＝Dij+Wij
其中，Wij为罚函数，与危险位置有关，设置为二元高斯函数。
优选地，在所述步骤S3中：
基于人工势场以及Morphin路径优化方法，综合实现规划与局部路径优化，人工势场法在禁止驶入区域表面构造一个虚拟的斥力场，并在目标位置构造虚拟的引力场，引导车辆向目标位置运动并避开禁止驶入区域；Morphin路径优化方法在车辆在前进方向上生成能够通行的离散通行点路径集合，对每条路径进行性能评估，确定一条性能最佳且安全性符合预设标准的路径作为目标通行的参考轨迹，性能评估以每条路径的动力学性能以及安全通行的可行性进行计算得到该条路径的可通行性。
优选地，在所述步骤S4中：
基于规划路线，利用位置信息进行实时导航引导，导航过程实时显示导航相关信息，包括：当前道路信息、当前行驶速度、剩余时间、距离目的地距离；提供实时路况、天气、气象、危险区域、突发事件动态信息的报警功能；实现导航信息智能语音提示功能，导航过程中进行导航信息的实时语音提示，包括：高速出口提示、转弯提示、转弯距离；具有偏离规划路线的重算功能，导航过程中当前位置偏离规划路线一定距离，系统自动提示并进行路线的重新规划。
根据本发明提供的一种基于多传感器信息融合实时动态智能路径规划系统，包括：
模块M1：基于多传感器信息融合感知环境；
模块M2：优化全局能够通行路径搜索与轨迹；
模块M3：基于多传感器实时感知的情况，动态调整规划路径；
模块M4：根据规划路径进行实时导航和机动引导。
优选地，在所述模块M1中：
基于深度学习环境感知方法，检测环境中的预设目标，根据导航环境特性，建立场景分类标准；分析多模态信号特性，研究基于多模态融合特征的场景空间及环境变量识别方法，对环境进行三维重构，实现导航环境下的环境信息智能感知。
模块M1.1：基于多模态传感器数据的越野环境地形重构：
在预设环境下，车辆的导航构建局部地形的高程地形栅格图，为车辆在预设环境下进行路径规划提供感知基础；
系统基于视差图特征和稠密点云进行预设环境地形重构，通过视差方法提取平面，去除无需重构的地形部分，确定地形有效重构区域；融合惯性传感器和里程计信息，对高程栅格地形图进行修正，并进行实时地更新管理；通过高程栅格滤波方法对高程地形栅格进行滤波处理，获得局部高程栅格地形图；
模块M1.2：基于多模态数据的目标检测与识别：
目标检测与识别获取环境中影响车辆通行的动态障碍物以及静态障碍物，采用基于师生网络的多模态融合目标障碍物检测算法；对于特征表示近似程度符合预设标准，具有预设成像能力或者点云生成能力的模态，采用师生网络的方式对数据进行目标级标注；
教师网络采用二维图像目标识别网络Mask RCNN得到目标识别结果；在特征提取部分，多模态数据的特征被整合为相同大小的二维张量，学生网络采用其他模态的数据特征作为输入，输入至同样结构的二维卷积网络中进行目标识别；跨模态监督部分根据教师网络的结果指导学生网络的训练，其中包括跨模态的目标识别结果对齐以及对较差检测结果的滤除；
模块M1.3：多模态融合导航特征地图生成：
通过多传感器采集光、电、磁数据，对多模态特征进行编码和抽取之后形成认知快照，由多源传感器融合定位技术中给出的机器人自身位姿数据，将前一时刻的导航特征地图旋转到当前坐标系下，将认知快照与上一时刻的导航特征地图结合形成新时刻的导航特征地图；导航特征地图是分层级的，每个层级分别代表了不同模态数据所抽取的特征以及目标识别的结果。
优选地，在所述模块M2中：
智能路径规划系统通过多传感器单元采集到的环境信息和系统中已存储的栅格地图，实现导航单元在环境地图定位，搜索出能够到达目标地点的路径；进行通行点规划与点间最优轨迹生成并进行线段轨迹间的转接，包括：
依据车辆导航任务要求，结合已构建的场景地图信息，通过基于图论算法求取出全局最快通行点；路径搜索在栅格地图中搜寻能够到达目标区域的无障碍连通区域，在所有连通区域内搜寻通行路径，计算寻优并生成通行点，对通行点连线并做平滑处理，提供满足不同的路径需求的服务，分别设置不同的代价函数满足要求；
模块M2.1：基于多模态导航特征地图的位置识别
基于多模态感知地图的自适应位置识别方法将采集的多模态传感器数据按照多模态特征的提取方法进行处理，将生成的多模态特征与事先生成的多模态导航特征地图进行匹配，输出最近邻的定位点位位置信息，完成全天候、全时域位置识别的要求；基于多模态导航特征地图的位置识别包括两部分：多模态特征融合和位置识别；
模块M2.2：全局通行轨迹点计算：
基于图论进行双向启发式搜索，以起点到目标点的最短代价路径为目标，搜索路径时对每一个搜索位置进行评估，获取最优通行点，从所得通行点进行搜索直到目标位置为止；输入基于离线或在线多传感器信息所构建的栅格地图信息及执行任务的评估结果，输出离散的导航单元参考轨迹点；
模块M2.3：不同路径规划需求服务：
设置不同的代价函数：
模块M2.3.1：速度优先：
设置节点之间的代价函数为距离、节点间的速度，即相邻节点所用时间；相邻节点代价函数为
其中cost(i，j)为节点i到节点j所用时间，Dij为节点i与节点j之间的距离，Vij为节点i到节点j之间的平均速度；
模块M2.3.2：距离优先：
设置节点之间的代价函数为距离；
模块M2.3.3：道路优先
将符合条件的路段进行代价缩小设置；设置节点之间的代价函数为距离，相邻节点代价函数为：
若ij路段非优先考虑路段，代价函数：
Cost(i，j)＝Dij
若ij路段优先考虑路段，代价函数：
Cost(i，j)＝coef/Dij
其中，coef为区间[0，1)内的值；
模块M2.3.4：油耗优先：
设置节点之间的代价函数为两个节点之间的耗油量评估值，算法根据耗油量规划出一条耗油最少的路线：
Cost(i，j)＝Fij
其中Fij表示ij节点之间的耗油量。
模块M2.3.5：安全优先：
构建如下代价函数：
该代价函数用来计算起始栅格到目标栅格的代价值，sclear(n)为终点栅格维诺圆的面积，能够从维诺路网中保存的栅格离最近障碍物的距离值计算出来；σuneven(n)为终点栅格的维诺不平度值，能够从维诺不平度地图中查询得到；fn为安全优先的代价值，α为路径可行宽度系数，β为路径平稳性系数；
栅格通过代价函数计算出的代价值将与该栅格的能够通行的区域的面积成反比，而与该栅格的不平度值成正比，栅格的能够通行的区域面积越大，其代价值越小；而不平度值越大，其代价值越大；安全优先表示路线中保持与障碍物的距离，设置罚函数，使得当靠近障碍物时的代价大；此时设置节点之间的代价函数为距离，距离障碍物近的区域增加罚函数代价，相邻节点代价函数为
Cost(i，j)＝Dij+Wij
其中，Wij为罚函数，与危险位置有关，设置为二元高斯函数。
优选地，在所述模块M3中：
基于人工势场以及Morphin路径优化方法，综合实现规划与局部路径优化，人工势场法在禁止驶入区域表面构造一个虚拟的斥力场，并在目标位置构造虚拟的引力场，引导车辆向目标位置运动并避开禁止驶入区域；Morphin路径优化方法在车辆在前进方向上生成能够通行的离散通行点路径集合，对每条路径进行性能评估，确定一条性能最佳且安全性符合预设标准的路径作为目标通行的参考轨迹，性能评估以每条路径的动力学性能以及安全通行的可行性进行计算得到该条路径的可通行性。
优选地，在所述模块M4中：
基于规划路线，利用位置信息进行实时导航引导，导航过程实时显示导航相关信息，包括：当前道路信息、当前行驶速度、剩余时间、距离目的地距离；提供实时路况、天气、气象、危险区域、突发事件动态信息的报警功能；实现导航信息智能语音提示功能，导航过程中进行导航信息的实时语音提示，包括：高速出口提示、转弯提示、转弯距离；具有偏离规划路线的重算功能，导航过程中当前位置偏离规划路线一定距离，系统自动提示并进行路线的重新规划。
与现有技术相比，本发明具有如下的有益效果：
1、本发明解决了无法处理地图中的未知障碍物问题；将地图中的由于实时天气、地质等自然灾害和突发事件等因素动态未知障碍物以及静态未知障碍物纳入考虑范围，最终规划的路径能够规避所有的静态已知障碍物以及所有的未知障碍物；
2、本发明解决了动态信息来源单一问题；使用相机、激光雷达、毫米波雷达、惯性测量单元、卫星传感器等多模态传感器来获取环境中的动态信息。通过多模态信息间的学习映射，实现全天候的目标检测能力，快速获取环境中的未知障碍物信息；
3、本发明解决了路径过于曲折，不够平滑问题；通过算法消除路径中曲率过大的部分，生成满足车辆行驶条件的平滑路径；
4、本发明能够快速准确提取环境中的实时天气、地质等自然灾害和突发事件等动态实时信息，并且能够快速处理这些动态信息已获取地图中的未知静态障碍物以及未知动态障碍物。通过视觉、激光、惯性、毫米波雷达、卫星信号等多模态传感器获取到的环境中的动态信息之间的学习映射，实现全天候的未知障碍物间检测能力；
5、本发明提出的路径规划算法可以在动态环境中进行实时规划路径调整，并且路径满足车辆行驶要求。通过多传感器单元采集到的环境信息和系统中已存储的栅格地图，实现导航单元在环境地图中的快速定位，并搜索出可到达目标地点的可行路径；在此基础上进行通行点规划与点间最优轨迹快速生成并进行小线段轨迹间的平滑转接，以保证符合导航单元通行过程中的动力学性能；
6、本发明提供满足多种导航需求的路径规划服务。包括速度优先、距离优先、道路优先、安全优先、油耗优先等多种模式；
7、本发明能够实现对车辆的实时导航和机动引导。导航过程实时显示当前道路信息、当前行驶速度、剩余时间、距离目的地距离等导航相关信息；提供实时路况、天气、气象、危险区域、突发事件等动态信息的报警功能；实现导航过程中导航信息的实时语音提示；具有偏离规划路线的重算功能，导航过程中当前位置偏离规划路线一定距离，系统自动提示并进行路线的重新规划。
附图说明
通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：
图1为基本体元实现越野环境地形重构示意图
图2为目标识别师生网络结构示意图；
图3为基于多模态导航特征地图的位置识别示意图；
图4为二元高斯惩罚函数示意图；
图5为多传感器融合与最优轨迹动态生成示意图；
图6为基于多传感器实时感知的动态局部路径规划示意图。
具体实施方式
下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。
实施例1：
根据本发明提供的一种基于多传感器信息融合实时动态智能路径规划方法，如图1-图6所示，包括：
步骤S1：基于多传感器信息融合感知环境；
具体地，在所述步骤S1中：
基于深度学习环境感知方法，检测环境中的预设目标，根据导航环境特性，建立场景分类标准；分析多模态信号特性，研究基于多模态融合特征的场景空间及环境变量识别方法，对环境进行三维重构，实现导航环境下的环境信息智能感知。
步骤S1.1：基于多模态传感器数据的越野环境地形重构：
在预设环境下，车辆的导航构建局部地形的高程地形栅格图，为车辆在预设环境下进行路径规划提供感知基础；
系统基于视差图特征和稠密点云进行预设环境地形重构，通过视差方法提取平面，去除无需重构的地形部分，确定地形有效重构区域；融合惯性传感器和里程计信息，对高程栅格地形图进行修正，并进行实时地更新管理；通过高程栅格滤波方法对高程地形栅格进行滤波处理，获得局部高程栅格地形图；
步骤S1.2：基于多模态数据的目标检测与识别：
目标检测与识别获取环境中影响车辆通行的动态障碍物以及静态障碍物，采用基于师生网络的多模态融合目标障碍物检测算法；对于特征表示近似程度符合预设标准，具有预设成像能力或者点云生成能力的模态，采用师生网络的方式对数据进行目标级标注；
教师网络采用二维图像目标识别网络Mask RCNN得到目标识别结果；在特征提取部分，多模态数据的特征被整合为相同大小的二维张量，学生网络采用其他模态的数据特征作为输入，输入至同样结构的二维卷积网络中进行目标识别；跨模态监督部分根据教师网络的结果指导学生网络的训练，其中包括跨模态的目标识别结果对齐以及对较差检测结果的滤除；
步骤S1.3：多模态融合导航特征地图生成：
通过多传感器采集光、电、磁数据，对多模态特征进行编码和抽取之后形成认知快照，由多源传感器融合定位技术中给出的机器人自身位姿数据，将前一时刻的导航特征地图旋转到当前坐标系下，将认知快照与上一时刻的导航特征地图结合形成新时刻的导航特征地图；导航特征地图是分层级的，每个层级分别代表了不同模态数据所抽取的特征以及目标识别的结果。
步骤S2：优化全局能够通行路径搜索与轨迹；
具体地，在所述步骤S2中：
智能路径规划系统通过多传感器单元采集到的环境信息和系统中已存储的栅格地图，实现导航单元在环境地图定位，搜索出能够到达目标地点的路径；进行通行点规划与点间最优轨迹生成并进行线段轨迹间的转接，包括：
依据车辆导航任务要求，结合已构建的场景地图信息，通过基于图论算法求取出全局最快通行点；路径搜索在栅格地图中搜寻能够到达目标区域的无障碍连通区域，在所有连通区域内搜寻通行路径，计算寻优并生成通行点，对通行点连线并做平滑处理，提供满足不同的路径需求的服务，分别设置不同的代价函数满足要求；
步骤S2.1：基于多模态导航特征地图的位置识别
基于多模态感知地图的自适应位置识别方法将采集的多模态传感器数据按照多模态特征的提取方法进行处理，将生成的多模态特征与事先生成的多模态导航特征地图进行匹配，输出最近邻的定位点位位置信息，完成全天候、全时域位置识别的要求；基于多模态导航特征地图的位置识别包括两部分：多模态特征融合和位置识别；
步骤S2.2：全局通行轨迹点计算：
基于图论进行双向启发式搜索，以起点到目标点的最短代价路径为目标，搜索路径时对每一个搜索位置进行评估，获取最优通行点，从所得通行点进行搜索直到目标位置为止；输入基于离线或在线多传感器信息所构建的栅格地图信息及执行任务的评估结果，输出离散的导航单元参考轨迹点；
步骤S2.3：不同路径规划需求服务：
设置不同的代价函数：
步骤S2.3.1：速度优先：
设置节点之间的代价函数为距离、节点间的速度，即相邻节点所用时间；相邻节点代价函数为
其中Cost(i，j)为节点i到节点j所用时间，Dij为节点i与节点j之间的距离，Vij为节点i到节点j之间的平均速度；
步骤S2.3.2：距离优先：
设置节点之间的代价函数为距离；
步骤S2.3.3：道路优先
将符合条件的路段进行代价缩小设置；设置节点之间的代价函数为距离，相邻节点代价函数为：
若ij路段非优先考虑路段，代价函数：
Cost(i，j)＝Dij
若ij路段优先考虑路段，代价函数：
Cost(i，j)＝coef*Dij
其中，coef为区间[0，1)内的值；
步骤S2.3.4：油耗优先：
设置节点之间的代价函数为两个节点之间的耗油量评估值，算法根据耗油量规划出一条耗油最少的路线：
Cost(i，j)＝Fij
其中Fij表示ij节点之间的耗油量。
步骤S2.3.5：安全优先：
构建如下代价函数：
该代价函数用来计算起始栅格到目标栅格的代价值，sclear(n)为终点栅格维诺圆的面积，能够从维诺路网中保存的栅格离最近障碍物的距离值计算出来；σuneven(n)为终点栅格的维诺不平度值，能够从维诺不平度地图中查询得到；fn为安全优先的代价值，α为路径可行宽度系数，β为路径平稳性系数；
栅格通过代价函数计算出的代价值将与该栅格的能够通行的区域的面积成反比，而与该栅格的不平度值成正比，栅格的能够通行的区域面积越大，其代价值越小；而不平度值越大，其代价值越大；安全优先表示路线中保持与障碍物的距离，设置罚函数，使得当靠近障碍物时的代价大；此时设置节点之间的代价函数为距离，距离障碍物近的区域增加罚函数代价，相邻节点代价函数为
Cost(i，j)＝Dij+Wij
其中，Wij为罚函数，与危险位置有关，设置为二元高斯函数。
步骤S3：基于多传感器实时感知的情况，动态调整规划路径；
具体地，在所述步骤S3中：
基于人工势场以及Morphin路径优化方法，综合实现规划与局部路径优化，人工势场法在禁止驶入区域表面构造一个虚拟的斥力场，并在目标位置构造虚拟的引力场，引导车辆向目标位置运动并避开禁止驶入区域；Morphin路径优化方法在车辆在前进方向上生成能够通行的离散通行点路径集合，对每条路径进行性能评估，确定一条性能最佳且安全性符合预设标准的路径作为目标通行的参考轨迹，性能评估以每条路径的动力学性能以及安全通行的可行性进行计算得到该条路径的可通行性。
步骤S4：根据规划路径进行实时导航和机动引导。
具体地，在所述步骤S4中：
基于规划路线，利用位置信息进行实时导航引导，导航过程实时显示导航相关信息，包括：当前道路信息、当前行驶速度、剩余时间、距离目的地距离；提供实时路况、天气、气象、危险区域、突发事件动态信息的报警功能；实现导航信息智能语音提示功能，导航过程中进行导航信息的实时语音提示，包括：高速出口提示、转弯提示、转弯距离；具有偏离规划路线的重算功能，导航过程中当前位置偏离规划路线一定距离，系统自动提示并进行路线的重新规划。
实施例2：
实施例2为实施例1的优选例，以更为具体地对本发明进行说明。
本发明还提供一种基于多传感器信息融合实时动态智能路径规划系统，所述基于多传感器信息融合实时动态智能路径规划系统可以通过执行所述基于多传感器信息融合实时动态智能路径规划方法的流程步骤予以实现，即本领域技术人员可以将所述基于多传感器信息融合实时动态智能路径规划方法理解为所述基于多传感器信息融合实时动态智能路径规划系统的优选实施方式。
根据本发明提供的一种基于多传感器信息融合实时动态智能路径规划系统，包括：
模块M1：基于多传感器信息融合感知环境；
具体地，在所述模块M1中：
基于深度学习环境感知方法，检测环境中的预设目标，根据导航环境特性，建立场景分类标准；分析多模态信号特性，研究基于多模态融合特征的场景空间及环境变量识别方法，对环境进行三维重构，实现导航环境下的环境信息智能感知。
模块M1.1：基于多模态传感器数据的越野环境地形重构：
在预设环境下，车辆的导航构建局部地形的高程地形栅格图，为车辆在预设环境下进行路径规划提供感知基础；
系统基于视差图特征和稠密点云进行预设环境地形重构，通过视差方法提取平面，去除无需重构的地形部分，确定地形有效重构区域；融合惯性传感器和里程计信息，对高程栅格地形图进行修正，并进行实时地更新管理；通过高程栅格滤波方法对高程地形栅格进行滤波处理，获得局部高程栅格地形图；
模块M1.2：基于多模态数据的目标检测与识别：
目标检测与识别获取环境中影响车辆通行的动态障碍物以及静态障碍物，采用基于师生网络的多模态融合目标障碍物检测算法；对于特征表示近似程度符合预设标准，具有预设成像能力或者点云生成能力的模态，采用师生网络的方式对数据进行目标级标注；
教师网络采用二维图像目标识别网络Mask RCNN得到目标识别结果；在特征提取部分，多模态数据的特征被整合为相同大小的二维张量，学生网络采用其他模态的数据特征作为输入，输入至同样结构的二维卷积网络中进行目标识别；跨模态监督部分根据教师网络的结果指导学生网络的训练，其中包括跨模态的目标识别结果对齐以及对较差检测结果的滤除；
模块M1.3：多模态融合导航特征地图生成：
通过多传感器采集光、电、磁数据，对多模态特征进行编码和抽取之后形成认知快照，由多源传感器融合定位技术中给出的机器人自身位姿数据，将前一时刻的导航特征地图旋转到当前坐标系下，将认知快照与上一时刻的导航特征地图结合形成新时刻的导航特征地图；导航特征地图是分层级的，每个层级分别代表了不同模态数据所抽取的特征以及目标识别的结果。
模块M2：优化全局能够通行路径搜索与轨迹；
具体地，在所述模块M2中：
智能路径规划系统通过多传感器单元采集到的环境信息和系统中已存储的栅格地图，实现导航单元在环境地图定位，搜索出能够到达目标地点的路径；进行通行点规划与点间最优轨迹生成并进行线段轨迹间的转接，包括：
依据车辆导航任务要求，结合已构建的场景地图信息，通过基于图论算法求取出全局最快通行点；路径搜索在栅格地图中搜寻能够到达目标区域的无障碍连通区域，在所有连通区域内搜寻通行路径，计算寻优并生成通行点，对通行点连线并做平滑处理，提供满足不同的路径需求的服务，分别设置不同的代价函数满足要求；
模块M2.1：基于多模态导航特征地图的位置识别
基于多模态感知地图的自适应位置识别方法将采集的多模态传感器数据按照多模态特征的提取方法进行处理，将生成的多模态特征与事先生成的多模态导航特征地图进行匹配，输出最近邻的定位点位位置信息，完成全天候、全时域位置识别的要求；基于多模态导航特征地图的位置识别包括两部分：多模态特征融合和位置识别；
模块M2.2：全局通行轨迹点计算：
基于图论进行双向启发式搜索，以起点到目标点的最短代价路径为目标，搜索路径时对每一个搜索位置进行评估，获取最优通行点，从所得通行点进行搜索直到目标位置为止；输入基于离线或在线多传感器信息所构建的栅格地图信息及执行任务的评估结果，输出离散的导航单元参考轨迹点；
模块M2.3：不同路径规划需求服务：
设置不同的代价函数：
模块M2.3.1：速度优先：
设置节点之间的代价函数为距离、节点间的速度，即相邻节点所用时间；相邻节点代价函数为
其中Cost(i，j)为节点i到节点j所用时间，Dij为节点i与节点j之间的距离，Vij为节点i到节点j之间的平均速度；
模块M2.3.2：距离优先：
设置节点之间的代价函数为距离；
模块M2.3.3：道路优先
将符合条件的路段进行代价缩小设置；设置节点之间的代价函数为距离，相邻节点代价函数为：
若ij路段非优先考虑路段，代价函数：
Cost(i，j)＝Dij
若ij路段优先考虑路段，代价函数：
Cost(i，j)＝coef*Dij
其中，coef为区间[0，1)内的值；
模块M2.3.4：油耗优先：
设置节点之间的代价函数为两个节点之间的耗油量评估值，算法根据耗油量规划出一条耗油最少的路线：
Cost(i，j)＝Fij
其中Fij表示ij节点之间的耗油量。
模块M2.3.5：安全优先：
构建如下代价函数：
该代价函数用来计算起始栅格到目标栅格的代价值，sclear(n)为终点栅格维诺圆的面积，能够从维诺路网中保存的栅格离最近障碍物的距离值计算出来；σuneven(n)为终点栅格的维诺不平度值，能够从维诺不平度地图中查询得到；fn为安全优先的代价值，α为路径可行宽度系数，β为路径平稳性系数；
栅格通过代价函数计算出的代价值将与该栅格的能够通行的区域的面积成反比，而与该栅格的不平度值成正比，栅格的能够通行的区域面积越大，其代价值越小；而不平度值越大，其代价值越大；安全优先表示路线中保持与障碍物的距离，设置罚函数，使得当靠近障碍物时的代价大；此时设置节点之间的代价函数为距离，距离障碍物近的区域增加罚函数代价，相邻节点代价函数为
Cost(i，j)＝Dij+Wij
其中，Wij为罚函数，与危险位置有关，设置为二元高斯函数。
模块M3：基于多传感器实时感知的情况，动态调整规划路径；
具体地，在所述模块M3中：
基于人工势场以及Morphin路径优化方法，综合实现规划与局部路径优化，人工势场法在禁止驶入区域表面构造一个虚拟的斥力场，并在目标位置构造虚拟的引力场，引导车辆向目标位置运动并避开禁止驶入区域；Morphin路径优化方法在车辆在前进方向上生成能够通行的离散通行点路径集合，对每条路径进行性能评估，确定一条性能最佳且安全性符合预设标准的路径作为目标通行的参考轨迹，性能评估以每条路径的动力学性能以及安全通行的可行性进行计算得到该条路径的可通行性。
模块M4：根据规划路径进行实时导航和机动引导。
具体地，在所述模块M4中：
基于规划路线，利用位置信息进行实时导航引导，导航过程实时显示导航相关信息，包括：当前道路信息、当前行驶速度、剩余时间、距离目的地距离；提供实时路况、天气、气象、危险区域、突发事件动态信息的报警功能；实现导航信息智能语音提示功能，导航过程中进行导航信息的实时语音提示，包括：高速出口提示、转弯提示、转弯距离；具有偏离规划路线的重算功能，导航过程中当前位置偏离规划路线一定距离，系统自动提示并进行路线的重新规划。
实施例3：
实施例3为实施例1的优选例，以更为具体地对本发明进行说明。
本发明属于一种应用于信息系统软件。本方法包括如下步骤：
步骤1：基于多传感器信息融合的复杂环境智能感知
面向复杂环境，针对导航装备对复杂环境态势感知的需求，研究利用视觉、激光、惯性、毫米波雷达、卫星信号等多模态传感器数据的信息互补与深度融合，基于深度学习等环境感知方法，检测环境中的重要目标。研究复杂导航环境特性，建立场景分类标准；分析多模态信号特性，研究基于多模态融合特征的场景空间及环境变量识别方法，对环境进行快速三维重构，实现复杂导航环境下的环境信息智能感知。
步骤2：全局可行路径搜索与轨迹优化
智能路径规划系统通过多传感器单元采集到的环境信息和系统中已存储的栅格地图，实现导航单元在环境地图中的快速定位，并搜索出可到达目标地点的可行路径；在此基础上进行通行点规划与点间最优轨迹快速生成并进行小线段轨迹间的平滑转接，以保证符合导航单元通行过程中的动力学性能。具体包括：
依据车辆导航任务要求，结合已构建的场景地图信息，通过基于图论算法来快速求取出全局最快通行点。路径搜索在栅格地图中搜寻可达目标区域的无障碍连通区域，然后在所有连通区域内搜寻快速通行路径，计算寻优并生成一系列的通行点，最后对通行点连线并做平滑处理，使车辆能够沿参考路径平稳行进，减少能耗和通行时间。同时提供满足不同的路径需求的服务，比如速度优先、距离优先、道路优先、安全优先、油耗优先等情况，分别了设置一些不同的代价函数来满足要求。
步骤3：基于多传感器实时感知的动态局部路径规划调整
基于人工势场以及Morphin路径优化等方法，综合实现有效的规划与局部路径优化。其中，人工势场法作为一种基础的规划策略，在不可驶入区域“表面”构造一个虚拟的斥力场，并在目标位置构造虚拟的引力场。从而引导车辆向目标位置运动并合理避开不可驶入区域。Morphin路径优化方法则首先在车辆在前进方向上尝试生成一组可行的离散通行点路径集合，然后对每条路径进行性能评估，以确定一条性能最佳且安全的路径作为目标通行的参考轨迹。性能评估主要以每条路径的动力学性能以及安全通行的可信性来进行计算得到该条路径的可通行性。
步骤4：实时导航和机动引导
基于规划路线，利用位置信息进行实时导航引导。导航过程实时显示导航相关信息，包括：当前道路信息、当前行驶速度、剩余时间、距离目的地距离等；提供实时路况、天气、气象、危险区域、突发事件等动态信息的报警功能；实现导航信息智能语音提示功能，导航过程中进行导航信息的实时语音提示，包括：高速出口提示、转弯提示、转弯距离等(行驶过程中距离路口、转换点、危险区域等的距离2千米、1千米、500米、100米应有语音提示)；具有偏离规划路线的重算功能，导航过程中当前位置偏离规划路线一定距离，系统自动提示并进行路线的重新规划。
其中，基于多传感器信息融合的复杂环境智能感知：
(1)基于多模态传感器数据的越野环境地形重构
结构化环境下道路特征十分明显，而非结构化环境下，没有明确的道路定义，道路和非道路界限比较模糊。越野地形环境复杂粗糙，不仅存在岩石、斜坡、树木、礁石、坑洞等几何物质特点，同时也存在植被、水系和泥浆等非几何物质特点，导致越野环境的感知检测更加复杂。因此，在复杂越野环境下，车辆的导航不能简单靠抽取道路平面解决，而需要快速准确构建出局部地形的高程地形栅格图，为自动驾驶车辆在复杂地形环境下进行路径规划提供感知基础。
系统基于视差图特征和稠密点云进行复杂环境地形重构，通过视差方法提取近似平面，去除无需重构的地形部分，从而确定地形有效重构区域。接下来，融合惯性传感器(IMU)和里程计信息，对高程栅格地形图进行修正，并进行实时地更新管理。最后，通过一种高程栅格滤波方法对高程地形栅格进行滤波处理，获得最终的局部高程栅格地形图，如图1所示。系统通过选择性重构地形，去除了许多无需重构的部分，大大提高重构的效率，通过惯性传感器(IMU)和里程计融合点云信息和滤波处理，能有效还原越野地形环境，保障了驾驶车辆能实时地在复杂地形环境下安全行驶。
(2)基于多模态数据的目标检测与识别
目标检测与识别是为了获取环境中影响车辆通行的动态未知障碍物以及静态未知障碍物，主要采用基于师生网络的多模态融合目标障碍物检测算法。师生网络的整体结构如图2所示。对于特征表示近似，具有一定成像能力或者点云生成能力的模态，采用“师生网络”的方式对数据进行目标级标注。
所谓师生网络，即用一个已经训练好的神经网络去指导另一个网络的训练，通常教师与学生网络的输入为不同模态的数据，但其网络输出为相同形式的结果。师生网络主要分为三个部分：教师网络、学生网络、跨模态监督部分。本系统教师网络采用目前已经比较成熟的二维图像目标识别网络Mask RCNN，得到目标识别的结果。而学生网络采用其他模态的数据特征作为输入，在之前特征提取部分，多模态数据的特征已被整合为相同大小的二维张量，因此可以输入至同样结构的二维卷积网络中进行后续的目标识别。跨模态监督部分则是根据教师网络的结果来指导学生网络的训练，其中包括跨模态的目标识别结果对齐以及对较差检测结果的滤除。使用该方式的好处在于能够利用某一个模态数据高质量的目标识别结果对其他模态数据信息进行标注。
(3)多模态融合导航特征地图生成
多模态融合导航特征地图生成实际上是一个不断探索和感知环境，生成认知快照，持续投影叠加在经验地图中，形成覆盖一定区域的导航特征地图的过程。首先系统通过多传感器采集光、电、磁数据，之后对多模态特征进行编码和抽取之后形成认知快照，由多源传感器融合定位技术中给出的机器人自身位姿数据，用于将前一时刻的导航特征地图旋转到当前坐标系下，之后将认知快照与上一时刻的导航特征地图结合形成新时刻的导航特征地图。导航特征地图是分层级的，每个层级分别代表了不同模态数据所抽取的特征以及目标识别的结果，以支持更加鲁棒的位置识别与场景感知等服务。
其中，全局可行路径搜索与轨迹优化：
(1)基于多模态导航特征地图的位置识别
基于多模态感知地图的自适应位置识别方法如图3所示，将采集的多模态传感器数据按照之前的多模态特征的提取方法进行处理，将生成的多模态特征与事先生成的多模态导航特征地图进行匹配，从而输出最近邻的定位点位位置信息，完成全天候、全时域位置识别的要求。基于多模态导航特征地图的位置识别主要包括两部分：多模态特征融合和位置识别。
(2)全局通行轨迹点计算
基于图论进行双向启发式搜索(如Dijkstra算法以及A*算法)，以起点到目标点的最短代价路径为目标，搜索路径时对每一个搜索位置进行评估，以获取最优通行点，再从所得通行点进行搜索直到目标位置为止。输入基于离线或在线多传感器信息所构建的栅格地图信息及执行任务的评估结果，输出是离散的导航单元参考轨迹点。
(3)不同路径规划需求服务
同时为了满足不同的路径规划需求，如速度优先、距离优先、道路优先、安全优先、油耗优先等情况，分别设置不同的代价函数来满足要求。
A)速度优先：
速度优先表示到达时间越快越好，此时设置节点之间的代价函数为距离/节点间的速度，即相邻节点所用时间。相邻节点代价函数为
其中Cost(i，j)为节点i到节点j所用时间，Dij为节点i与节点j之间的距离，Vij为节点i到节点j之间的平均速度。
B)距离优先
距离优先表示起点和终点之间路程越短越好，该情形下设置节点之间的代价函数为距离。
C)道路优先
道路优先表示优先考虑某些比较好的路段进行通行。对此，我们可以将符合条件的优秀路段进行代价缩小设置。此时设置节点之间的代价函数仍然为距离。即相邻节点代价函数为：
若ij路段非优先考虑路段，代价函数：
Cost(i，j)＝Dij
若ij路段优先考虑路段，代价函数：
Cost(i，j)＝coef*Dij
其中，coef为区间[0，1)内的值。
D)油耗优先
油耗优先表示到达终点的耗油量越少越好。忽略其他因素可能造成的风险，直接设置节点之间的代价函数为两个节点之间的耗油量评估值，算法便会根据耗油量规划出一条耗油最少的路线。
Cost(i，j)＝Fij
其中Fij表示ij节点之间的耗油量。
E)安全优先
由前面所述，在越野环境的全局规划中，我们将注重车辆的安全性，使得车辆尽量跟随可行宽度较大而且路径较平稳的全局路径前进。这就要求全局规划算法要保证所得路径在两方面体现路径的安全性，一是路径可行宽度，一是路径平稳性。而由前面介绍可知，这两方面的要求主要由算法的代价函数来体现。因此，基于安全性的考虑，我们构建如下代价函数：
/>
该代价函数为用来计算起始栅格到目标栅格的代价值。式中包含两项，sclear(n)为终点栅格维诺圆的面积，它的值可以从维诺路网中保存的栅格离最近障碍物的距离值计算出来；σuneven(n)为终点栅格的维诺不平度值，该值可以直接从维诺不平度地图中查询得到。fn为安全优先的代价值，α为路径可行宽度系数，β为路径平稳性系数。
可以看到，一个栅格通过该代价函数计算出来的代价值将与该栅格的可行区域的面积成反比，而与该栅格的不平度值成正比，也即栅格的可行区域面积越大，其代价值越小；而不平度值越大，其代价值越大。由于算法在搜索过程中将优先搜索代价值小的栅格，也即可行区域面积大且不平度值更小的栅格，因此该代价函数能够帮助算法最终找到满足最优标准的高安全性路径。安全优先表示路线中尽量保持与障碍物的距离。此种情况下，直接设置罚函数，使得当靠近障碍物时的代价很大甚至无穷大，算法即会绕过危险区域；此时设置节点之间的代价函数仍然为距离，而距离障碍物近的区域增加罚函数代价。即相邻节点代价函数为
Cost(i，j)＝Dij+Wij
其中，Wij为罚函数，其与危险位置有关，可以设置为二元高斯函数(如图4所示)。
其中，基于多传感器实时感知的动态局部路径规划调整：
如图5图6所示，智能路径规划系统针对复杂越野环境，对局部导航过程中障碍物突然出现的情形进行分析，依据场景环境和实时采集到的感知数据在局部导航区域快速重新搜索可通行点，完成并实现路径规划，以保证可靠的避障能力。主要通过人工势场以及Morphin路径优化等方法进行实现。基于人工势场法在不可驶入区域“表面”构造一个虚拟的斥力场，并在目标位置构造虚拟的引力场。从而引导车辆向目标位置运动并合理避开不可驶入区域。基于Morphin路径优化方法在车辆在前进方向上尝试生成一组可行的离散通行点路径集合，然后对每条路径进行性能评估，以确定一条性能最佳且安全的路径作为目标通行的参考轨迹。性能评估主要以每条路径的动力学性能以及安全通行的可信性来进行计算得到该条路径的可通行性。
其中，实时导航和机动引导：
系统基于规划路线，基于位置信息提供对车辆的智能导航与机动引导。在车辆机动实时智能导航过程中，将会存在着各种复杂环境。其中有各种天气情况下的机动，也有不同地质条件下的机动，更有各种突发事件情况下的机动，面对这些不同机动场景，实时智能导航和引导需要在车辆机动过程中根据实时天气、地质等自然灾害和其它突发事件，发送警告数据信息和检测确认信息，同时进行实时的路径重规划。
具有通过定位信息进行规划路线的实时导航功能，导航过程实时显示导航相关信息，包括：当前道路信息、当前行驶速度、剩余时间、距离目的地距离等；
具有实时路况、天气、气象、危险区域、突发事件等动态信息的报警功能；
具有导航信息智能语音提示功能，导航过程中进行导航信息的实时语音提示，包括：高速出口提示、转弯提示、转弯距离等(行驶过程中距离路口、转换点、危险区域等的距离2千米、1千米、500米、100米应有语音提示)；
具有导航语音设置功能，提供语音提示开关设置、声音类别设置、声音大小；
具有偏离规划路线的重算功能，导航过程中当前位置偏离规划路线一定距离，系统自动提示并进行路线的重新规划。
本领域技术人员知道，除了以纯计算机可读程序代码方式实现本发明提供的系统、装置及其各个模块以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统、装置及其各个模块以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同程序。所以，本发明提供的系统、装置及其各个模块可以被认为是一种硬件部件，而对其内包括的用于实现各种程序的模块也可以视为硬件部件内的结构；也可以将用于实现各种功能的模块视为既可以是实现方法的软件程序又可以是硬件部件内的结构。
以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本申请的实施例和实施例中的特征可以任意相互组合。

Claims (10)

1.一种基于多传感器信息融合实时动态智能路径规划方法，其特征在于，包括：
步骤S1：基于多传感器信息融合感知环境；
步骤S2：优化全局能够通行路径搜索与轨迹；
步骤S3：基于多传感器实时感知的情况，动态调整规划路径；
步骤S4：根据规划路径进行实时导航和机动引导。
2.根据权利要求1所述的基于多传感器信息融合实时动态智能路径规划方法，其特征在于，在所述步骤S1中：
基于深度学习环境感知方法，检测环境中的预设目标，根据导航环境特性，建立场景分类标准；分析多模态信号特性，研究基于多模态融合特征的场景空间及环境变量识别方法，对环境进行三维重构，实现导航环境下的环境信息智能感知。
步骤S1.1：基于多模态传感器数据的越野环境地形重构：
在预设环境下，车辆的导航构建局部地形的高程地形栅格图，为车辆在预设环境下进行路径规划提供感知基础；
系统基于视差图特征和稠密点云进行预设环境地形重构，通过视差方法提取平面，去除无需重构的地形部分，确定地形有效重构区域；融合惯性传感器和里程计信息，对高程栅格地形图进行修正，并进行实时地更新管理；通过高程栅格滤波方法对高程地形栅格进行滤波处理，获得局部高程栅格地形图；
步骤S1.2：基于多模态数据的目标检测与识别：
目标检测与识别获取环境中影响车辆通行的动态障碍物以及静态障碍物，采用基于师生网络的多模态融合目标障碍物检测算法；对于特征表示近似程度符合预设标准，具有预设成像能力或者点云生成能力的模态，采用师生网络的方式对数据进行目标级标注；
教师网络采用二维图像目标识别网络Mask RCNN得到目标识别结果；在特征提取部分，多模态数据的特征被整合为相同大小的二维张量，学生网络采用其他模态的数据特征作为输入，输入至同样结构的二维卷积网络中进行目标识别；跨模态监督部分根据教师网络的结果指导学生网络的训练，其中包括跨模态的目标识别结果对齐以及对较差检测结果的滤除；
步骤S1.3：多模态融合导航特征地图生成：
通过多传感器采集光、电、磁数据，对多模态特征进行编码和抽取之后形成认知快照，由多源传感器融合定位技术中给出的机器人自身位姿数据，将前一时刻的导航特征地图旋转到当前坐标系下，将认知快照与上一时刻的导航特征地图结合形成新时刻的导航特征地图；导航特征地图是分层级的，每个层级分别代表了不同模态数据所抽取的特征以及目标识别的结果。
3.根据权利要求1所述的基于多传感器信息融合实时动态智能路径规划方法，其特征在于，在所述步骤S2中：
智能路径规划系统通过多传感器单元采集到的环境信息和系统中已存储的栅格地图，实现导航单元在环境地图定位，搜索出能够到达目标地点的路径；进行通行点规划与点间最优轨迹生成并进行线段轨迹间的转接，包括：
依据车辆导航任务要求，结合已构建的场景地图信息，通过基于图论算法求取出全局最快通行点；路径搜索在栅格地图中搜寻能够到达目标区域的无障碍连通区域，在所有连通区域内搜寻通行路径，计算寻优并生成通行点，对通行点连线并做平滑处理，提供满足不同的路径需求的服务，分别设置不同的代价函数满足要求；
步骤S2.1：基于多模态导航特征地图的位置识别
基于多模态感知地图的自适应位置识别方法将采集的多模态传感器数据按照多模态特征的提取方法进行处理，将生成的多模态特征与事先生成的多模态导航特征地图进行匹配，输出最近邻的定位点位位置信息，完成全天候、全时域位置识别的要求；基于多模态导航特征地图的位置识别包括两部分：多模态特征融合和位置识别；
步骤S2.2：全局通行轨迹点计算：
基于图论进行双向启发式搜索，以起点到目标点的最短代价路径为目标，搜索路径时对每一个搜索位置进行评估，获取最优通行点，从所得通行点进行搜索直到目标位置为止；输入基于离线或在线多传感器信息所构建的栅格地图信息及执行任务的评估结果，输出离散的导航单元参考轨迹点；
步骤S2.3：不同路径规划需求服务：
设置不同的代价函数：
步骤S2.3.1：速度优先：
设置节点之间的代价函数为距离、节点间的速度，即相邻节点所用时间；相邻节点代价函数为
其中Cost(i，j)为节点i到节点j所用时间，Dij为节点i与节点j之间的距离，Vij为节点i到节点j之间的平均速度；
步骤S2.3.2：距离优先：
设置节点之间的代价函数为距离；
步骤S2.3.3：道路优先
将符合条件的路段进行代价缩小设置；设置节点之间的代价函数为距离，相邻节点代价函数为：
若ij路段非优先考虑路段，代价函数：
Cost(i，j)＝Dij
若ij路段优先考虑路段，代价函数：
Cost(i，j)＝coef*Dij
其中，coef为区间[0，1)内的值；
步骤S2.3.4：油耗优先：
设置节点之间的代价函数为两个节点之间的耗油量评估值，算法根据耗油量规划出一条耗油最少的路线：
Cost(i，j)＝Fij
其中Fij表示ij节点之间的耗油量。
步骤S2.3.5：安全优先：
构建如下代价函数：
该代价函数用来计算起始栅格到目标栅格的代价值，sclear(n)为终点栅格维诺圆的面积，能够从维诺路网中保存的栅格离最近障碍物的距离值计算出来；σuneven(n)为终点栅格的维诺不平度值，能够从维诺不平度地图中查询得到；fn为安全优先的代价值，α为路径可行宽度系数，β为路径平稳性系数；
栅格通过代价函数计算出的代价值将与该栅格的能够通行的区域的面积成反比，而与该栅格的不平度值成正比，栅格的能够通行的区域面积越大，其代价值越小；而不平度值越大，其代价值越大；安全优先表示路线中保持与障碍物的距离，设置罚函数，使得当靠近障碍物时的代价大；此时设置节点之间的代价函数为距离，距离障碍物近的区域增加罚函数代价，相邻节点代价函数为
Cost(i，j)＝Dij+Wij
其中，Wij为罚函数，与危险位置有关，设置为二元高斯函数。
4.根据权利要求1所述的基于多传感器信息融合实时动态智能路径规划方法，其特征在于，在所述步骤S3中：
基于人工势场以及Morphin路径优化方法，综合实现规划与局部路径优化，人工势场法在禁止驶入区域表面构造一个虚拟的斥力场，并在目标位置构造虚拟的引力场，引导车辆向目标位置运动并避开禁止驶入区域；Morphin路径优化方法在车辆在前进方向上生成能够通行的离散通行点路径集合，对每条路径进行性能评估，确定一条性能最佳且安全性符合预设标准的路径作为目标通行的参考轨迹，性能评估以每条路径的动力学性能以及安全通行的可行性进行计算得到该条路径的可通行性。
5.根据权利要求1所述的基于多传感器信息融合实时动态智能路径规划方法，其特征在于，在所述步骤S4中：
基于规划路线，利用位置信息进行实时导航引导，导航过程实时显示导航相关信息，包括：当前道路信息、当前行驶速度、剩余时间、距离目的地距离；提供实时路况、天气、气象、危险区域、突发事件动态信息的报警功能；实现导航信息智能语音提示功能，导航过程中进行导航信息的实时语音提示，包括：高速出口提示、转弯提示、转弯距离；具有偏离规划路线的重算功能，导航过程中当前位置偏离规划路线一定距离，系统自动提示并进行路线的重新规划。
6.一种基于多传感器信息融合实时动态智能路径规划系统，其特征在于，包括：
模块M1：基于多传感器信息融合感知环境；
模块M2：优化全局能够通行路径搜索与轨迹；
模块M3：基于多传感器实时感知的情况，动态调整规划路径；
模块M4：根据规划路径进行实时导航和机动引导。
7.根据权利要求6所述的基于多传感器信息融合实时动态智能路径规划系统，其特征在于，在所述模块M1中：
基于深度学习环境感知方法，检测环境中的预设目标，根据导航环境特性，建立场景分类标准；分析多模态信号特性，研究基于多模态融合特征的场景空间及环境变量识别方法，对环境进行三维重构，实现导航环境下的环境信息智能感知。
模块M1.1：基于多模态传感器数据的越野环境地形重构：
在预设环境下，车辆的导航构建局部地形的高程地形栅格图，为车辆在预设环境下进行路径规划提供感知基础；
系统基于视差图特征和稠密点云进行预设环境地形重构，通过视差方法提取平面，去除无需重构的地形部分，确定地形有效重构区域；融合惯性传感器和里程计信息，对高程栅格地形图进行修正，并进行实时地更新管理；通过高程栅格滤波方法对高程地形栅格进行滤波处理，获得局部高程栅格地形图；
模块M1.2：基于多模态数据的目标检测与识别：
目标检测与识别获取环境中影响车辆通行的动态障碍物以及静态障碍物，采用基于师生网络的多模态融合目标障碍物检测算法；对于特征表示近似程度符合预设标准，具有预设成像能力或者点云生成能力的模态，采用师生网络的方式对数据进行目标级标注；
教师网络采用二维图像目标识别网络Mask RCNN得到目标识别结果；在特征提取部分，多模态数据的特征被整合为相同大小的二维张量，学生网络采用其他模态的数据特征作为输入，输入至同样结构的二维卷积网络中进行目标识别；跨模态监督部分根据教师网络的结果指导学生网络的训练，其中包括跨模态的目标识别结果对齐以及对较差检测结果的滤除；
模块M1.3：多模态融合导航特征地图生成：
通过多传感器采集光、电、磁数据，对多模态特征进行编码和抽取之后形成认知快照，由多源传感器融合定位技术中给出的机器人自身位姿数据，将前一时刻的导航特征地图旋转到当前坐标系下，将认知快照与上一时刻的导航特征地图结合形成新时刻的导航特征地图；导航特征地图是分层级的，每个层级分别代表了不同模态数据所抽取的特征以及目标识别的结果。
8.根据权利要求6所述的基于多传感器信息融合实时动态智能路径规划系统，其特征在于，在所述模块M2中：
智能路径规划系统通过多传感器单元采集到的环境信息和系统中已存储的栅格地图，实现导航单元在环境地图定位，搜索出能够到达目标地点的路径；进行通行点规划与点间最优轨迹生成并进行线段轨迹间的转接，包括：
依据车辆导航任务要求，结合已构建的场景地图信息，通过基于图论算法求取出全局最快通行点；路径搜索在栅格地图中搜寻能够到达目标区域的无障碍连通区域，在所有连通区域内搜寻通行路径，计算寻优并生成通行点，对通行点连线并做平滑处理，提供满足不同的路径需求的服务，分别设置不同的代价函数满足要求；
模块M2.1：基于多模态导航特征地图的位置识别
基于多模态感知地图的自适应位置识别方法将采集的多模态传感器数据按照多模态特征的提取方法进行处理，将生成的多模态特征与事先生成的多模态导航特征地图进行匹配，输出最近邻的定位点位位置信息，完成全天候、全时域位置识别的要求；基于多模态导航特征地图的位置识别包括两部分：多模态特征融合和位置识别；
模块M2.2：全局通行轨迹点计算：
基于图论进行双向启发式搜索，以起点到目标点的最短代价路径为目标，搜索路径时对每一个搜索位置进行评估，获取最优通行点，从所得通行点进行搜索直到目标位置为止；输入基于离线或在线多传感器信息所构建的栅格地图信息及执行任务的评估结果，输出离散的导航单元参考轨迹点；
模块M2.3：不同路径规划需求服务：
设置不同的代价函数：
模块M2.3.1：速度优先：
设置节点之间的代价函数为距离、节点间的速度，即相邻节点所用时间；相邻节点代价函数为
其中Cost(i，j)为节点i到节点j所用时间，Dij为节点i与节点j之间的距离，Vij为节点i到节点j之间的平均速度；
模块M2.3.2：距离优先：
设置节点之间的代价函数为距离；
模块M2.3.3：道路优先
将符合条件的路段进行代价缩小设置；设置节点之间的代价函数为距离，相邻节点代价函数为：
若ij路段非优先考虑路段，代价函数：
Cost(i，j)＝Dij
若ij路段优先考虑路段，代价函数：
Cost(i，j)＝coef*Dij
其中，coef为区间[0，1)内的值；
模块M2.3.4：油耗优先：
设置节点之间的代价函数为两个节点之间的耗油量评估值，算法根据耗油量规划出一条耗油最少的路线：
Cost(i，j)＝Fij
其中Fij表示ij节点之间的耗油量。
模块M2.3.5：安全优先：
构建如下代价函数：
该代价函数用来计算起始栅格到目标栅格的代价值，sclear(n)为终点栅格维诺圆的面积，能够从维诺路网中保存的栅格离最近障碍物的距离值计算出来；σuneven(n)为终点栅格的维诺不平度值，能够从维诺不平度地图中查询得到；fn为安全优先的代价值，α为路径可行宽度系数，β为路径平稳性系数；
栅格通过代价函数计算出的代价值将与该栅格的能够通行的区域的面积成反比，而与该栅格的不平度值成正比，栅格的能够通行的区域面积越大，其代价值越小；而不平度值越大，其代价值越大；安全优先表示路线中保持与障碍物的距离，设置罚函数，使得当靠近障碍物时的代价大；此时设置节点之间的代价函数为距离，距离障碍物近的区域增加罚函数代价，相邻节点代价函数为
Cost(i，j)＝Dij+Wij
其中，Wij为罚函数，与危险位置有关，设置为二元高斯函数。
9.根据权利要求6所述的基于多传感器信息融合实时动态智能路径规划系统，其特征在于，在所述模块M3中：
基于人工势场以及Morphin路径优化方法，综合实现规划与局部路径优化，人工势场法在禁止驶入区域表面构造一个虚拟的斥力场，并在目标位置构造虚拟的引力场，引导车辆向目标位置运动并避开禁止驶入区域；Morphin路径优化方法在车辆在前进方向上生成能够通行的离散通行点路径集合，对每条路径进行性能评估，确定一条性能最佳且安全性符合预设标准的路径作为目标通行的参考轨迹，性能评估以每条路径的动力学性能以及安全通行的可行性进行计算得到该条路径的可通行性。
10.根据权利要求6所述的基于多传感器信息融合实时动态智能路径规划系统，其特征在于，在所述模块M4中：
基于规划路线，利用位置信息进行实时导航引导，导航过程实时显示导航相关信息，包括：当前道路信息、当前行驶速度、剩余时间、距离目的地距离；提供实时路况、天气、气象、危险区域、突发事件动态信息的报警功能；实现导航信息智能语音提示功能，导航过程中进行导航信息的实时语音提示，包括：高速出口提示、转弯提示、转弯距离；具有偏离规划路线的重算功能，导航过程中当前位置偏离规划路线一定距离，系统自动提示并进行路线的重新规划。
CN202310525420.0A 2023-05-10 2023-05-10 基于多传感器信息融合实时动态智能路径规划方法及系统 Pending CN116678394A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310525420.0A CN116678394A (zh) 2023-05-10 2023-05-10 基于多传感器信息融合实时动态智能路径规划方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310525420.0A CN116678394A (zh) 2023-05-10 2023-05-10 基于多传感器信息融合实时动态智能路径规划方法及系统

Publications (1)

Publication Number Publication Date
CN116678394A true CN116678394A (zh) 2023-09-01

Family

ID=87780016

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310525420.0A Pending CN116678394A (zh) 2023-05-10 2023-05-10 基于多传感器信息融合实时动态智能路径规划方法及系统

Country Status (1)

Country Link
CN (1) CN116678394A (zh)

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117130263A (zh) * 2023-10-26 2023-11-28 博创联动科技股份有限公司 一种基于车联网大数据的整车智能控制方法及系统
CN117151578A (zh) * 2023-11-01 2023-12-01 江苏洛尧智慧通信科技有限公司 一种基于三维gis的辅助规划监管系统及方法
CN117369480A (zh) * 2023-12-05 2024-01-09 北京理工大学 一种室内复杂环境下轮腿机器人路径规划方法与系统
CN117475115A (zh) * 2023-11-11 2024-01-30 华中师范大学 虚实融合环境中路径引导系统及其工作方法
CN117521937A (zh) * 2024-01-08 2024-02-06 苏州观瑞汽车技术有限公司 适于多维尺度的协同感知环境的动态路径诱导方法及系统
CN117952498A (zh) * 2024-03-26 2024-04-30 云南电投绿能科技有限公司 一种光伏运输车的配置方法、装置、设备及存储介质

Cited By (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117130263A (zh) * 2023-10-26 2023-11-28 博创联动科技股份有限公司 一种基于车联网大数据的整车智能控制方法及系统
CN117130263B (zh) * 2023-10-26 2024-01-16 博创联动科技股份有限公司 一种基于车联网大数据的整车智能控制方法及系统
CN117151578A (zh) * 2023-11-01 2023-12-01 江苏洛尧智慧通信科技有限公司 一种基于三维gis的辅助规划监管系统及方法
CN117151578B (zh) * 2023-11-01 2024-01-26 江苏洛尧智慧通信科技有限公司 一种基于三维gis的辅助规划监管系统及方法
CN117475115A (zh) * 2023-11-11 2024-01-30 华中师范大学 虚实融合环境中路径引导系统及其工作方法
CN117369480A (zh) * 2023-12-05 2024-01-09 北京理工大学 一种室内复杂环境下轮腿机器人路径规划方法与系统
CN117369480B (zh) * 2023-12-05 2024-02-23 北京理工大学 一种室内复杂环境下轮腿机器人路径规划方法与系统
CN117521937A (zh) * 2024-01-08 2024-02-06 苏州观瑞汽车技术有限公司 适于多维尺度的协同感知环境的动态路径诱导方法及系统
CN117521937B (zh) * 2024-01-08 2024-05-07 苏州观瑞汽车技术有限公司 适于多维尺度的协同感知环境的动态路径诱导方法及系统
CN117952498A (zh) * 2024-03-26 2024-04-30 云南电投绿能科技有限公司 一种光伏运输车的配置方法、装置、设备及存储介质

Similar Documents

Publication Publication Date Title
CN116678394A (zh) 基于多传感器信息融合实时动态智能路径规划方法及系统
Badue et al. Self-driving cars: A survey
EP3526737B1 (en) Neural network system for autonomous vehicle control
JP7140849B2 (ja) 確率的オブジェクト追跡及び予測フレームワーク
Vivacqua et al. Self-localization based on visual lane marking maps: An accurate low-cost approach for autonomous driving
CN109641589B (zh) 用于自主车辆的路线规划
US20200209857A1 (en) Multimodal control system for self driving vehicle
Sotelo et al. Virtuous: Vision-based road transportation for unmanned operation on urban-like scenarios
Sivaraman et al. Dynamic probabilistic drivability maps for lane change and merge driver assistance
CN110531770B (zh) 一种基于改进的rrt路径规划方法和系统
CN113313154A (zh) 一体化融合多传感器自动驾驶智能感知装置
WO2019168679A1 (en) Automatic creation and updating of maps
CN110553660B (zh) 一种基于a*算法和人工势场的无人车轨迹规划方法
KR20160048530A (ko) 자율 이동 차량의 경로 생성 방법 및 경로 생성 장치
Tanzmeister et al. Grid-based multi-road-course estimation using motion planning
Qin et al. Deep imitation learning for autonomous navigation in dynamic pedestrian environments
Lee et al. Ods-bot: Mobile robot navigation for outdoor delivery services
CN114450735A (zh) 一种联合预测运动对象轨迹的系统和方法
Bayerl et al. Detection and tracking of rural crossroads combining vision and LiDAR measurements
Milz et al. Is artificial intelligence the solution to all our problems? Exploring the applications of AI for automated driving
CN116257058A (zh) 一种园区自动驾驶物流小车智能局部规划方法
Chipka et al. Estimation and navigation methods with limited information for autonomous urban driving
Raipuria et al. Road infrastructure indicators for trajectory prediction
US11878684B2 (en) System and method for trajectory prediction using a predicted endpoint conditioned network
US20220053124A1 (en) System and method for processing information from a rotatable camera

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination