一种基于多导航模块的定位方法及移动机器人与流程

本发明涉及移动机器人导航定位的技术领域，尤其涉及一种基于多导航模块的定位方法及移动机器人。

背景技术：

目前的移动机器人导航方式主要有卫星导航、激光导航和超声波导航等。单一导航系统都有各自的独特性能和局限性，由于机器人运行环境的不同，使用单一导航系统的机器人的运动在每一步运行中都存在系统误差，而基于多传感器信息的组合导航技术是对机器人自身携带的传感器所观察到的多种信息使用信息融合算法来综合消除单一导航系统存在的误差，估计环境地图和在此环境地图中机器人的移动轨迹。

在公开号为cn101576384的专利中描述了一种基于视觉信息和里程计信息对移动机器人进行定位的方法，该方法基于里程计数据预估机器人位置，再通过摄像头视觉信息对预估位置进行校正。该方法主要融合了视觉和里程计两种传感器数据，相对于单一导航系统稳定性和定位精度有所提高。但该方法仅在室内简单环境下有良好的定位效果，在较为复杂的环境下会产生较大的误差，且在室外强光环境下无法工作。

公开号为cn106017458a)的专利中描述了一种基于红外相机和惯导系统的组合导航方式，在设定的机器人移动路径的每一个控制点的位置上方设置位姿控制板，利用机器人上设置的红外照相机拍摄包含有位于机器人上方的位姿控制板的图像，并采用图像颜色识别技术，识别出图像中的位姿控制板确定机器人的当前位置及当前偏移角度，实现对机器人绝对位置的定位。该方法成本较低，但是需要机器人所带相机准确捕捉到控制板图像信息，稳定性较差，抗干扰能力一般。

公开号为cn105737820的专利中描述了一种基于红外射线仪与特殊路标的定位方法，红外led发射红外线，经路标标签反射，在摄像机上成像，通过成像分析路标信息，路标位置即为小车位置。该方法中路标已事先标定，其坐标已知，此方法依赖于准确地发现路标，读取路标，但摄像机的视频范围有限，容易错过路标，系统实时性较差，定位效果一般。

公开号为cn104102222的专利中描述了一种基于激光测距仪和反射板的定位方法，激光测距仪发射的激光经反射板返回，机器人通过多个方向反射板与本体的距离计算得到机器人定位。该方法易于实现，算法简单，但需要在现场布设大量反射板，成本较高，且激光必须准确射到反射板才能有效工作，工作环境要求较高，而定位精度不高。

可见，上述组合导航的方式需要对现场进行特殊的布置，且存在定位不够精确等技术问题，影响导航的顺利进行。

技术实现要素：

本发明的主要目的在于提出一种基于多导航模块的定位方法，及移动机器人，旨在无需在现场布设任何其他装置，实现对机器人本体的实时定位并提高定位精度。

为实现上述目的，本发明提出一种基于多导航模块的定位方法，应用于移动机器人，其特征在于，所述移动机器人包括gps导航模块、惯性导航模块、激光雷达导航模块；所述方法包括：

通过所述gps导航模块获取所述移动机器人在t-1时刻以及t时刻的经纬度坐标，并转换至xy坐标系下坐标值；通过所述惯性导航模块在t-1时刻和t时刻对应的信号信息得出所述移动机器人的偏航角变化；获取所述激光雷达导航模块在t-1时刻和t时刻的扫描数据；

根据预先建立的所述移动机器人的运动学模型、所述移动机器人在t-1时刻和t时刻的坐标值、所述偏航角变化得出所述移动机器人在t时刻的惯导位姿；

根据所述移动机器人在t-1时刻和t时刻的扫描数据计算所述移动机器人在t时刻的激光位姿；

结合环境路标信息对所述移动机器人的惯导位姿与所述激光位姿进行融合，得到所述移动机器人在t时刻对应的当前位姿。

可选地，根据预先建立的所述移动机器人的运动学模型、所述移动机器人在t-1时刻和t时刻的坐标值和所述偏航角变化得出所述移动机器人在t时刻的惯导位姿包括：

判断所述移动机器人在t-1时刻和t时刻的坐标值的差值是否在预设的合理范围内；

若是，则根据所述移动机器人在t时刻的坐标值、预先建立的所述移动机器人的运动学模型和所述偏航角的变化得出所述移动机器人在t时刻的惯导位姿；

若否，则通过所述惯性导航模块的t-1时刻和t时刻对应的信号信息得出所述移动机器人的运动距离和方向，并根据所述移动机器人在t-1时刻的坐标值、所述移动机器人的运动距离和方向以及预先建立的所述移动机器人的运动学模型、所述偏航角的变化得出所述移动机器人在t时刻的惯导位姿。

可选地，所述根据所述移动机器人在t-1时刻和t时刻的坐标值以及在t-1时刻和t时刻的扫描数据、所述偏航角变化计算所述移动机器人在t时刻的激光位姿包括：

将所述移动机器人从t-1时刻至t时刻的环境变化信息作为从t-1时刻的扫描数据至t时刻的扫描数据的初始位姿变换，执行plicp算法，迭代计算得到最终位姿变换后，计算所述移动机器人在t时刻的激光位姿。

可选地，所述移动机器人还包括视觉导航模块，所述移动机器人从t-1时刻至t时刻的环境变化信息为所述视觉导航模块对在t时刻对应的环境的匹配信息。

可选地，所述结合环境路标信息对所述移动机器人的惯导位姿与所述激光位姿进行融合，得到所述移动机器人在t时刻对应的当前位姿包括：

根据所述环境路标信息的观测结果计算卡尔曼增益矩阵；

利用所述卡尔曼增益矩阵依据扩展卡尔曼滤波算法对所述移动机器人的惯导位姿与所述激光位姿进行计算，得到所述移动机器人在t时刻对应的当前位姿。

此外，为实现上述目的，本发明还提供一种移动机器人，其特征在于，包括：

gps导航模块，用于获取在t-1时刻以及t时刻的经纬度坐标，并转换至xy坐标系下坐标值；

惯性导航模块，用于在t-1时刻和t时刻对应的信号信息得出所述移动机器人的偏航角变化；

激光雷达导航模块，用于获取在t-1时刻和t时刻的扫描数据；

第一计算模块，用于根据预先建立的所述移动机器人的运动学模型、所述移动机器人在t-1时刻和t时刻的坐标值、所述偏航角变化得出所述移动机器人在t时刻的惯导位姿；

第二计算模块，用于根据所述移动机器人在t-1时刻和t时刻的扫描数据计算所述移动机器人在t时刻的激光位姿；

第三计算模块，用于结合环境路标信息对所述移动机器人的惯导位姿与所述激光位姿进行融合，得到所述移动机器人在t时刻对应的当前位姿。

可选地，所述第一计算模块用于：

判断所述移动机器人在t-1时刻和t时刻的坐标值的差值是否在预设的合理范围内；

若是，则根据所述移动机器人在t时刻的坐标值、预先建立的所述移动机器人的运动学模型和所述偏航角的变化得出所述移动机器人在t时刻的惯导位姿；

若否，则通过所述惯性导航模块的t-1时刻和t时刻对应的信号信息得出所述移动机器人的运动距离和方向，并根据所述移动机器人在t-1时刻的坐标值、所述移动机器人的运动距离和方向以及预先建立的所述移动机器人的运动学模型、所述偏航角的变化得出所述移动机器人在t时刻的惯导位姿。

可选地，所述第二计算模块用于：

将所述移动机器人从t-1时刻至t时刻的环境变化信息作为从t-1时刻的扫描数据至t时刻的扫描数据的初始位姿变换，执行plicp算法，迭代计算得到最终位姿变换后，计算所述移动机器人在t时刻的激光位姿。

可选地，所述移动机器人还包括视觉导航模块，所述移动机器人从t-1时刻至t时刻的环境变化信息为所述视觉导航模块对在t时刻对应的环境的匹配信息。

可选地，所述第三计算模块用于：

根据所述环境路标信息的观测结果计算卡尔曼增益矩阵；

利用所述卡尔曼增益矩阵依据扩展卡尔曼滤波算法对所述移动机器人的惯导位姿与所述激光位姿进行计算，得到所述移动机器人在t时刻对应的当前位姿。

本发明提出的基于多导航模块的定位方法及移动机器人，克服了现有技术中单一导航系统存在的局限性和系统误差，以及现有简单组合导航技术对使用环境的抗干扰性较差的问题，提高了定位精度与机器人导航稳定性，大大扩展的机器人的适运行环境。

附图说明

图1为本发明实施例的移动机器人的基于多导航模块的定位方法的流程示意图；

图2为本发明实施例的路径的规划的流程示意图

图3为本发明实施例的移动机器人的结构示意图；

图4为本发明另一实施例的移动机器人的结构示意图；

本发明目的的实现、功能特点及优点将结合实施例，参照附图做进一步说明。

具体实施方式

应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

在后续的描述中，使用用于表示元件的诸如“模块”、“部件”或“单元”的后缀仅为了有利于本发明的说明，其本身并没有特定的意义。因此，"模块"与"部件"可以混合地使用。

如图1所示，本发明提供一种基于多导航模块的定位方法，应用于移动机器人，所述移动机器人包括gps导航模块、惯性导航模块、激光雷达导航模块；所述方法包括步骤：

s1、通过所述gps导航模块获取所述移动机器人在t-1时刻以及t时刻的经纬度坐标，并转换至xy坐标系下坐标值；通过所述惯性导航模块在t-1时刻和t时刻对应的信号信息得出所述移动机器人的偏航角变化；获取所述激光雷达导航模块在t-1时刻和t时刻的扫描数据；

s2、根据预先建立的所述移动机器人的运动学模型、所述移动机器人在t-1时刻和t时刻的坐标值、所述偏航角变化得出所述移动机器人在t时刻的惯导位姿；

s3、根据所述移动机器人在t-1时刻和t时刻的扫描数据计算所述移动机器人在t时刻的激光位姿；

s4、结合环境路标信息对所述移动机器人的惯导位姿与所述激光位姿进行融合，得到所述移动机器人在t时刻对应的当前位姿。

在具体实施时，本实施例采用以下算法执行上述步骤s2、s3；具体地，在步骤s2中，根据移动机器人的运动学模型，计算从t-1时刻到t时刻位置的变化(△xt,△yt)以及imu的偏航角变化θt，并计算出t时刻里程计所对应的机器人惯导位姿podom(t)＝[x(t),y(t),θ(t)]^t。在步骤s3中，可以令q0＝(△xt,△yt,θt)，根据t时刻激光雷达的扫描数据st以及t-1时刻的参考扫描数据st-1，将q0。作为从st-1到st的初始位姿变换，执行plicp算法，迭代计算得到最终的位姿变换qk。再计算t时刻雷达扫描匹配所对应的机器人位姿pscan(t)＝r(θk)pscan-(t-1)+tk。

一般地，在步骤s1之前，还可以完成移动机器人的运动学模型的建立以及路径的规划。

在建立移动机器人的运动学模型时，可以根据机器人底盘结构，确定机器人运动线速度与角速度之间的关系，从而分析机器人移动位姿规律，搭建机器人运动模型。

请参照图2，在进行路径的规划时，可以包括以下步骤：

s11、在环境现场进行激光扫描，利用激光雷达采集障碍物信息二维点集；

s12、将采集到的障碍物信息二维点集转换为栅格地图作为参考扫描结果，并将机器人位置转换为xy坐标系下坐标；

s13、在室外环境实时获取机器人经纬度坐标，并转换为与激光导航系统同参数的xy坐标；

s14、在xy坐标系下对机器人运动轨迹进行规划；

此时，移动机器人在行进过程中，可以利用本体携带激光扫描仪在现场对环境轮廓进行激光扫描；根据扫描点建立极坐标系和笛卡尔坐标系。

在本发明的一个实施例中中，上述步骤s2可以包括：

判断所述移动机器人在t-1时刻和t时刻的坐标值的差值是否在预设的合理范围内；

若是，则说明此时gps导航模块的精确度较高，可以根据所述移动机器人在t时刻的坐标值直接获取移动机器人的位置；然后结合预先建立的所述移动机器人的运动学模型和所述偏航角的变化得出所述移动机器人在t时刻的惯导位姿；

若否，则说明此时gps导航模块可能信号较弱，可以通过所述惯性导航模块的t-1时刻和t时刻对应的信号信息得出所述移动机器人的运动距离和方向，更具体地，可以根据编码器计算机器人运动距离；根据电子罗盘与陀螺仪判别机器人运动方向；然后根据所述移动机器人在t-1时刻的坐标值、所述移动机器人的运动距离和方向计算出移动机器人的位置，然后结合预先建立的所述移动机器人的运动学模型和所述偏航角的变化得出所述移动机器人在t时刻的惯导位姿。

在本发明的另一实施例中，上述步骤s3具体包括：将所述移动机器人从t-1时刻至t时刻的环境变化信息作为从t-1时刻的扫描数据至t时刻的扫描数据的初始位姿变换，执行plicp算法，迭代计算得到最终位姿变换后，计算所述移动机器人在t时刻的激光位姿。

更具体地，移动机器人还包括视觉导航模块，所述移动机器人从t-1时刻至t时刻环境变化信息为所述视觉导航模块对在t时刻对应的环境的匹配信息。

在本发明的又一实施例中，可以根据在得出惯导位姿后采用迭代最近点算法将扫描点集映射到路径规划时采集障碍物信息二维点集上，根据机器人学空间坐标转换原理得到两者变换关系；该变换关系用作激光位姿。

在本发明的一个实施例中，上述步骤s4具体包括：

根据所述环境路标信息的观测结果计算卡尔曼增益矩阵；

利用所述卡尔曼增益矩阵依据扩展卡尔曼滤波算法对所述移动机器人的惯导位姿与所述激光位姿进行计算，得到所述移动机器人在t时刻对应的当前位姿。

上面对本发明实施例中的基于多导航模块的定位方法，进行了描述，下面对本发明实施例中的移动机器人进行描述。

如图3所示，本发明提出一种移动机器人，包括gps导航模块10、惯性导航模块20、激光雷达导航模块30、第一计算模块71、第二计算模块72、第三计算模块73。

其中，gps导航模块10、惯性导航模块20、激光雷达导航模块30与工控机50相连，其采集到的数据发送至数据采集板60，第一计算模块71、第二计算模块72、第三计算模块73集成在控制板70上，控制板70可以控制电机驱动器80以驱动前后轮运动，电机驱动器80可以包括后轮无刷直流电机驱动器、前轮步进电机驱动器。

gps导航模块10用于获取在t-1时刻以及t时刻的经纬度坐标，并转换至xy坐标系下坐标值；

惯性导航模块20用于在t-1时刻和t时刻对应的信号信息得出所述移动机器人的偏航角变化；惯性导航模块20包括电子罗盘、角度传感器、编码器等。

激光雷达导航模块30用于获取在t-1时刻和t时刻的扫描数据；

第一计算模块71用于根据预先建立的所述移动机器人的运动学模型、所述移动机器人在t-1时刻和t时刻的坐标值、所述偏航角变化得出所述移动机器人在t时刻的惯导位姿；在具体实施时，可以根据移动机器人的运动学模型，计算从t-1时刻到t时刻位置的变化(△xt,△yt)以及imu的偏航角变化θt，并计算出t时刻里程计所对应的机器人惯导位姿podom(t)＝[x(t),y(t),θ(t)]^t；

第二计算模块72用于根据所述移动机器人在t-1时刻和t时刻的扫描数据计算所述移动机器人在t时刻的激光位姿；在具体实施时，可以令q0＝(△xt,△yt,θt)，根据t时刻激光雷达的扫描数据st以及t-1时刻的参考扫描数据st-1，将q0。作为从st-1到st的初始位姿变换，执行plicp算法，迭代计算得到最终的位姿变换qk。再计算t时刻雷达扫描匹配所对应的机器人位姿pscan(t)＝r(θk)pscan-(t-1)+tk；

第三计算模块73用于结合环境路标信息对所述移动机器人的惯导位姿与所述激光位姿进行融合，得到所述移动机器人在t时刻对应的当前位姿。

一般地，还可以包括路径规划模块，集成在控制板上，路径规划模块用于在环境现场进行激光扫描，利用激光雷达采集障碍物信息二维点集；将采集到的障碍物信息二维点集转换为栅格地图作为参考扫描结果，并将机器人位置转换为xy坐标系下坐标；在室外环境实时获取机器人经纬度坐标，并转换为与激光导航系统同参数的xy坐标；在xy坐标系下对机器人运动轨迹进行规划；

此时，移动机器人在行进过程中，可以利用本体携带激光扫描仪在现场对环境轮廓进行激光扫描；根据扫描点建立极坐标系和笛卡尔坐标系。

在本发明的一个实施例中，上述第一计算模块71可以用于：

判断所述移动机器人在t-1时刻和t时刻的坐标值的差值是否在预设的合理范围内；

若是，则说明此时gps导航模块的精确度较高，可以根据所述移动机器人在t时刻的坐标值直接获取移动机器人的位置；然后结合预先建立的所述移动机器人的运动学模型和所述偏航角的变化得出所述移动机器人在t时刻的惯导位姿；

若否，则说明此时gps导航模块可能信号较弱，可以通过所述惯性导航模块的t-1时刻和t时刻对应的信号信息得出所述移动机器人的运动距离和方向，更具体地，可以根据编码器计算机器人运动距离；根据电子罗盘与陀螺仪判别机器人运动方向；然后根据所述移动机器人在t-1时刻的坐标值、所述移动机器人的运动距离和方向计算出移动机器人的位置，然后结合预先建立的所述移动机器人的运动学模型和所述偏航角的变化得出所述移动机器人在t时刻的惯导位姿。

在本发明的一个实施例中，上述第二计算模块72可以用于：将所述移动机器人从t-1时刻至t时刻的环境变化信息作为从t-1时刻的扫描数据至t时刻的扫描数据的初始位姿变换，执行plicp算法，迭代计算得到最终位姿变换后，计算所述移动机器人在t时刻的激光位姿。将所述移动机器人从t-1时刻至t时刻的环境变化信息作为从t-1时刻的扫描数据至t时刻的扫描数据的初始位姿变换，执行plicp算法，迭代计算得到最终位姿变换后，计算所述移动机器人在t时刻的激光位姿。

更具体地，如图4所示，移动机器人还包括视觉导航模块40，所述移动机器人从t-1时刻至t时刻环境变化信息为所述视觉导航模块对在t时刻对应的环境的匹配信息。

在本发明的一个实施例中，上述第三计算模块73用于：

根据所述环境路标信息的观测结果计算卡尔曼增益矩阵；

利用所述卡尔曼增益矩阵依据扩展卡尔曼滤波算法对所述移动机器人的惯导位姿与所述激光位姿进行计算，得到所述移动机器人在t时刻对应的当前位姿。

本发明提出的基于多导航模块的定位方法及移动机器人，采用多种主流导航方式进行融合，改进了算法，提高了定位精度与机器人导航稳定性，大大扩展的机器人的适运行环境，且克服了现有技术中单一导航系统存在的局限性和系统误差，以及现有简单组合导航技术对使用环境的抗干扰性较差的问题。

需要说明的是，在本文中，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者装置不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者装置所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括该要素的过程、方法、物品或者装置中还存在另外的相同要素。

通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到上述实施例方法可借助软件加必需的通用硬件平台的方式来实现，当然也可以通过硬件，但很多情况下前者是更佳的实施方式。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质(如rom/ram、磁碟、光盘)中，包括若干指令用以使得一台终端设备(可以是手机，计算机，服务器，空调器，或者网络设备等)执行本发明各个实施例所述的方法。

以上仅为本发明的优选实施例，并非因此限制本发明的专利范围，凡是利用本发明说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本发明的专利保护范围内。