CN109100738B - 一种基于多传感器信息融合的可靠定位系统及方法 - Google Patents

Abstract

本发明公开了一种基于多传感器信息融合的可靠定位系统及方法，该系统采用视觉导航、磁条导航与激光导航相结合的方式实现运动装置的定位和导航。系统具体结构包括图像识别单元，地面磁条识读单元，激光雷达测距单元，信息处理器，数据通讯接口等。图像识别单元识别室内顶端新型二维码条的图像信息，激光测距雷达对室内两侧的距离信息进行采集，磁条传感器感应地面的磁条信息，并将采集到的数据返回到信息处理器中处理，得到相应的返回值，根据得到的返回值输出运动装置的位姿信息。本发明可应用于室内环境，稳定可靠，导航路径可修改，能实现高精度的导航定位工作。

Description

一种基于多传感器信息融合的可靠定位系统及方法

技术领域

本发明涉及智能控制领域，特别是一种基于多传感器信息融合的可靠定位系统及方法。

背景技术

随着近几年新兴技术的不断发展以及对应的应用市场的拉动，智能化控制领域得到快速发展，同样，在室内智能运动装置上也不例外，应用该系统的室内智能运动装置可以是智能机器人或是智能AGV(Automatic Guided Vechicle)。虽然在市场的拉动下，自动化和智能化控制领域得到了快速发展，但是不可否认，由于我国在自动化和智能化控制领域起步较晚，相应的技术和国外比起来较为落后，在该领域还有很大的发展空间。

智能运动装置的技术含量高，开发难度大，综合应用了机械、电子、传感器技术、计算机技术以及自动控制技术等多学科多领域的内容。现在国际范围内普遍应用的引导定位方式主要有电磁引导方式、磁带引导方式、光学引导方式、激光引导方式以及视觉引导方式等。

其中电磁引导方式需要预先在行驶路径地面下埋设电磁引导线，通过电磁感应线圈检测电磁信号强度差异实现路径跟踪，这种引导方式是目前应用最为广泛且最为成熟的引导方式，它具有导引线隐蔽、成本低、原理简单可靠和定位精度高的优点，但其路径固定，变更和扩展困难。

磁带引导方式需要预先在行驶路径地面上铺设磁带，通过磁场传感器检测磁带信号实现路径跟踪，这种引导方式灵活性好，扩充和改变线路简单，但其极易受线路附近磁性材料或金属物质的影响。

光学引导方式利用光敏元件对色带信号进行检测实现路径跟踪，这种引导方式路径设置简单，但对环境要求较高，可靠性较差，定位精确较低。

激光引导方式需要预先在行驶路径周围放置位置己知的激光反射板，通过激光扫描测距仪实时检测与激光反射板的距离和方向信息实现路径跟踪，这种引导方式技术较为先进，定位精确，行驶路径灵活多变，能够适合各种环境，但该项技术仅被世界上少数几个生产厂商所掌握，目前我国还没有该项完整技术。

视觉引导方式需要预先在行驶路径地面上铺设路径中心线，利用CCD摄像头实时采集行驶路径信息，经图像处理单元识别路径中心线并计算运动装置与路径中心线的夹角从而实现路径跟踪，这种引导方式需要配备高性能的处理器，成本较高，定位精确。

随着图像处理技术的发展，基于人工路标的机器人视觉导航定位得到了广泛应用，但是却存在以下不足：1)路标常置于地面，易被周围过往的人或其他事物所干扰；2)简单的路标虽然图像处理简单，但是不具备纠错能力，对于一些复杂路标虽然具有完整的纠错能力，但图像过于复杂，实时性差；3)不易扩展到大环境下的机器人室内定位导航。

发明内容

本发明所要解决的技术问题是：针对现有技术的不足，综合了磁带引导方式、激光引导方式及视觉引导方式的优点，提出一种基于多传感器信息融合的可靠定位系统及方法。

本发明解决其技术问题采用以下的技术方案：

本发明提供的基于多传感器信息融合的可靠定位系统，包括图像识别单元、地面磁条识读单元、激光雷达测距单元、信息处理器、数据通讯接口，还包括张贴在室内顶端的用于导航中测量位置和速度的新型二维码条，安装在运动装置顶端光轴竖直向上的图像识读单元，布于地面的具有校正位置和速度的磁条，安装在运动装置中轴底部的磁条传感器，安装在运动装置两侧的激光测距雷达和激光测距雷达数据采集器，以及安装在运动装置上的信息处理器。

所述的新型二维码条为直线，其长度可延伸至运动装置所能经过的室内空间长，其宽度为10-20cm，由16进制数进行编码，由黑、白两色表示，其中黑色表示1，白色表示0；该新型二维码条分成四行，一行代表一位16进制数，其前端分别红色代表起始位，每行的每一位16进制数后有红色的过渡位，在新型二维码条的末端有红色的终止位。

新型二维码每一字符的长度由以下公式来确定：

式中：W表示新型二维码每一字符的长度，L表示室内空间的纵向长度。

所述的激光雷达测距单元安装在运动装置的两侧，两侧相对呈一条直线安装，每一侧包括两个激光测距雷达，每一侧包括两个激光雷达数据采集器，室内的两边墙面涂有反光油漆，激光能垂直入射墙面。

所述磁条位于新型二维码条的竖直下方，在磁条任意一侧每隔十米处增设一段磁条，该增设的磁条长度宽度不限，运动装置经过增设的磁条时，磁信号发生改变，信息处理器接受到这一信号，可进行计数与计时，若已经计时则运动装置经过所述增设的磁条时，重新从0开始计时。

本发明提供的基于多传感器信息融合的可靠定位方法，包括以下步骤：

步骤1：图像采集器获取室内顶端的新型二维码条的图像信息，将采集到的新型二维码条的图像信息传输到运动装置的信息处理器；

步骤2：与此同时激光测距雷达开始对室内两侧的距离信息进行采集，将采集到的距离数据传输到运动装置的信息处理器；

步骤3：与此同时磁条传感器开始感应地面的磁条信息，将采集到的磁信号传输到运动装置的信息处理器；

步骤4：信息处理器将接收到来自图像识读单元、激光雷达测距单元以及磁条传感器传来的信号进行处理：

步骤五：信息处理器接收到相应的返回值后，进行以下处理：

(101)返回值0：

①根据读取到的新型二维码图像信息，确定运动装置当前位置且每隔一固定时间周期计算运动装置的速度，计算方法如下式：

其中v表示此时运动装置的速度，S表示距上一次计算速度后新型二维码条之间的位置间隔，T₁表示时间间隔，

②运动装置经过在磁条任意一侧每隔十米处所增设一段磁条时，计算出此时运动装置运动距离与运动速度，计算运动距离的方法如下式：p＝10×n，其中p表示运动装置运动的总距离，n表示运动装置记录下经过增设磁条的个数，计算运动速度的方法如下式：

其中v₁表示运动装置的速度，t表示经过两增设磁条所经历的时间间隔，根据读取磁条得到的距离信息能够验证新型二维码条所得到的距离以及运动装置的速度，

③激光雷达测距单元采集距离信息，输出此时运动装置各个激光雷达到两边墙面的距离，输出此时航向角为0，偏移量为0；

(102)返回值1：

根据(101)步骤中①、②所读取磁条得到的距离信息，能够验证新型二维码条所得到的距离以及运动装置的速度；根据(101)步骤中③激光雷达测距单元采集距离信息，输出此时运动装置各个激光雷达到两边墙面的距离，输出此时航向角为0；④输出偏移量为0；

(103)返回值2：

根据(101)步骤中①、②所读取磁条得到的距离信息，能够验证新型二维码条所得到的距离以及运动装置的速度；根据(101)步骤中③激光雷达测距单元采集距离信息，输出此时运动装置各个激光雷达到两边墙面的距离，计算此时运动装置的航向角，计算方法采用下式：

其中|a_r-b_r|表示运动装置右侧前、后激光雷达测距单元采集到的距离差的绝对值，下标r表示右侧，同理可得

其中|a_l-b_l|表示运动装置左侧前、后激光雷达测距单元采集到的距离差的绝对值，下标l表示右侧，m表示前、后激光雷达测距单元的距离；④输出此时的偏移量为0；

(104)返回值3：

根据(101)步骤中①、②所读取磁条得到的距离信息，能够验证新型二维码条所得到的距离以及运动装置的速度；根据(101)步骤中③输出运动装置的航向角；计算此时运动装置在该航向角下的偏移量s_p，若θ＜5°，计算方法采用下式

若θ＞5°，计算方法采用下式

其中(a_r+b_r)表示右侧前、后激光雷达测距单元的距离和，(a_l+b_l)表示左侧前、后激光雷达测距单元的距离和，cosθ为航向角的余弦值；

(105)返回值4：

当图像识别单元不能识别新型二维码条图像时，根据(101)步骤中②输出运动装置的速度，位置信息无法验证，根据(101)步骤中③输出运动装置的航向角，根据(104)步骤中所述运动装置在该航向角下的偏移量，进行下述处理：

当磁条传感器无磁信号时，采用(101)步骤中①输出运动装置的速度，位置信息无法验证，(103)步骤中所述运动装置的航向角，以及(104)步骤中所述运动装置在该航向角下的偏移量，

当运动装置两侧的激光雷达测距单元无距离信号时，采用(101)步骤中①、②所读取磁条得到的距离信息，能够验证新型二维码条所得到的距离，以及运动装置的速度，不能输出运动装置的航向角及偏移量；

(106)返回值5：

图像识读单元，激光雷达测距单元以及磁传感器停止工作，并发出警报。

上述方法步骤4中，信息处理器将接收到来自图像识读单元、激光雷达测距单元以及磁条传感器传来的信号按以下几种情况进行处理：

(1)情况一：

图像识别单元采集到的新型二维码条图像能正确识别、新型二维码条图像处于图像识别框的正中，磁条传感器感应到的磁条处于正中、磁信号宽度不变，运动装置两侧的激光雷达测距单元到两侧墙面的距离相等，返回值0；

(2)情况二：

图像识别单元采集到的新型二维码条图像能正确识别、新型二维码条图像处于图像识别框的正中，磁条传感器感应到的磁条处于正中、磁信号宽度不变，运动装置单侧激光雷达测距单元到单侧墙面的距离相等，返回值1；

(3)情况三：

图像识别单元采集到的新型二维码条图像能正确识别、新型二维码条图像不处于图像识别框的正中，磁条传感器感应到的磁条处于正中、磁信号宽度变宽，运动装置两侧的激光雷达测距单元到两侧墙面的距离不相等，返回值2；

(4)情况四：

图像识别单元采集到的新型二维码条图像能正确识别、新型二维码条图像不处于图像识别框的正中，磁条传感器感应到的磁条不处于正中、磁信号宽度不变，运动装置两侧的激光雷达测距单元到两侧墙面的距离不相等，返回值3；

(5)情况五：

图像识别单元不能识别新型二维码条图像，或磁条传感器无磁信号，或运动装置两侧的激光雷达测距单元无距离信号，返回值4；

(6)情况六：

图像识别单元不能识别新型二维码条图像且磁条传感器无磁信号或运动装置两侧的激光雷达测距单元无距离信号，或磁条传感器无磁信号或运动装置两侧的激光雷达测距单元无距离信号，返回值5。

上述方法步骤3所述的磁条具有校正位置和速度的功能，其铺设方法是：铺设磁条前保持所铺设路段干燥、整洁、干净，预先对齐室内顶端新型二维码条，使磁条位于新型二维码条的竖直下方，由于该磁条依靠背面双面胶粘贴物体来实现固定，粘贴之后需要在磁条上轻轻按压，铺设时在每隔十米的位置在磁条任意位置增设一段长宽不限的磁条。

本发明提供的基于多传感器信息融合的可靠定位系统及方法，具有以下技术效果：

1.新型二维码条部署快速简单，制作成本低，呈直线排列，路线可灵活更改。

2.地面磁条成本低，引导方式灵活，扩充和改变路线方便。

3.激光测距雷达定位精确，行驶路径灵活多变，能够适合各种环境。

4.在室内环境下，运动装置难免要经过光线较暗的路段，此时仅靠图像采集器识别室内顶端的新型二维码条来引导运动装置，将难以维持运动装置以较高的精度在运动轨迹上行驶，本发明提出的基于多传感器信息融合的可靠定位系统及方法，可确保运动装置在图像采集器无法正常工作时，依然能保持运动装置不发生偏移。

5.在室内环境下，铺设在地面上的磁条可能会受路线附近磁性材料或金属物质的影响，而使得磁信号发生改变，本发明提出的基于多传感器信息融合的可靠定位系统及方法，可确保运动装置在磁传感器采集到错误的磁信号以及磁传感器无法正常工作时，依然能保持运动装置不发生偏移。当激光测距雷达发生意外不能正常工作时，本发明提出的基于多传感器信息融合的可靠定位系统及方法，依然能保持运动装置不发生偏移。

附图说明

图1为本发明多传感器信息融合的可靠定位系统及方法的整体流程示意图。

图2位本发明的系统机构图。

图3为本发明中传感器安装示意图。

图4为本发明的新型二维码条的示意图。

图5为本发明情况一运动装置的位姿示意图。

图6为本发明情况二运动装置的位姿示意图。

图7为本发明情况三运动装置的位姿示意图。

图8为本发明情况四运动装置的位姿示意图。

图9为本发明情况五运动装置的位姿示意图。

图10为本发明情况六运动装置的位姿示意图。

具体实施方式

下面结合实施例及附图对本发明作进一步说明，但不限定本发明。

本发明的基于多传感器信息融合的可靠定位系统，如图3所示，包括张贴在室内顶端的新型二维码条，安装在运动装置顶端光轴竖直向上的图像识读单元，布于地面的磁条识读单元，安装在运动装置中轴底部的磁条传感器，安装在运动装置两侧的激光测距雷达和激光测距雷达数据采集器，以及安装在运动装置上的信息处理器。

本发明设计了一种新型直线型二维码条，张贴在室内空间的顶端，可有效避免地面人或是障碍物对新型二维码条造成污损或是阻挡。新型二维码条的长度可延伸至运动装置所能经过的室内空间长，新型二维码条的宽度为10-20cm，由16进制数进行编码，由黑，白两色表示，其中黑色表示1，白色表示0。新型二维码条分成四行，一行代表一位16进制数，其前端分别红色代表起始位，每行的每一位16进制数后有红色的过渡位，在新型二维码条的末端有红色的终止位。新型二维码每一字符的长度可由如下公式来确定：

其中W表示新型二维码每一字符的长度，L表示室内空间的纵向长度。

所述新型二维码条，用于确定运动装置的位姿，该新型二维码条所示区域的数据如图4所示，如下：

0001001000110100/0101011001111000/1001101010111100/1101111011110000/0001001101010111。

所述图像识读单元，采用晴驰U3ST507-H型号的USB3.0CCD相机，该图像识读单元用于识别该新型二维码条的图像信息，输出运动装置的位置信息以及运动装置的速度，根据如下公式计算运动装置的速度。

式中：S表示距上一次计算速度后新型二维码条之间的间隔，T₁表示时间间隔。

所述磁条识读单元，用于读取地面磁条信号确定运动装置的位姿，所述磁条传感器采用CNS-MGS-16，该磁条传感器用于识别地面位于新型二维码条竖直下方的磁条信号，当经过增设于磁条任意一侧的磁条时，磁信号发生改变，此信号传输到信号处理器后，能输出此时运动装置的运动距离与运动速度，公式如下：

p＝10×n

式中：p表示运动装置运动的总距离，n表示运动装置记录下经过增设磁条的个数。

式中：v₁表示运动装置的速度，t表示经过两增设磁条所经历的时间间隔。

本发明根据读取磁条得到的距离信息，能够验证新型二维码条所得到的距离以及运动装置的速度。

所述激光测距雷达，用于测量运动装置的相对位置以及航向角，该激光测距雷达采用MSE-D500。

所述激光测距雷达数据采集器，用于收集激光测距雷达返回的距离信息，该激光测距雷达数据采集器采用EM9008多功能数据采集设备。

所述信息处理器，用于处理多传感器收集的信号，提供正确的输出值，该信息处理器采集器采用SLTS2F02-1龙芯2F高性能64位微处理器。

在运动装置所经过路径的两侧墙面涂有反光油漆，在运动装置的两侧各安装有至少两个激光雷达测距单元，激光测距雷达将激光垂直入射墙面，激光测距雷达数据采集器能采集由墙面反射回来的激光，并计算出运动装置的航向角，计算公式如下：

式中：|a_r-b_r|表示运动装置右侧前、后激光雷达测距单元采集到的距离差的绝对值，下标r表示右侧，m表示前、后激光雷达测距单元的距离。

式中：|a_l-b_l|表示运动装置左侧前、后激光雷达测距单元采集到的距离差的绝对值，下标l表示右侧，m表示前、后激光雷达测距单元的距离。

进一步地，确定在航向角θ＜5°时及在航向角θ＞5°时，计算出运动装置的偏移量S_p，计算公式如下：

式中：(a_r+b_r)表示右侧前、后激光雷达测距单元的距离和，(a_l+b_l)表示左侧前、后激光雷达测距单元的距离和，cosθ为航向角的余弦值。

本发明提供的基于多传感器信息融合的可靠定位方法，包括以下步骤：

步骤1：图像采集器获取室内顶端的新型二维码条的图像信息，将采集到的新型二维码条的图像信息传输到运动装置的信息处理器；

步骤2：与此同时激光测距雷达开始对室内两侧的距离信息进行采集，将采集到的距离数据传输到运动装置的信息处理器；

步骤3：与此同时磁条传感器开始感应地面的磁条信息，将采集到的磁信号传输到运动装置的信息处理器；

步骤4：信息处理器接收到来自图像识读单元、激光雷达测距单元以及磁条传感器传来的信号按以下几种情况处理：

(1)情况一：图像识别单元采集到的新型二维码条图像能正确识别、新型二维码条图像处于图像识别框的正中，磁条传感器感应到的磁条处于正中、磁信号宽度不变，运动装置两侧的激光雷达测距单元到两侧墙面的距离相等，返回值0；

(2)情况二：图像识别单元采集到的新型二维码条图像能正确识别、新型二维码条图像处于图像识别框的正中，磁条传感器感应到的磁条处于正中、磁信号宽度不变，运动装置单侧激光雷达测距单元到单侧墙面的距离相等，返回值1；

(3)情况三：图像识别单元采集到的新型二维码条图像能正确识别、新型二维码条图像不处于图像识别框的正中，磁条传感器感应到的磁条处于正中、磁信号宽度变宽，运动装置两侧的激光雷达测距单元到两侧墙面的距离不相等，返回值2；

(4)情况四：图像识别单元采集到的新型二维码条图像能正确识别、新型二维码条图像不处于图像识别框的正中，磁条传感器感应到的磁条不处于正中、磁信号宽度不变，运动装置两侧的激光雷达测距单元到两侧墙面的距离不相等，返回值3；

(5)情况五：图像识别单元不能识别新型二维码条图像，或磁条传感器无磁信号，或运动装置两侧的激光雷达测距单元无距离信号，返回值4；

(6)情况六：图像识别单元不能识别新型二维码条图像且磁条传感器无磁信号或运动装置两侧的激光雷达测距单元无距离信号，或磁条传感器无磁信号或运动装置两侧的激光雷达测距单元无距离信号，返回值5；

步骤5：信息处理器接收到相应的返回值后会做出如下处理：

情况一，如图5所示，图像识别单元采集到的新型二维码条图像能正确识别、新型二维码条图像处于图像识别框的正中，磁条传感器感应到的磁条处于正中、磁信号宽度不变，运动装置两侧的激光雷达测距单元到两侧墙面的距离相等，得到一个返回值0，运动装置处于正常行驶状态，能通过公式

计算出此时运动装置分别以新型二维码条得到的图像信息及以增设的磁条得到的磁信号信息，计算出此时运动装置的速度v，v₁，其中S表示距上一次计算速度后新型二维码条之间的间隔，T₁表示时间间隔，t表示经过两增设磁条所经历的时间间隔，以增设的磁条得到的磁信号信息计算出的速度为基准来验证以新型二维码条得到的图像信息计算出的速度。此时运动装置没有偏移，输出航向角为0，偏移量为0。

情况二，如图6所示，图像识别单元采集到的新型二维码条图像能正确识别、新型二维码条图像处于图像识别框的正中，磁条传感器感应到的磁条处于正中、磁信号宽度不变，运动装置单侧激光雷达测距单元到单侧墙面的距离相等，得到一个返回值1，运动装置的行进道路的墙面可能有支撑梁或是凹陷及突起，改变了运动装置到两侧墙面的距离，使同侧激光测距雷达单元到同侧的墙面距离相等，而异侧不等。此时能通过公式

计算出此时运动装置分别以新型二维码条得到的图像信息及以增设的磁条得到的磁信号信息，计算出此时运动装置的速度v，v₁，其中S表示距上一次计算速度后新型二维码条之间的间隔，T₁表示时间间隔，t表示经过两增设磁条所经历的时间间隔，以增设的磁条得到的磁信号信息计算出的速度为基准来验证以新型二维码条得到的图像信息计算出的速度。此时运动装置没有偏移，输出航向角为0，偏移量为0，并输出此时运动装置到两侧墙面的距离，运动装置能依据识别到的新型二维码条以及磁条信号继续在当前路径行进。

情况三，如图7所示，图像识别单元采集到的新型二维码条图像能正确识别、新型二维码条图像不处于图像识别框的正中，磁条传感器感应到的磁条处于正中、磁信号宽度变宽，运动装置两侧的激光雷达测距单元到两侧墙面的距离不相等，得到一个返回值2，可能由于地面打滑或是运动装置的轮系出现滑动摩擦，运动装置发生侧移，导致运动装置产生了某一航向角，但是运动装置没有发生偏移，此时能通过公式

计算出此时运动装置分别以新型二维码条得到的图像信息及以增设的磁条得到的磁信号信息，计算出此时运动装置的速度v，v₁，其中S表示距上一次计算速度后新型二维码条之间的间隔，T₁表示时间间隔，t表示经过两增设磁条所经历的时间间隔，以增设的磁条得到的磁信号信息计算出的速度为基准来验证以新型二维码条得到的图像信息计算出的速度，由新型二维码条可得出此时运动装置的位置信息，并由磁条验证。此时运动装置航向角发生了偏转，能通过

输出运动装置的航向角，其中|a_r-b_r|表示运动装置右侧前、后激光雷达测距单元采集到的距离差的绝对值，下标r表示右侧；|a_l-b_l|表示运动装置左侧前、后激光雷达测距单元采集到的距离差的绝对值，下标l表示右侧，m表示前、后激光雷达测距单元的距离，此时运动装置输出的偏移量为0。根据得到的返回值，根据当前速度计算出转向轮所要转过的角度，将此信号传递给运动装置的控制系统，可控制运动装置的转向轮朝运动装置偏移的反向转动，直到运动装置恢复到情况一的状态。

情况四，如图8所示，图像识别单元采集到的新型二维码条图像能正确识别、新型二维码条图像不处于图像识别框的正中，磁条传感器感应到的磁条不处于正中、磁信号宽度不变，运动装置两侧的激光雷达测距单元到两侧墙面的距离不相等，得到一个返回值3，可能由于地面打滑或是运动装置的轮系出现滑动摩擦，造成运动装置产生了位置以及航向的改变，此时能通过公式

计算出此时运动装置分别以新型二维码条得到的图像信息及以增设的磁条得到的磁信号信息，计算出此时运动装置的速度v，v₁，其中S表示距上一次计算速度后新型二维码条之间的间隔，T₁表示时间间隔，t表示经过两增设磁条所经历的时间间隔，以增设的磁条得到的磁信号信息计算出的速度为基准来验证以新型二维码条得到的图像信息计算出的速度，由新型二维码条可得出此时运动装置的位置信息，并由磁条验证。能通过

输出运动装置的航向角，其中|a_r-b_r|表示运动装置右侧前、后激光雷达测距单元采集到的距离差的绝对值，下标r表示右侧；|a_l-b_l|表示运动装置左侧前、后激光雷达测距单元采集到的距离差的绝对值，下标l表示右侧，m表示前、后激光雷达测距单元的距离，计算此时运动装置的偏移量S_p，若θ＜5°，此时航向角较小说明航向角较小可近似认为航向角没有发生改变，计算方法如下式

若θ＞5°，计算方法如下式

其中(a_r+b_r)表示右侧前、后激光雷达测距单元的距离和，(a_l+b_l)表示左侧前、后激光雷达测距单元的距离和，cosθ为航向角的余弦值，根据得到的返回值，传递给运动装置的控制系统，可根据得到的航向角θ，以及运动装置的偏移量S_p，控制运动装置的转向轮朝运动装置偏移的反向转动，运动装置中心运动到磁条上时，转向路回正，并进行情况三时的调整。

情况五，如图9所示，图像识别单元不能识别新型二维码条图像，或磁条传感器无磁信号，或运动装置两侧的激光雷达测距单元无距离信号，此时多传感器所采集到的数据返回到信息处理器中进行处理，得到一个返回值4，此时可能因为环境光过暗或是图像识别单元出现故障造成图像识别单元不能识别新型二维码图像，能根据

计算出运动装置的速度，但是由于新型二维码条无法识别，故此时无法对运动装置的位置以及速度进行验证，能通过

输出运动装置的航向角，其中|a_r-b_r|表示运动装置右侧前、后激光雷达测距单元采集到的距离差的绝对值，下标r表示右侧；|a_l-b_l|表示运动装置左侧前、后激光雷达测距单元采集到的距离差的绝对值，下标l表示右侧，m表示前、后激光雷达测距单元的距离，

输出运动装置的偏移量，其中(a_r+b_r)表示右侧前、后激光雷达测距单元的距离和，(a_l+b_l)表示左侧前、后激光雷达测距单元的距离和，cosθ为航向角的余弦值，并根据情况三、情况四对运动装置的运动状态进行调整；此时有可能因为环境中的杂质或是磁传感器出现故障导致磁条传感器无磁信号时，能根据

计算出运动装置的速度以及位置信息，其中S表示距上一次计算速度后新型二维码条之间的间隔，T₁表示时间间隔，但是无法由磁条信号来验证，能通过

输出运动装置的航向角，

输出运动装置的偏移量，根据得到的航向角θ，偏移量S_p以及当前速度，计算出转向轮所要转过的角度，将此信号传递给运动装置的控制系统，可控制运动装置的转向轮朝运动装置偏移的反向转动，直到运动装置恢复到情况一的状态；若两侧的激光测距雷达单元无距离信号，能通过公式

计算出此时运动装置分别以新型二维码条得到的图像信息及以增设的磁条得到的磁信号信息，计算出此时运动装置的速度v，v₁，以增设的磁条得到的磁信号信息计算出的速度为基准来验证以新型二维码条得到的图像信息计算出的速度，由新型二维码条可得出此时运动装置的位置信息，并由磁条验证，但是无法输出此时的航向角和偏移量。

情况六，如图10所示，图像识别单元不能识别新型二维码条图像且磁条传感器无磁信号或运动装置两侧的激光雷达测距单元无距离信号，或磁条传感器无磁信号或运动装置两侧的激光雷达测距单元无距离信号，得到一个返回值5，此时多传感器至少有两组不工作，为保障运动装置的正常工作，此时向外发出警报，并可通过外部控制器继续控制运动装置。

明显地，在运动装置的行进过程中，新型二维码条难免会离开图像识别单元的视野范围，若没有安装地面磁条识读单元、激光雷达测距单元的情况下，运动装置图像识别单元的视野若识别不到新型二维码条时，将难以保证运动装置的运动精度。运用此系统时，有效的综合了各个传感器的优点，提高了控制精度与可靠性，可防止当其中某一传感器出现故障时，运动装置无法正常工作的情况。

通过上述步骤，即可获得运动装置当前的位置、速度、航向角以及偏移量，从而实现了运动装置的室内定位导航。

Claims (7)

1.一种基于多传感器信息融合的可靠定位方法，其特征在于包括以下步骤：

步骤1：图像采集器获取室内顶端的新型二维码条的图像信息，将采集到的新型二维码条的图像信息传输到运动装置的信息处理器；

步骤2：与此同时激光测距雷达开始对室内两侧的距离信息进行采集，将采集到的距离数据传输到运动装置的信息处理器；

步骤3：与此同时磁条传感器开始感应地面的磁条信息，将采集到的磁信号传输到运动装置的信息处理器；

步骤4：信息处理器将接收到来自图像识读单元、激光雷达测距单元以及磁条传感器传来的信号进行处理：

步骤五：信息处理器接收到相应的返回值后，进行以下处理：

(101)返回值0：

①根据读取到的新型二维码图像信息，确定运动装置当前位置且每隔一固定时间周期计算运动装置的速度，计算方法如下式：

其中v表示此时运动装置的速度，S表示距上一次计算速度后新型二维码条之间的位置间隔，T₁表示时间间隔，

②运动装置经过在磁条任意一侧每隔十米处所增设一段磁条时，计算出此时运动装置运动距离与运动速度，计算运动距离的方法如下式：p＝10×n，其中p表示运动装置运动的总距离，n表示运动装置记录下经过增设磁条的个数，计算运动速度的方法如下式：

其中v₁表示运动装置的速度，t表示经过两增设磁条所经历的时间间隔，根据读取磁条得到的距离信息能够验证新型二维码条所得到的距离以及运动装置的速度，

③激光雷达测距单元采集距离信息，输出此时运动装置各个激光雷达到两边墙面的距离，输出此时航向角为0，偏移量为0；

(102)返回值1：

根据(101)步骤中①、②所读取磁条得到的距离信息，能够验证新型二维码条所得到的距离以及运动装置的速度；根据(101)步骤中③激光雷达测距单元采集距离信息，输出此时运动装置各个激光雷达到两边墙面的距离，输出此时航向角为0；④输出偏移量为0；

(103)返回值2：

根据(101)步骤中①、②所读取磁条得到的距离信息，能够验证新型二维码条所得到的距离以及运动装置的速度；根据(101)步骤中③激光雷达测距单元采集距离信息，输出此时运动装置各个激光雷达到两边墙面的距离，计算此时运动装置的航向角，计算方法采用下式：

其中|a_r-b_r|表示运动装置右侧前、后激光雷达测距单元采集到的距离差的绝对值，下标r表示右侧，同理可得

其中|a_l-b_l|表示运动装置左侧前、后激光雷达测距单元采集到的距离差的绝对值，下标l表示右侧，m表示前、后激光雷达测距单元的距离；④输出此时的偏移量为0；

(104)返回值3：

根据(101)步骤中①、②所读取磁条得到的距离信息，能够验证新型二维码条所得到的距离以及运动装置的速度；根据(101)步骤中③输出运动装置的航向角；计算此时运动装置在该航向角下的偏移量S_p，若θ＜5°，计算方法采用下式

若θ＞5°，计算方法采用下式

其中(a_r+b_r)表示右侧前、后激光雷达测距单元的距离和，(a_l+b_l)表示左侧前、后激光雷达测距单元的距离和，cosθ为航向角的余弦值；

(105)返回值4：

当图像识别单元不能识别新型二维码条图像时，根据(101)步骤中②输出运动装置的速度，位置信息无法验证，根据(101)步骤中③输出运动装置的航向角，根据(104)步骤中所述运动装置在该航向角下的偏移量，进行下述处理：

当磁条传感器无磁信号时，采用(101)步骤中①输出运动装置的速度，位置信息无法验证，(103)步骤中所述运动装置的航向角，以及(104)步骤中所述运动装置在该航向角下的偏移量，

当运动装置两侧的激光雷达测距单元无距离信号时，采用(101)步骤中①、②所读取磁条得到的距离信息，能够验证新型二维码条所得到的距离，以及运动装置的速度，不能输出运动装置的航向角及偏移量；

(106)返回值5：

图像识读单元，激光雷达测距单元以及磁传感器停止工作，并发出警报。

2.根据权利要求1所述基于多传感器信息融合的可靠定位方法，其特征在于步骤4中，信息处理器将接收到来自图像识读单元、激光雷达测距单元以及磁条传感器传来的信号按以下几种情况进行处理：

(1)情况一：

图像识别单元采集到的新型二维码条图像能正确识别、新型二维码条图像处于图像识别框的正中，磁条传感器感应到的磁条处于正中、磁信号宽度不变，运动装置两侧的激光雷达测距单元到两侧墙面的距离相等，返回值0；

(2)情况二：

图像识别单元采集到的新型二维码条图像能正确识别、新型二维码条图像处于图像识别框的正中，磁条传感器感应到的磁条处于正中、磁信号宽度不变，运动装置单侧激光雷达测距单元到单侧墙面的距离相等，返回值1；

(3)情况三：

图像识别单元采集到的新型二维码条图像能正确识别、新型二维码条图像不处于图像识别框的正中，磁条传感器感应到的磁条处于正中、磁信号宽度变宽，运动装置两侧的激光雷达测距单元到两侧墙面的距离不相等，返回值2；

(4)情况四：

图像识别单元采集到的新型二维码条图像能正确识别、新型二维码条图像不处于图像识别框的正中，磁条传感器感应到的磁条不处于正中、磁信号宽度不变，运动装置两侧的激光雷达测距单元到两侧墙面的距离不相等，返回值3；

(5)情况五：

图像识别单元不能识别新型二维码条图像，或磁条传感器无磁信号，或运动装置两侧的激光雷达测距单元无距离信号，返回值4。

3.根据权利要求1所述基于多传感器信息融合的可靠定位方法，其特征在于步骤3所述的磁条具有校正位置和速度的功能，其铺设方法是：铺设磁条前保持所铺设路段干燥、整洁、干净，预先对齐室内顶端新型二维码条，使磁条位于新型二维码条的竖直下方，由于该磁条依靠背面双面胶粘贴物体来实现固定，粘贴之后需要在磁条上轻轻按压，铺设时在每隔十米的位置在磁条任意位置增设一段长宽不限的磁条。

4.一种实现权利要求1至3中任一所述方法的基于多传感器信息融合的可靠定位系统，包括图像识别单元、地面磁条识读单元、激光雷达测距单元、信息处理器、数据通讯接口，其特征在于：还包括张贴在室内顶端的用于导航中测量位置和速度的新型二维码条，安装在运动装置顶端光轴竖直向上的图像识读单元，布于地面的具有校正位置和速度的磁条，安装在运动装置中轴底部的磁条传感器，安装在运动装置两侧的激光测距雷达和激光测距雷达数据采集器，以及安装在运动装置上的信息处理器；

在运动装置所经过路径的两侧墙面涂有反光油漆，在运动装置的两侧各安装有至少两个激光雷达测距单元，两侧相对呈一条直线安装，每一侧包括两个激光测距雷达，每一侧包括两个激光雷达数据采集器，激光测距雷达将激光垂直入射墙面，激光测距雷达数据采集器能采集由墙面反射回来的激光，并计算出运动装置的航向角。

5.根据权利要求4所述的基于多传感器信息融合的可靠定位系统，其特征在于：所述的新型二维码条为直线，其长度可延伸至运动装置所能经过的室内空间长，其宽度为10-20cm，由16进制数进行编码，由黑、白两色表示，其中黑色表示1，白色表示0；该新型二维码条分成四行，一行代表一位16进制数，其前端分别红色代表起始位，每行的每一位16进制数后有红色的过渡位，在新型二维码条的末端有红色的终止位。

6.根据权利要求5所述的基于多传感器信息融合的可靠定位系统，其特征在于：新型二维码每一字符的长度由以下公式来确定：

式中：W表示新型二维码每一字符的长度，L表示室内空间的纵向长度。

7.根据权利要求4所述的基于多传感器信息融合的可靠定位系统，其特征在于：所述磁条位于新型二维码条的竖直下方，在磁条任意一侧每隔十米处增设一段磁条，该增设的磁条长度宽度不限，运动装置经过增设的磁条时，磁信号发生改变，信息处理器接受到这一信号，可进行计数与计时，若已经计时则运动装置经过所述增设的磁条时，重新从0开始计时。

Images