CN109186597B - 一种基于双mems-imu的室内轮式机器人的定位方法 - Google Patents

Abstract

本发明提供了一种基于双MEMS‑IMU的室内轮式机器人定位方法，首先分别通过第一、第二惯性导航模块采集车轮、车身的运动信息，单独解算得到第一、第二定位信息，包括车轮以及车身的位置、速度、姿态角；再通过第二惯性模块的俯仰角对第一惯性模块的俯仰角进行补偿，进而使用补偿后的俯仰角和第一姿态角估计第一模块的安装角。同时，通过第一惯性模块测量车轮转动角速度换算得到的车体移动速度信息，分别提供给第一、第二惯导模块，分别构建运动约束进行组合解算，进而提升第一、第二定位信息的精度。最后将第一、第二定位信息以及两模块安装位置的空间几何信息组合提升整个系统的定位精度从而实现一种高精度、低成本的室内机器人相对定位方法。

Description

一种基于双MEMS-IMU的室内轮式机器人的定位方法

技术领域

本发明涉及室内定位技术领域，尤其涉及一种基于双MEMS-IMU的室内轮式机器人相对定位方法。

背景技术

随着全球导航卫星系统(Global Navigation Satellite System GNSS)的发展，位置服务(Location Based Service LBS)日渐成为人们生活工作当中必不可少的一部分，而作为LBS最后阶段研究的室内定位技术得到空前的重视，成为现阶段定位技术的研究热点。室内定位方法可分为相对定位和绝对定位，相对定位又称航迹推算，主要利用传感器如惯导、里程计、视觉里程计和激光雷达等测量机器人相对于初始状态的变化量来确定移动机器人当前的位姿。绝对定位又称全局定位，主要利用预先布设定位基站发射的信号交会定位(如超宽带)或利用预先测绘构建的导航数据库匹配定位(如地磁匹配)等对机器人在参考坐标系中的绝对位姿进行估算。

实际应用中，绝对定位因覆盖面积增大成本急剧增长的原因，定位基站和导航数据库常常无法对全区域以及无限大小的区域进行覆盖，因此相对定位对于保障系统的稳健性具有无可替代的作用。相对定位方法中，视觉里程计和激光雷达利用周围环境信息进行航位递推和估计，而系统稳定性通常因周围环境的变化而无法保证。惯性导航系统是一种全自主且不受周围环境影响的相对定位方法，常用于机器人位姿推算和估计。但惯性器件的观测误差会造成系统定位误差会随时间不断累积，导致其短时间内导航精度迅速下降。由此可见，目前的相对室内定位方法存在导航精度较低的技术问题。

发明内容

本发明实施例提供了一种基于双MEMS-IMU的室内轮式机器人相对定位方法，用以解决或者至少部分解决现有技术中室内相对定位方法存在导航精度较低的技术问题。

为了解决上述技术问题，本发明提供了一种基于双MEMS-IMU的室内轮式机器人定位方法，所述室内轮式机器人包括车轮与车身，所述MEMS-IMU为微型惯性测量单元，所述微型惯性测量单元包括第一惯性导航模块与第二惯性导航模块，所述方法包括：

步骤S101：通过所述第一惯性导航模块采集所述车轮的运动信息，并单独解算获得第一定位信息，其中，所述第一惯性导航模块位于所述车轮的中心，所述第一定位信息包括所述车轮的位置、速度和姿态角；

步骤S102：通过所述第二惯性导航模块采集所述车身的运动信息，并单独解算获得第二定位信息，其中，所述第二惯性导航模块位于所述车身的顶部中央，所述第二定位信息包括所述车身的位置、速度和姿态角；

步骤S103：通过所述第二惯性导航模块的俯仰角对所述第一惯性导航模块的俯仰角进行补偿，获得补偿后的俯仰角，使用补偿后的俯仰角和所述第一惯导模块的姿态角估计所述第一惯性导航模块的安装角；

步骤S104：通过所述第一惯性导航模块测量采集的车轮转动角速度，获得车体移动速度，将车体移动速度分别提供给所述第一惯性导航模块和第二惯性导航模块，并构建第一运动约束进行组合解算，获得组合后的第一定位信息，构建第二运动约束进行组合解算，获得组合后的第二定位信息；

步骤S105：基于所述组合后的第一定位信息、所述组合后的第二定位信息以及所述第一惯性模块和第二惯性模块的安装位置的空间关系，获得组合后的第三定位信息。

进一步地，步骤S101具体包括：

所述第一惯性导航模块输出第一观测值，将所述第一输出观测值用于惯性导航机械编排解算，获得所述第一定位信息，包括所述车轮的位置、速度和姿态角。

进一步地，步骤S102具体包括：

所述第二惯性导航模块输出第二观测值，将所述第二输出观测值用于惯性导航机械编排解算，获得所述第二定位信息，包括所述车身的位置、速度和姿态角。

进一步地，步骤S103具体包括：

步骤S1031：将所述第一惯性导航模块的安装角定义为从b系到v系的姿态差异，其中，所述b系为传感器坐标系，所述v系为载体坐标系，所述载体坐标系为所述车轮机器人坐标系，所述第一惯性导航模块的所述姿态差异的旋转矩阵为

所述姿态的欧拉角表示为:

其中，

表示所述第一惯性导航模块中，传感器坐标系到载体坐标系的横滚角、

表示所述第一惯性导航模块中，传感器坐标系到载体坐标系的俯仰角，

表示所述第一惯性导航模块中，传感器坐标系到载体坐标系的航向角；

步骤S1032：通过所述第二惯性导航模块的俯仰角对所述第一惯性导航模块的俯仰角进行补偿，获得所述补偿后的俯仰角

具体通过式(1)和式(2)获得：

其中，

表示所述第一惯性导航模块中，传感器坐标系到载体坐标系的横滚角，

表示所述第一惯性导航模块中，传感器坐标系到当地水平坐标系的横滚角，

表示所述第一惯性导航模块中，传感器坐标系到当地水平坐标系的俯仰角，

表示第二惯性导航模块中，传感器坐标系到当地水平坐标系的俯仰角，且基于预设关系，可以通过所述欧拉角获得相应的旋转矩阵

步骤S1033：采用补偿后的俯仰角和所述第一惯导模块的姿态角估计所述第一惯性导航模块的安装角。

进一步地，所述第一惯性导航模块和第二惯性导航模块包括陀螺仪和加速度计，所述方法还包括：

构建所述室内轮式机器人的非完整性约束，所述室内轮式机器人的包括前进方向速度、横向速度和垂向速度，所述前进方向速度、所述横向速度和所述垂向速度构成的三维速度约束为所述非完整性约束：

v_wheel＝ω_y·r_w(4)

其中，v_wheel前进方向速度，由陀螺仪的输出计算得到，横向速度和垂向速度均为0，ω_y为陀螺仪y轴角速率输出，r_w表示车轮半径。

进一步地，所述第一惯性导航模块和第二惯性导航模块分别包括陀螺仪和加速度计，所述第一观测值和第二观测值分别包括角速率和比力，步骤S104具体包括：

步骤S1041：将所述第一惯性导航模块测量的角速率和比力用于惯性导航机械编排解算，获得所述车轮的位置、速度和姿态；同时通过所述陀螺仪输出的角速率计算所述车体的里程速度，其中，所述角速率为垂直于所述车轮平面轴向的陀螺输出值，同时通过所述第一惯性导航模块的加速度计输出的比力计算水平角，其中所述比力为平行于所述车轮平面的两轴比力值，同时构建第一运动约束，其中所述第一运动约束为车体在运动过程中不发生侧移和跳跃；采用扩展卡尔曼滤波对所述车轮的位置、速度和姿态、所述里程速度、所述水平角和所述运动约束进行数据融合，获得位置、速度和姿态的最优估计，将其作为所述组合后的第一定位信息；

步骤S1042：将所述第二惯性导航模块测量的角速率和比力用于惯性导航机械编排解算，获得所述车体的位置、速度和姿态；同时接收所述第一惯性导航模块测量的角速率计算所述车体的里程速度，同时构建第二运动约束，其中所述第二运动约束为车体在运动过程中不发生侧移和跳跃；采用扩展卡尔曼滤波对所述车体的位置、速度和姿态、所述里程速度和所述运动约束进行数据融合，获得位置、速度和姿态的最优估计，将其作为所述组合后的第二定位信息。

进一步地，步骤S105具体包括：

构建所述第一惯性导航模块和所述第二惯性导航模块的安装位置的空间关系，所述空间关系由式(5)表示：

其中，d表示第一惯性导航模块与第二惯性导航模块之间的距离，const为预设常数，x₁表示第一惯性导航模块X轴坐标，y₁表示第一惯性导航模块Y轴坐标，z₁表示第一惯性导航模块Z轴坐标，x₂表示第二惯性导航模块的X轴坐标，y₂表示第二惯性导航模块的Y轴坐标，z₂表示第二惯性导航模块的Z轴坐标；

对所述组合后的第一定位信息、所述组合后的第二定位信息进行加权平均，并基于所述安装位置的空间关系对所述第一惯性导航模块和所述第二惯性导航模块的位置进行约束，获得组合后的第三定位信息。

进一步地，步骤S1041具体包括：

基于所述扩展卡尔曼滤波对所述惯导速度与所述里程速度进行加权融合，基于所述扩展卡尔曼滤波对所述惯导水平角与所述水平角进行加权融合，获得所述车轮的位置、速度和姿态的最优估计，将其作为所述组合后的第一定位信息。

进一步地，所述第一惯性导航模块的中心与所述车轮的中心完全重合。

本申请实施例中的上述一个或多个技术方案，至少具有如下一种或多种技术效果：

在本发明提供的方法中，通过位于车轮的中心的第一惯性导航模块采集车轮的运动信息，并单独解算获得第一定位信息，通过位于车身的顶部中央的第二惯性导航模块测量采集车身的运动信息，并单独解算获得第二定位信息，然后通过第二惯性导航模块的俯仰角对第一惯性导航模块的俯仰角进行补偿，获得补偿后的俯仰角，使用补偿后的俯仰角和第一惯导模块的姿态角估计第一惯性导航模块的安装角，再通过第一惯性导航模块测量采集的车轮转动角速度，获得车体移动速度，将车体移动速度分别提供给第一惯性导航模块和第二惯性导航模块，并构建第一运动约束进行组合解算，获得组合后的第一定位信息，构建第二运动约束进行组合解算，获得组合后的第二定位信息，最后基于组合后的第一定位信息、组合后的第二定位信息以及第一惯性模块和第二惯性模块的安装位置的空间关系，获得组合后的第三定位信息。

由于第一惯性导航模块和第二惯性导航模块构成双微惯性测量系统，可以通过第二惯性导航模块提供的俯仰角对第一惯性导航模块中的俯仰角进行补偿，从而可以实现了无里程计条件下的INS/Odo(Inertial Navigation System/Odometer)组合导航，使得定位结果在北向东向和垂向上均有明显提升，故而提高了室内定位的精度，且不需要安装里程计传感器，降低了成本。解决了现有技术中导航精度较低的技术问题。

附图说明

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

图1为本发明实施例中一种基于双MEMS-IMU的室内轮式机器人定位方法的流程图。

图2为双MEMS-IMU的在室内轮式机器人上的安装示意图；

图3为图1中所示方法的坐标示意图；

图4为图1中所示方法获得组合后的第一定位信息的原理图。

具体实施方式

本发明实施例提供了一种基于双MEMS-IMU的室内轮式机器人相对定位方法，用以改善现有技术中导航精度较低的技术问题。

本申请实施例中的技术方案，总体思路如下：

通过将第一惯性导航模块采集车轮的运动信息，并单独解算获得第一定位信息作为主要的定位信息，然后通过第二惯性导航模块采集的车身的运动信息，并单独解算获得第二定位信息中的俯仰角对第一定位信息中的俯仰角进行补偿，得到补偿后的俯仰角，并分别构建第一运动约束和第二运动约束进行组合解算，获得组合后的第一定位信息和组合后的第二定位信息，最后基于组合后的第一定位信息、组合后的第二定位信息以及第一惯性模块和第二惯性模块的安装位置的空间关系，获得组合后的第三定位信息，从而可以得到更为准确的定位结果，并实现无里程计条件下的INS/Odo组合导航，使得定位结果在北向东向和垂向上均有明显提升，故而提高了室内定位的精度，且不需要安装里程计传感器，降低了成本。解决了现有技术中导航精度较低的技术问题。

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本实施例提供了一种基于双MEMS-IMU的室内轮式机器人定位方法，其特征在于，室内轮式机器人包括车轮与车身，MEMS-IMU为微电子机械系统惯性测量单元，包括第一惯性导航模块与第二惯性导航模块，请参见图1。本方法包括：

步骤S101：通过第一惯性导航模块采集车轮的运动信息，并单独解算获得第一定位信息，其中，第一惯性导航模块位于车轮的中心，第一定位信息包括车轮的位置、速度和姿态角。

步骤S102：通过第二惯性导航模块采集车身的运动信息，并单独解算获得第二定位信息，其中，第二惯性导航模块位于车身的顶部中央，第二定位信息包括车身的位置、速度和姿态角。

具体来说，第一惯性导航模块和第二惯性导航模块用于位置信息的采集，其集成了多种传感器，包括陀螺仪、加速度计、磁力计、气压计，也可增加GNSS(Global NavigationSatellite System全球导航卫星系统)芯片、视觉传感器和测距传感器(红外传感器、超声波传感器等)。

在具体的实施过程中，双MEMS-IMU的安装示意图如图2所示，双MEMS-IMU即为包括两个惯导模块(第一惯性导航模块和第二惯性导航模块)的双系统，其中，第一惯性导航模块100安装于车轮的中心，用以采集车轮的运动信息，并单独解算获得第一定位信息，将第二惯性导航模块200安装于车身的顶部中央，用以采集车身的运动信息，并单独解算获得第二定位信息，其中，第一定位信息包括车轮的位置、速度和姿态角，第二定位信息均包括车身的位置、速度和姿态，姿态包括横滚角、俯仰角和航向角。

作为一种可选实施方式，步骤S101具体包括：

第一惯性导航模块输出第一观测值，将第一输出观测值用于惯性导航机械编排解算，获得第一定位信息。

具体来说，可以通过机械编排方法对观测值进行解算，从而获得第一定位信息。

作为一种可选实施方式，步骤S102具体包括：

第二惯性导航模块输出第二观测值，将第二输出观测值用于惯性导航机械编排解算，获得第二定位信息。

具体来说，第二惯性导航模块可以通过与第一惯性导航模块同样的方法，解算获得第二定位信息。

步骤S103：通过第二惯性导航模块的俯仰角对第一惯性导航模块的俯仰角进行补偿，获得补偿后的俯仰角，使用补偿后的俯仰角和第一惯导模块的姿态角估计第一惯性导航模块的安装角。

具体来说，第二惯性导航模块解算出的第二定位信息中包括车身的位置、速度和姿态角，而车身的姿态角包括横滚角、俯仰角和航向角，可以利用其中的俯仰角为第一惯性导航模块提供轮式机器人所处环境的平面倾斜角度，从而对第一惯性导航模块的第一定位信息中的俯仰角进行补偿，获得补偿后俯仰角，从而提升安装角估计的精度。

作为一种可选实施方式，步骤S103具体包括：

步骤S1031：将第一惯性导航模块的安装角定义为从b系到v系的姿态差异，其中，b系为传感器坐标系，v系为载体坐标系，载体坐标系为车轮机器人坐标系，第一惯性导航模块的姿态差异的旋转矩阵为

姿态的欧拉角表示为:

其中，

表示第一惯性导航模块中，传感器坐标系到载体坐标系的横滚角，

表示第一惯性导航模块中，传感器坐标系到载体坐标系的俯仰角，

表示第一惯性导航模块中，传感器坐标系到载体坐标系的航向角；

步骤S1032：通过所述第二惯性导航模块的俯仰角对所述第一惯性导航模块的俯仰角进行补偿，获得所述补偿后的俯仰角

具体通过式(1)和式(2)获得：

其中，

表示第一惯性导航模块中，传感器坐标系到载体坐标系的横滚角，

表示第一惯性导航模块中，传感器坐标系到自然坐标系的横滚角，

表示第一惯性导航模块中，传感器坐标系到自然坐标系的俯仰角，

表示第二惯性导航模块中，传感器坐标系到自然坐标系的俯仰角，且基于预设关系，可以通过欧拉角获得相应的旋转矩阵

步骤S1033：采用补偿后的俯仰角和第一惯导模块的姿态角估计第一惯性导航模块的安装角。

具体来说，上标中的数字为1时，表示第一惯性导航模块，上标中的数字为2时，表示第二惯性导航模块。传感器坐标系和载体坐标系的示意图如图3所示，由第一惯性导航模块和第二惯性导航模块的安装方式可知，传感器坐标系和载体坐标系之间始终保持y坐标轴一致，俯仰角即为一周期变化的相位角

随车轮转动不断更新。

式中，

表示k时刻相位角，dt表示第一惯性导航模块IMU的采样周期。

具体地，对于第一惯性导航模块而言，其从传感器坐标系到载体坐标系的横滚角，与传感器坐标系到自然坐标系的横滚角相等，然后由第二惯性导航模块的传感器坐标系到自然坐标系的俯仰角与第一惯性导航模块传感器坐标系到自然坐标系的俯仰角作差，得到补偿后的俯仰角

作为一种可选实施方式，第一惯性导航模块包括陀螺仪，在步骤S101之后，本实施例提供的方法还包括：

构建室内轮式机器人的非完整性约束，室内轮式机器人的包括前进方向速度、横向速度和垂向速度，前进方向速度、横向速度和垂向速度构成的三维速度约束为非完整性约束：

v_wheel＝ω_y·r_w (4)

其中，v_wheel前进方向速度，由陀螺仪的输出计算得到，横向速度和垂向速度均为0，ω_y为陀螺仪y轴角速率输出，r_w表示车轮半径。

具体来说，室内轮式机器人的前进方向速度即为通过陀螺仪输出的陀螺速率计算得出车轮的里程速度，并利用室内轮式机器人的运动特性构建非完整性约束，从而提高速度估计的准确性，进而提高定位的准确性。

步骤S104：通过第一惯性导航模块测量采集的车轮转动角速度，获得车体移动速度，将车体移动速度分别提供给第一惯性导航模块和第二惯性导航模块，并构建第一运动约束进行组合解算，获得组合后的第一定位信息，构建第二运动约束进行组合解算，获得组合后的第二定位信息。

其中，在一种实施方式中，第一惯性导航模块和第二惯性导航模块分别包括陀螺仪和加速度计，第一观测值和第二观测值分别包括角速率和比力，步骤S104具体包括：

步骤S1041：将第一惯性导航模块测量的角速率和比力用于惯性导航机械编排解算，获得车轮的位置、速度和姿态；同时通过陀螺仪y轴输出的角速率计算车体的里程速度，其中，y轴角速率为垂直于车轮平面轴向的陀螺输出值，同时通过第一惯性导航模块的加速度计输出的比力计算水平角，其中比力为平行于车轮平面的两轴比力值，同时构建第一运动约束，其中第一运动约束为车体在运动过程中不发生侧移和跳跃；采用扩展卡尔曼滤波对车轮的位置、速度和姿态、里程速度、水平角和运动约束进行数据融合，获得位置、速度和姿态的最优估计，将其作为组合后的第一定位信息。

步骤S1042：将第二惯性导航模块测量的角速率和比力用于惯性导航机械编排解算，获得车体的位置、速度和姿态；同时接收第一惯性导航模块测量的角速率计算车体的里程速度，同时构建第二运动约束，其中第二运动约束为车体在运动过程中不发生侧移和跳跃；采用扩展卡尔曼滤波对车体的位置、速度和姿态、里程速度和运动约束进行数据融合，获得位置、速度和姿态的最优估计，将其作为组合后的第二定位信息。

在一种实施方式中，步骤S1041具体包括：

基于扩展卡尔曼滤波对惯导速度与里程速度进行加权融合，基于扩展卡尔曼滤波对惯导水平角与水平角进行加权融合，获得车轮的位置、速度和姿态的最优估计，将其作为组合后的第一定位信息。

具体来说，组合后的第一定位信息的原理图如图4所示，扩展卡尔曼滤波采用的误差状态量为：

其中，δp、δv、ψ分别为三维位置误差、速度误差、姿态误差，b_g、b_a分别为陀螺零偏和加表零偏。其中系统状态误差模型为：

δX_k,k-1＝Φ_k,k-1δX_k-1+Γ_k,k-1w_k-1

其中，w为系统噪声向量，Φ为状态转移矩阵，Γ表示系统噪声分配矩阵，下标k表示k时刻。线性化后的观测误差模型可表示为：

δZ_k＝H_kδX_k,k-1+v_k

其中，δZ_k为观测向量误差，H_k是观测矩阵，v_k为观测噪声。

系统状态误差模型是预测阶段，观测误差模型是滤波修正阶段，当没有观测值时，只有预测阶段，没有滤波修正阶段，观测模型右侧的δX_k,k-1与系统状态误差模型左侧的δX_k,k-1相同，均为误差状态量。观测误差模型是EKF模块的一部分，修正信息包括位置、速度、姿态以及传感器误差。

图4中的惯性传感器表示第一惯性导航模块，EKF表示扩展卡尔曼滤波(ExtendedKalman Filter)，陀螺仪输出的角速率和加速度计的比力(即第一观测值)，进行惯导机械编排，得到惯导位置、速度和姿态，由第一惯性导航模块y轴输出的角度速率得到车体的里程速度。其中，图4中的俯仰角为通过第二模块的俯仰角，该俯仰角和第一惯导模块的姿态角(惯导姿态)估计出第一惯性导航模块的安装角，然后进行扩展卡尔曼滤波，对车轮的位置、速度和姿态进行数据融合，获得修正后的位置、速度和姿态(即位置、速度和姿态的最优估计)，将其作为所述组合后的第一定位信息。同理，将第二惯性导航模块信息与第一惯性导航模块提供的速度信息进行组合导航解算可以得到第二定位信息。

步骤S105：基于组合后的第一定位信息、组合后的第二定位信息以及第一惯性模块和第二惯性模块的安装位置的空间关系，获得组合后的第三定位信息。

在一种实施方式中，步骤S105具体包括：

步骤S1051：构建第一惯性导航模块和第二惯性导航模块的安装位置的空间关系，空间关系由式(5)表示：

其中，d表示第一惯性导航模块与第二惯性导航模块之间距离，const为预设常数，x₁表示第一惯性导航模块的X轴坐标，y₁表示第一惯性导航模块Y轴坐标，z₁表示第一惯性导航模块Z轴坐标，x₂表示第二惯性导航模块的X轴坐标，y₂表示第二惯性导航模块的Y轴坐标，z₂表示第二惯性导航模块的Z轴坐标；

步骤S1052：对组合后的第一定位信息、组合后的第二定位信息进行加权平均，并基于安装位置的空间关系对第一惯性导航模块和第二惯性导航模块的位置进行约束，获得组合后的第三定位信息。

具体来说，d为两模块(第一惯性导航模块和第二惯性导航模块)间的距离，保持最初的安装距离不变的，为预设常数，(x,y,z)表示模块的物理位置的坐标。本实施例的定位方法中采用的双MEMS-IMU(Micro-Electro-Mechanical System-Inertial measurementunit)系统，包括两个惯性导航模块，可以利用其安装的位置关系对第一惯性导航模块和第二惯性导航模块进行位置约束，相较于单个车轮模块系统而言，可以显著提高定位的稳定性。安装位置关系是指两个模块间的物理位置关系，安装角则是指单个模块本身相对于水平面的状态是否完全水平。通过对组合后的第一定位信息和组合后的第二定位信息进行加权平均后，可以换得到第三定位信息，其中，第三定位信息包括最终得到的位置、速度、姿态信息。

在本发明实施例中，惯性器件信息(车轮的位置、速度和姿态)与里程信息(车轮的里程速度)均来源于同一器件，即安装在车轮中心的第一惯性导航模块，另一方面，为了进一步提高定位的准确性，第一惯性导航模块的中心与车轮的中心完全重合。第一惯性导航模块的中心与车轮轴心完全重合，做同轴周期旋转，结合陀螺与车轮平面垂直轴的输出和车轮半径，经过姿态变换亦可得到速度信息，并将惯导水平角与由加速度计输出的比力计算出的水平角进行加权融合，将两种速度(惯导速度和里程速度)组合，构造前向速度约束，从而得到位置、速度和姿态的最优估计，将其作为组合后的定位信息。

总体来说，本实施例的方法可以将组合后的第一定位信息和组合后第二定位信息进行加权平均，从而可以得到更为准确的定位信息，可以实现无里程计条件下的INS/Odo组合导航，使得定位结果在北向东向和垂向上均有明显提升，故而提高了室内定位的精度，且不需要安装外部传感器，大大降低了成本。解决了现有技术中导航精度较低的技术问题。

本申请实施例中的上述一个或多个技术方案，至少具有如下一种或多种技术效果：

在本发明提供的方法中，通过位于车轮的中心的第一惯性导航模块采集车轮的运动信息，并单独解算获得第一定位信息，通过位于车身的顶部中央的第二惯性导航模块测量采集车身的运动信息，并单独解算获得第二定位信息，然后通过第二惯性导航模块的俯仰角对第一惯性导航模块的俯仰角进行补偿，获得补偿后的俯仰角，使用补偿后的俯仰角和第一惯导模块的姿态角估计第一惯性导航模块的安装角，再通过第一惯性导航模块测量采集的车轮转动角速度，获得车体移动速度，将车体移动速度分别提供给第一惯性导航模块和第二惯性导航模块，并构建第一运动约束进行组合解算，获得组合后的第一定位信息，构建第二运动约束进行组合解算，获得组合后的第二定位信息，最后基于组合后的第一定位信息、组合后的第二定位信息以及第一惯性模块和第二惯性模块的安装位置的空间关系，获得组合后的第三定位信息。由于第一惯性导航模块和第二惯性导航模块构成双微机械惯性测量系统，可以通过第二惯性导航模块提供的俯仰角对第一惯性导航模块中的俯仰角进行补偿，从而可以实现了无里程计条件下的INS/Odo(Inertial Navigation System/Odometer)组合导航，使得定位结果在北向东向和垂向上均有明显提升，故而提高了室内定位的精度，且不需要安装外部传感器，降低了成本。解决了现有技术中导航精度较低的技术问题。

尽管已描述了本发明的优选实施例，但本领域内的技术人员一旦得知了基本创造性概念，则可对这些实施例作出另外的变更和修改。所以，所附权利要求意欲解释为包括优选实施例以及落入本发明范围的所有变更和修改。

显然，本领域的技术人员可以对本发明实施例进行各种改动和变型而不脱离本发明实施例的精神和范围。这样，倘若本发明实施例的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包含这些改动和变型在内。

Claims (9)

1.一种基于双MEMS-IMU的室内轮式机器人定位方法，其特征在于，所述室内轮式机器人包括车轮与车身，所述MEMS-IMU为微型惯性测量单元，所述微型惯性测量单元包括第一惯性导航模块与第二惯性导航模块，所述方法包括：

步骤S101：通过所述第一惯性导航模块采集所述车轮的运动信息，并单独解算获得第一定位信息，其中，所述第一惯性导航模块位于所述车轮的中心，所述第一定位信息包括所述车轮的位置、速度和姿态角；

步骤S102：通过所述第二惯性导航模块采集所述车身的运动信息，并单独解算获得第二定位信息，其中，所述第二惯性导航模块位于所述车身的顶部中央，所述第二定位信息包括所述车身的位置、速度和姿态角；

步骤S103：通过所述第二惯性导航模块的俯仰角对所述第一惯性导航模块的俯仰角进行补偿，获得补偿后的俯仰角，使用补偿后的俯仰角和所述第一惯性导航模块的姿态角估计所述第一惯性导航模块的安装角；

步骤S104：通过所述第一惯性导航模块测量采集的车轮转动角速度，获得车体移动速度，将车体移动速度分别提供给所述第一惯性导航模块和第二惯性导航模块，并构建第一运动约束进行组合解算，其中，第一运动约束为利用第一惯性导航模块进行定位时构建的约束，该约束用以保证车体在运动过程中不发生侧移和跳跃，获得组合后的第一定位信息，构建第二运动约束进行组合解算，获得组合后的第二定位信息，第二运动约束为在利用第二惯性导航模块进行定位时构建的约束，该约束用以保证车体在运动过程中不发生侧移和跳跃；

步骤S105：基于所述组合后的第一定位信息、所述组合后的第二定位信息以及所述第一惯性模块和第二惯性模块的安装位置的空间关系，获得组合后的第三定位信息。

2.如权利要求1所述的方法，其特征在于，步骤S101具体包括：

所述第一惯性导航模块输出第一观测值，将所述第一观测值用于惯性导航机械编排解算，获得所述第一定位信息，包括所述车轮的位置、速度和姿态角。

3.如权利要求1所述的方法，其特征在于，步骤S102具体包括：

所述第二惯性导航模块输出第二观测值，将所述第二观测值用于惯性导航机械编排解算，获得所述第二定位信息，包括所述车身的位置、速度和姿态角。

4.如权利要求1所述的方法，其特征在于，步骤S103具体包括：

步骤S1031：将所述第一惯性导航模块的安装角定义为从b系到v系的姿态差异，其中，所述b系为传感器坐标系，所述v系为载体坐标系，所述载体坐标系为所述轮式机器人坐标系，所述第一惯性导航模块的所述姿态差异的旋转矩阵为

所述姿态差异的欧拉角表示为:

其中，

表示所述第一惯性导航模块中，传感器坐标系到载体坐标系的横滚角、

表示所述第一惯性导航模块中，传感器坐标系到载体坐标系的俯仰角，

表示所述第一惯性导航模块中，传感器坐标系到载体坐标系的航向角；

步骤S1032：通过所述第二惯性导航模块的俯仰角对所述第一惯性导航模块的俯仰角进行补偿，获得所述补偿后的俯仰角

具体通过式(1)和式(2)获得：

其中，

表示所述第一惯性导航模块中，传感器坐标系到载体坐标系的横滚角，

表示所述第一惯性导航模块中，传感器坐标系到当地水平坐标系的横滚角，

表示所述第一惯性导航模块中，传感器坐标系到当地水平坐标系的俯仰角，

表示第二惯性导航模块中，传感器坐标系到当地水平坐标系的俯仰角，且基于预设关系，可以通过所述欧拉角获得相应的旋转矩阵

步骤S1033：采用补偿后的俯仰角和所述第一惯性导航模块的姿态角估计所述第一惯性导航模块的安装角。

5.如权利要求1所述的方法，其特征在于，所述第一惯性导航模块包括陀螺仪和加速度计，所述方法还包括：

构建所述室内轮式机器人的非完整性约束，所述室内轮式机器人的速度包括前进方向速度、横向速度和垂向速度，所述前进方向速度、所述横向速度和所述垂向速度构成的三维速度约束为所述非完整性约束：

v_wheel＝ω_y·r_w (4)

其中，v_wheel为前进方向速度，由陀螺仪的输出计算得到，横向速度和垂向速度均为0，ω_y为陀螺仪y轴角速率输出，r_w表示车轮半径。

6.如权利要求1所述的方法，其特征在于，所述第一惯性导航模块包括陀螺仪和加速度计，第二惯性导航模块包括陀螺仪和加速度计，第一观测值包括角速率和比力，所述第一观测值由第一惯性导航模块输出，第二观测值包括角速率和比力，所述第二观测值由第二惯性导航模块输出，步骤S104具体包括：

步骤S1041：将所述第一惯性导航模块测量的角速率和比力用于惯性导航机械编排解算，获得所述车轮的位置、速度和姿态；同时通过所述陀螺仪输出的角速率计算所述车体的里程速度，其中，所述角速率为垂直于所述车轮平面轴向的陀螺输出值，同时通过所述第一惯性导航模块的加速度计输出的比力计算水平角，其中所述比力为平行于所述车轮平面的两轴比力值，同时构建第一运动约束，其中所述第一运动约束为车体在运动过程中不发生侧移和跳跃；采用扩展卡尔曼滤波对所述车轮的位置、速度和姿态、所述里程速度、所述水平角和所述运动约束进行数据融合，获得位置、速度和姿态的最优估计，将其作为所述组合后的第一定位信息；

步骤S1042：将所述第二惯性导航模块测量的角速率和比力用于惯性导航机械编排解算，获得所述车体的位置、速度和姿态；同时接收所述第一惯性导航模块测量的角速率计算所述车体的里程速度，同时构建第二运动约束，其中所述第二运动约束为车体在运动过程中不发生侧移和跳跃；采用扩展卡尔曼滤波对所述车体的位置、速度和姿态、所述里程速度和所述运动约束进行数据融合，获得位置、速度和姿态的最优估计，将其作为所述组合后的第二定位信息。

7.如权利要求1所述的方法，其特征在于，步骤S105具体包括：

构建所述第一惯性导航模块和所述第二惯性导航模块的安装位置的空间关系，所述空间关系由式(5)表示：

其中，d表示第一惯性导航模块与第二惯性导航模块之间的距离，const为预设常数，x₁表示第一惯性导航模块的X轴坐标，y₁表示第一惯性导航模块Y轴坐标，z₁表示第一惯性导航模块Z轴坐标，x₂表示第二惯性导航模块的X轴坐标，y₂表示第二惯性导航模块的Y轴坐标，z₂表示第二惯性导航模块的Z轴坐标；

对所述组合后的第一定位信息、所述组合后的第二定位信息进行加权平均，并基于所述安装位置的空间关系对所述第一惯性导航模块和所述第二惯性导航模块的位置进行约束，获得组合后的第三定位信息。

8.如权利要求6所述的方法，其特征在于，步骤S1041具体包括：

基于所述扩展卡尔曼滤波对惯导速度与里程速度进行加权融合，基于所述扩展卡尔曼滤波对所述惯导水平角与所述水平角进行加权融合，获得所述车轮的位置、速度和姿态的最优估计，将其作为所述组合后的第一定位信息。

9.如权利要求1所述的方法，其特征在于，所述第一惯性导航模块的中心与所述车轮的中心完全重合。

Images