一种面向室内移动机器人的高精度组合导航系统及方法与流程

本发明涉及一种利用惯性系统进行定位的技术领域，尤其涉及无GPS信号区域惯性导航系统的技术领域。

背景技术：

惯性技术的民用领域主要是精密测量和定位。在GPS信号不能进入的水下、冰下、原始森林、隧道以及城市建筑物密集地区，惯性技术在精密导航、测量以及定位方面仍较GPS具有优势。此外，采用高精度的航海和航空重力仪用于陆地和海洋资源的物理勘探，以及采用惯性系统测量勘探深埋地下的各种管道的曲率半径等，都是惯性系统具有优势的应用领域。

目前国内外对惯性导航的理论研究比较多，导航方法主要通过惯性测量组件（IMU）测量载体相对惯性空间的角速率和加速度信息，利用牛顿运动定律自动推算物体的瞬时速度和位置信息，近几年也取得了一系列好的结果，国外主要用于AGV小车，而且也有相应的产品问世，但是价格昂贵。一般用于港口物流和仓储等领域。国内这方面理论研究比较多，但真正投入使用的国产惯性导航小型机器人却很少，高校中目前有浙江大学、湖南大学、南昌大学等在进行研究。市场应用目前主要依赖进口，价格普遍较高。

申请号为201210173646.0的中国专利申请公开了一种磁导航辅助GPS/INS 组合导航定位系统及其控制方法，属于机动车辆导航定位领域。组合导航定位系统包括GPS 接收机、INS 及INS 处理单元、铺设在道路上的磁钉、车载磁传感器及中央处理模块。中央处理单元包括解码器和卡尔曼滤波模块，接收GPS 接收机、INS 处理单元和磁传感器的导航信息，通过卡尔曼滤波计算，修正INS 处理模块的参数，得到最终的组合导航信息数据。该方法有效地解决了GPS 导航系统易受环境影响以及INS 的导航定位误差随时间延续不断增大的问题，提高了导航定位精度，实现了不间断的车辆高精度导航定位功能。然而GPS不适用于室内的导航系统。

申请号为201510590866.7的中国专利申请公开了一种适合仓储AGV 的惯性导航方法，它包括如下子步骤：S1 ：导航系统搭建，S2 ：读取磁钉数据，S3 ：获取小车偏移数据，S4 ：规划路径，S5 ：获取陀螺仪角度，S6 ：角度校准，S7 ：读取RFID，S8 ：确定运行模式，S9 ：运动控制，S10 ：重复步骤S2-S9。该方法采用磁钉代替磁条和二维码，有效地节约成本的同时能解决二维码受多尘环境影响较严重的缺陷，实现无轨导航方式，该系统测量位置准确，位置精度可达±5mm，测量角度准确，角度精度为±0.1 度，其导航精度高，导航精度为±10mm，导航速度可达1m/s，有效的节约资源。然而其获取航向信息的算法比较复杂，需要导航系统占用更多的时间和资源进行计算，制约了导航速度。

申请号为201410572116.2的中国专利申请公开了一种新型叉车型AGV 小车，包括车体, 车体的前部设置有升降系统，升降系统传动连接有物料叉，其特征在于：所述车体上设置有多功能一体控制装置，所述车体的中部设置有双轮差速驱动装置，双轮差速驱动装置连接多功能一体控制装置；所述车体的四周设置有防护装置；所述车体的底部设置有自适应移动机构。该发明实现了在叉车的性能加载了AGV 的原地打转，精准控制，自动运行的特点，同时能够保证本发明转向更灵活，转弯半径变小。然而该发明需要较多的传感器同时确定航向信息，磁导航装置、惯性导航装置、激光导航装置、无发射板激光导航装置、光带导航装置，并且磁条的使用使导航精度受到了制约。

技术实现要素：

本发明提供了一种面向室内移动机器人的高精度组合导航系统及方法，在该系统中加入自主设计的惯性导航控制算法模块，并且独创了新的航向角计算方法，其有益效果在于：1）采用信息融合的方式，利用多种传感器融合来实现惯性导航；2）自主设计惯性导航控制算法模块，总结出更简单的算法及航向角算法公式；3）导航精度高；4）不需要依赖GPS；5）适合复杂环境导航；6) 面向室内机器人领域。

本发明提供了一种面向室内移动机器人的高精度组合导航系统，包括传感器数据采集模块、微处理器和PC端以及惯性导航控制算法模块、磁钉和先验地图，所述微处理器包括数据融合模块、控制模块、无线模块，所述数据融合模块对所述传感器数据采集模块获得的信号进行数据融合，所述惯性导航控制算法模块依据数据融合得到的信息及所述磁钉的位置，结合所述先验地图经过计算确定航向信息。

优选的是，所述航向信息通过航向角算法获取，具体公式为：

da为机器人检测到磁钉A并调整航向后，所述磁钉A距离机器人中轴线的距离；db为检测到磁钉B时，磁钉B距离中轴线的距离；L为机器人从节点A到达节点B行驶的里程；（xa,ya）是磁钉A的全局坐标；（xb,yb）是磁钉B的全局坐标；angle为磁钉A、B直接的路径航向，根据angle的正负判断机器人左偏或右偏。

上述任一方案中优选的是，所述传感器数据采集模块包括惯性导航传感器、磁导航传感器和光电编码器。

上述任一方案中优选的是，所述惯性导航传感器为9自由度惯性导航传感器。

上述任一方案中优选的是，所述9自由度惯性导航传感器包括三轴陀螺仪、三轴加速器和三轴电子罗盘。

上述任一方案中优选的是，所述三轴陀螺仪为ITG3205。

上述任一方案中优选的是，所述三轴加速器为ADXL345。

上述任一方案中优选的是，所述三轴电子罗盘为HMC5883L。

上述任一方案中优选的是，所述磁导航传感器为16位磁导航传感器。

上述任一方案中优选的是，所述16位磁导航传感器为XGS16N型16位磁导航传感器。

上述任一方案中优选的是，所述光电编码器为欧姆龙E6B2-CWZ6C编码器。

上述任一方案中优选的是，所述微处理器包括主控芯片，所述主控芯片为STM32-F103。

上述任一方案中优选的是，所述数据融合模块包括移动平台自定位模块和航向信息修正模块，所述移动平台自定位模块通过卡尔曼滤波算法进行输入融合。

上述任一方案中优选的是，所述PC端包括上位机模块。

上述任一方案中优选的是，所述PC端包括对控制模式的选择模块，所述控制模式包括遥控模式和自循迹模式。

上述任一方案中优选的是，所述无线模块采用ESP-12F 无线WIFI模块。

本发明还提供了一种面向室内移动机器人的高精度组合导航的方法，包括以下步骤：

步骤一：通过终端获取传感器的值，

步骤二：进行数据融合计算出下一步的航向值，

步骤三：发送给上位机，

步骤四：传送给机器人导航系统进行决策，具体步骤为，

通过主控芯片采集惯性导航传感器、磁导航传感器和光电编码器的信号；对所述惯性导航传感器、磁导航传感器和光电编码器信息进行融合，获取控制量，依据所述磁导航传感器以及磁钉的位置，结合先验地图，通过计算获得航向角；实时输出控制量信号到驱动模块，将计算结果通过无线模块传送到PC上位机模块；PC上位机模块显示定位结果和地图。

优选的是，所述控制量的获取步骤为：

所述惯性导航传感器返回位姿信息，所述光电编码器测量机器人当前的速度，通过所述惯性导航传感器和光电编码器的信息融合推算出所述机器人的相对位姿变化；

通过所述磁钉的位置对所述机器人航向信息进行修正；

惯性导航控制算法模块依据所述数据融合得到的信息及所述磁钉的位置，结合所述先验地图经过计算确定所述航向信息，数据融合模块采用卡尔曼滤波算法进行输入融合，传送给控制模块修正航向信息。

上述任一方案中优选的是，通过航向角算法计算得到所述航向信息，基体公式如下：

da为机器人检测到磁钉A并调整航向后，磁钉A距离机器人中轴线的距离；db为检测到磁钉B时，磁钉B距离中轴线的距离；L为机器人从节点A到达节点B行驶的里程；（xa,ya）是磁钉A的全局坐标；（xb,yb）是磁钉B的全局坐标；angle为磁钉A、B直接的路径航向，根据angle的正负判断机器人左偏或右偏。

上述任一方案中优选的是，通过所述惯性导航传感器测量机器人当前的加速度、角加速度和方位角。

上述任一方案中优选的是，通过所述磁导航传感器检测磁钉的位置，所述磁导航传感器包含有数据口，通过所述数据口向微控制器发送数据。

上述任一方案中优选的是，通过所述光电编码器检测机器人的速度，所述光电编码器包含有光电开关，通过所述光电开关检测光栅转动的次数获取机器人的速度。

上述任一方案中优选的是，所述数据融合模块包括移动平台自定位模块和航向信息修正模块，所述移动平台自定位模块进行移动平台自定位，通过卡尔曼滤波得到精确的角度信息，结合所述光电编码器算出里程；航向信息修正模块进行航向信息的修正，根据所述移动平台自定位模块得到的里程信息得到累计误差，通过磁导航传感器来修正所述累计误差，所述数据融合模块通过磁导航传感器检测到磁钉时，从保存的路径信息中获取所述磁钉的全局坐标，以及所述磁钉和下一个磁钉之间的航向，根据所述磁钉对应的路径信息，实时修正机器人的导航信息，以正确的姿态驶入下一个节点。本发明中所述下一个磁钉指的是行进轨迹上的下一个磁钉，当行进轨迹经过两个相邻磁钉时，所述当前磁钉和下一个磁钉为相邻磁钉。

上述任一方案中优选的是，微控制器通过无线模块与PC端进行双向通讯。

附图说明

为了更清楚地说明本发明实施例中的技术方案，下面将对实施例中所需要使用的附图作一简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1 是作为按照本发明的一优选实施例的面向室内移动机器人的高精度组合导航系统的总体框图

图2是作为按照本发明的一优选实施例的面向室内移动机器人的高精度组合导航系统的惯性导航电路设计图

图3是作为按照本发明的一优选实施例的面向室内移动机器人的高精度组合导航系统的磁导航电路设计图

图4是作为按照本发明的一优选实施例的一种面向室内移动机器人的高精度组合导航系统的光电编码器电路设计图

图5 是作为按照本发明的一优选实施例的面向室内移动机器人的高精度组合导航系统的WIFI无线发射接口电路设计图

图6 是作为按照本发明的一优选实施例的面向室内移动机器人的高精度组合导航方法的主程序流程图

图7 是作为按照本发明的一优选实施例的面向室内移动机器人的高精度组合导航系统及方法的航向角算法示意图。

具体实施方式

下面结合附图对本发明作进一步详细描述，有必要在此指出的是，以下具体实施方式只用于对本发明进行进一步的说明，不能理解为对本发明保护范围的限制，该领域的技术人员可以根据上述发明内容对本发明作出一些非本质的改进和调整。

本发明利用高精度磁导航传感器去检测磁钉，在磁钉位置，机器人根据在此位置保存的先验知识去校准惯性导航航向误差和机器人位置误差，并调整机器人航向。本发明利用轮式机器人作为实验平台，搭载高精度磁导航传感器并结合先验地图，理论上可以将横向误差控制在厘米级。这种方案传感器成本较低，导航算法简单易行，在工程上容易实现，可以极大的提高机器人导航的实时性和定位精度。

图1为整个系统的总体设计框图，惯性导航传感器返回位姿信息，光电编码器测量当前的速度，通过两种传感器的信息融合推算出系统的相对位姿变化。然后通过磁钉的位置对机器人航向信息进行修正。数据融合模块采用卡尔曼滤波算法进行输入融合，然后传送给控制模块进行系统的修正。卡尔曼滤波是线性系统中最优估计理论，它仅需要存储上一时刻的历史数据，数据量小，非常适合于处理器实时运算。微控制器通过WIFI模块与PC端进行双向通讯。

本实施例提供了一种优选的面向室内移动机器人的高精度组合导航系统，包括传感器数据采集模块、微处理器和PC端，以及惯性导航控制算法模块、磁钉和先验地图，所述微处理器包括数据融合模块、控制模块、无线模块，所述数据融合模块对所述传感器数据采集模块获得的信号进行数据融合，所述惯性导航控制算法模块依据所述数据融合得到的信息及所述磁钉的位置，结合所述先验地图经过计算确定航向信息。

所述航向信息通过航向角算法获取，具体公式为：

如图7所示，da为机器人检测到磁钉A并调整航向后，所述磁钉A距离机器人中轴线的距离；db为检测到磁钉B时，磁钉B距离中轴线的距离；L为机器人从节点A到达节点B行驶的里程；（xa,ya）是磁钉A的全局坐标；（xb,yb）是磁钉B的全局坐标；angle为磁钉A、B直接的路径航向，根据angle的正负判断机器人左偏或右偏。

在本实施例中，所述传感器数据采集模块包括惯性导航传感器、磁导航传感器和光电编码器。

在本实施例中，所述惯性导航传感器优选为9自由度惯性导航传感器。

在本实施例中，所述9自由度惯性导航传感器包括三轴陀螺仪、三轴加速器和三轴电子罗盘。相对于单一自由度的惯性导航传感器，9自由度惯性导航传感器从更多的维度对导航系统进行检测，包括对角度、加速度及方位角的多自由度检测，使所得数据的精度更高，导航系统的定位更加精准，操控更加精确。

本实施例中，所述磁导航传感器优选为16位磁导航传感器，所述16位磁导航传感器是两排的，一排可以出8位数据，两排组合成16位的数据。图3中仅展示出一排的电路设计。16位磁导航传感器为高精度磁导航传感器，其有益的效果在于对磁钉信号的检测更加精确即便是磁钉微小的信号变化也能够检测到，使所得到的数据更加准确，使导航系统的操控更加精确。

本实施例中，优选使用光电编码器代替传统的编码器，其有益效果在于光电编码器体积小，更加精密，光电编码器本身分辨度很高，并且无接触无磨损；同时，光电编码器既可检测角度位移，又可在机械转换装置帮助下检测直线位移。

本实施例中，所述微处理器包括主控芯片，所述主控芯片为STM32-F103。

进一步在本实施例中，在主控端设置有先验地图，可以辅助机器人导航系统航向信息的矫正。

在本实施例中，所述数据融合模块包括移动平台自定位模块和航向信息修正模块。所述移动平台自定位模块进行移动平台的自定位和所述航向信息修正模块进行航向信息的修正。移动平台自定位需要通过卡尔曼滤波得到精确的角度信息然后结合光电编码器算出里程。航向信息的修正，通过以上得到的里程信息的累计误差，然后通过磁导航传感器来修正所述累计误差。所述数据融合模块通过磁导航传感器检测到某个磁钉时，从保存的路径信息中获取该磁钉的全局坐标，以及当前磁钉和下一个磁钉之间的航向。根据磁钉对应的路径信息，实时修正机器人的导航信息，继而以正确的姿态驶入下一个节点。

在本实施例中，所述PC端包括上位机模块。本实施例的导航系统的工作原理为通过主控芯片采集惯性导航传感器、磁导航传感器和光电编码器的信号；对所述惯性导航传感器、磁导航传感器和光电编码器信息进行融合，获取控制量，依据所述磁导航传感器以及磁钉的位置，结合先验地图，通过计算获得航向角；实时输出控制量信号到驱动模块，将计算结果通过无线模块传送到PC端上位机模块；PC端上位机模块显示定位结果和地图。

在本实施例中，所述PC端包括控制模式选择模块，所述控制模式包括遥控模式和自循迹模式。所述遥控模式指操作者可以通过键盘发送控制命令遥控小车前进或后退；所述自循迹模式指操作者通过预先加载地图的方式将地图存放在主控芯片中，这样机器人就可以根据当前轨迹匹配离线地图里面的轨迹信息来获取下一个节点的信息。

本实施例中主要硬件选择为：

1. 处理器的选择 ：STM32-F103作为信息处理的核心部分

2. 惯性导航传感器的选择：采用9自由度惯性导航传感器，包括ITG3205 、ADXL345 和HMC5883L，其中ITG3205为三轴陀螺仪 、ADXL345为三周加速器和HMC5883L为三周电子罗盘

3. 磁导航传感器的选择：XGS16N型16位磁导航传感器

4. 光电编码器的选择：欧姆龙E6B2-CWZ6C编码器。

5. WIFI无线发射模块的选择：采用ESP-12F 无线WIFI模块。

如图2所示为本实施例中惯性导航电路设计图，惯性导航传感器可以测量当前系统的加速度，角加速度以及方位角。

如图3所示为本实施例中磁导航电路，磁导航传感器选用XGS16N型16位磁导航传感器，其中8个数据口与微控制器相连，检测到磁钉的位置，通过数据口向微控制器发送数据。

如图4所示为本实施例中光电编码器电路，光电编码器可以通过光电开关检测光栅转动的次数从而检测系统的速度。

如图5所示为WIFI无线发射接口电路设计图，本实施例中WIFI模块的数据传输接口为标准异步通信串口，分别连接微控制器A12、A13管脚。微控制器通过串口中断向WIFI模块发送信息。

本实施例的导航系统软件设计中采用STM32-F103作为主控芯片，通过主控芯片采集各MEMS惯性导航系统信号、磁导航传感器信号以及光电编码器信号；对各传感器信息进行融合，获取控制量，并实时输出控制量信号到驱动模块以及将计算结果通过WIFI模块传送到PC端上位机模块；PC端上位机模块负责定位结果显示和地图显示。当然，PC端在初始阶段可以选择控制模式，可以选择遥控模式和自循迹模式。遥控模式指操作者可以通过键盘发送控制命令遥控小车前进或后退；自循迹模式指操作者通过预先加载地图的方式将地图存放在主控芯片中，这样机器人就可以根据当前轨迹匹配离线地图里面的轨迹信息来获取下一个节点的信息。

通过本实施例提供的导航系统可按照以下方法进行室内移动机器人的高精度组合导航，所述导航系统程序步骤：

步骤一：通过终端获取传感器的值；

步骤二：进行数据融合计算出下一步的航向值；

步骤三：发送给上位机；

步骤四：传送给系统进行决策。

具体步骤为，

通过主控芯片采集惯性导航传感器、磁导航传感器和光电编码器的信号；对所述惯性导航传感器、磁导航传感器和光电编码器信息进行融合，获取控制量，依据所述磁导航传感器以及磁钉的位置，结合先验地图，通过计算获得航向角；实时输出控制量信号到驱动模块，将计算结果通过无线模块传送到PC上位机模块；PC上位机模块显示定位结果和地图。

在本实施例中，所述控制量的获取步骤为：

所述惯性导航传感器返回位姿信息，所述光电编码器测量机器人当前的速度，通过所述惯性导航传感器和光电编码器的信息融合推算出所述机器人的相对位姿变化；

通过所述磁钉的位置对所述机器人航向信息进行修正；

惯性导航控制算法模块依据所述数据融合得到的信息及所述磁钉的位置，结合所述先验地图经过计算确定所述航向信息，数据融合模块采用卡尔曼滤波算法进行输入融合，传送给控制模块修正航向信息。

如图6所示为主程序流程图，导航开始后传感器数据采集模块进行信号的采集，然后进行数据的预处理，再经过数据融合模块对数据进行融合处理，获取的结果传输到执行机构模块和/或经过无线网进行数据传输到上位机模块，上位机模块显示定位结果和地图，然后再重新回到传感器数据采集模块进行下一个步骤的循环，完成机器人系统的定位导航。

本实施例中系统的实现方法主要通过两大主要的处理模块完成，包括传感器采集模块和数据融合模块。

本实施例中传感器采集模块的作用原理为，主控芯片通过本身自带的AD模块和SCI模块获取低精度惯性导航系统的信号，通过脉冲累计器模块获取光电编码器采集的轮速信号，通过XGS16N型16位磁导航传感器获取磁钉偏离磁导航传感器的距离信号。

本实施例中数据融合模块包括两部分，一个是移动平台自定位，需要通过卡尔曼滤波得到精确的角度信息然后结合光电编码器算出里程。另一个是航向信息的修正，以上得到的里程信息中得到累计误差，然后通过磁导航传感器来修正这个累计误差。此模块通过磁导航传感器检测到某个磁钉时，从保存的路径信息中获取该磁钉的全局坐标，以及当前磁钉和下一个磁钉之间的航向。根据磁钉对应的路径信息，实时修正机器人的导航信息，继而以正确的姿态驶入下一个节点。下面介绍下航向角算法，如图7所示，da为机器人检测到磁钉A并调整航向后，磁钉A距离机器人中轴线的距离；db为检测到磁钉B时，磁钉B距离中轴线的距离；L为机器人从节点A到达节点B行驶的里程，（xa,ya）是磁钉A的全局坐标，（xb,yb）是磁钉B的全局坐标；angle为磁钉A、B直接的路径航向。由图我们可以算出angle的值，根据angle的正负我们可以判断机器人左偏还是右偏。

本发明的创新之处在于：

1. 采用信息融合的方式，利用多种传感器融合来实现惯性导航；

2. 自主设计惯性导航控制算法模块，与现有技术中通过传感器直接测得航向角的方法不同的是，本发明主要是依据磁导航传感器以及磁钉的位置，结合之前保存的先验地图，通过计算获得的航向角。在本发明的这一过程中需要进行磁钉的定位以及相应地图的采集，主要作用是提高了导航的精度，而且使导航系统受环境影响小。本发明所述的导航系统含有惯性导航控制算法模块，所述惯性导航控制算法模块通过航向角算法计算得到航向信息及矫正信息，其公式，为利用本发明所述导航系统获得的数据所得到，并且为本发明所述的方法所特有的，其算法简单，使得数据处理速度更快，导航系统更加精确快速；

3. 导航精度高，位置精度±5mm，角度精度±0.1度，导航精度±10mm，导航速度2m/s；

4. 不需要依赖GPS，导航系统中不包括GPS装置；

5. 适合复杂环境导航；

6. 使用磁钉而不是磁条，这是由于磁条只能够对连续的位置进行检测，而磁钉用于对单独的点进行检测，因此检测更加灵活，定位精度更高，成本也更低、易维护；

7.面向室内机器人领域。