[发明专利]定位与导航系统以及方法

权利要求书

1.一种定位与导航系统，其特征在于，所述定位与导航系统包括：

激光雷达模块，设置于机器人的顶部，用于扫描所述激光雷达模块所在高度平面内的环境区域，识别预设半径内的障碍物的位置信息，并根据所述障碍物的位置信息生成环境点云图像；

双目视觉相机模块，设置于所述机器人的第一侧壁，用于获取所述机器人行进前方的视觉点云图像；

惯性测量模块，设置于所述机器人的几何中心位置，用于采集所述机器人的加速度信息和角速度信息；

轮式里程测量模块，设置于所述机器人的车轮上，用于采集所述机器人的位移路径；

工控机，用于接收所述环境点云图像、所述视觉点云图像、所述加速度信息、所述角速度信息以及所述位移路径，并根据所述环境点云图像、所述视觉点云图像、所述加速度信息、所述角速度信息以及所述位移路径对所述机器人进行定位和导航；

定位模块，用于接收所述环境点云图像、所述视觉点云图像、所述加速度信息、所述角速度信息以及所述位移路径，并将所述环境点云图像、所述视觉点云图像、所述加速度信息、所述角速度信息以及所述位移路径进行融合，获得融合信息，并根据所述融合信息对所述机器人进行定位，获得定位信息；

导航模块，用于获取所述机器人的目的地以及场景地图，根据所述目的地、所述场景地图以及所述定位信息，对所述机器人进行导航；

所述定位模块包括：

第一分析单元，用于接收所述加速度信息和所述角速度信息，并根据所述加速度信息和所述角速度信息计算所述机器人的第一位姿信息；

第二分析单元，用于接收所述环境点云图像，并根据所述环境点云图像计算所述机器人的第二位姿信息；

第三分析单元，用于接收所述视觉点云图像，并根据所述视觉点云图像和所述第二位姿信息计算所述机器人的第三位姿信息；

融合单元，用于采用扩展卡尔曼滤波算法将所述第一位姿信息、所述第二位姿信息、所述第三位姿信息以及所述位移路径进行融合，得到所述融合信息；

定位单元，用于接收所述融合信息，并根据所述融合信息对所述机器人进行定位，生成所述定位信息；

融合单元具体包括三次融合过程，分别是：首先，将第一位姿信息与位移路径进行第一次融合，获得第一子融合信息，然后，再将第一子融合信息与第二位姿信息进行第二次融合，获得第二子融合信息，最后，再将第二子融合信息与第三位姿信息进行第三次融合，获得融合信息；

其中，将第一位姿信息与位移路径进行第一次融合具体为：基于加速度信息计算机器人的位移变化情况，所述位移变化情况包括惯性测量总位移；实时测量记录机器人车轮转动的里程数据；将所述惯性测量总位移和所述机器人车轮转动的里程数据时时进行平均拟合计算，实现所述第一次融合；

再将第一子融合信息与第二位姿信息进行第二次融合，具体为：基于第一次融合信息中获得的机器人位置和位姿信息确定第一次融合信息的理论障碍物距离信息；基于激光雷达模块获得的实际距离信息；将所述第一次融合信息的理论障碍物距离信息与所述实际距离信息进行比对，拟合出多个距离信息，利用建立好的地图信息进行再次估算机器人的位置和姿态信息，实现第二次信息融合优化，获得第二次融合信息；

再将第二子融合信息与第三位姿信息进行第三次融合，获得融合信息，具体为：基于双目视觉相机模块间歇性测量测距信息得到视觉里程计信息；将第二次融合的信息估计出来的机器人运动位移路径和位置、位姿信息和构建的所述视觉里程计信息在同一时间轴上进行间歇性拟合优化；将视觉里程计信息间歇性的与第二次融合信息进行融合，融合时将惯性测量模块、轮式里程测量模块和视觉里程计信息输入进扩展卡尔曼滤波中进行一次的迭代计算；其中，测量一次距离信息就进行一次三者的信息融合，优化估计的信息，同时在地图中更新估计的位置和位姿信息，以实现第三次融合。