基于北斗/iNEMO惯性模块的机器人运动姿态解算方法

陈 凯，王广龙，王 成，薄洪波

(1.66008部队，天津 300250；2.军械工程学院 纳米技术与微系统实验室，石家庄 050003)

基于北斗/iNEMO惯性模块的机器人运动姿态解算方法

陈 凯1，王广龙2，王 成2，薄洪波1

(1.66008部队，天津 300250；2.军械工程学院 纳米技术与微系统实验室，石家庄 050003)

基于北斗/iNEMO惯性模块研究了一种组合式机器人运动姿态测量系统和解算方法;设计了以ARM为核心的嵌入式组合姿态解算平台，通过四元数法和卡尔曼滤波技术，对iNEMO惯性组件测量数据进行融合，进而在高动态环境中实时解算出机器人的运动姿态;同时，以北斗接收机输出的1PPS上升沿脉冲作为数据融合的同步信号，并利用其输出的导航信息辅助iNEMO惯性模块实现精对准;测试结果验证了设计方案的可行性和正确性，为机器人姿态解算提供了一种高精度、高稳定性、小体积、低功耗的解决方案。

姿态解算；机器人；惯性组件；北斗系统；初始对准

0 引言

移动机器人是一个集环境感知、动态决策与规划、行为控制与执行等多功能于一体的综合系统，是目前科学技术发展最活跃的领域之一[1]。它可以协助或取代人类的工作，已经广泛地应用于地质勘查、军事侦察、灾难救援等领域[2]。姿态检测作为机器人研究领域一个重要的组成部分，对于评定机器人的运动特性和姿态反馈控制具有非常重要的意义[3]。

目前，机器人的姿态检测方法按照接触方式分为间接检测法和直接检测法两种。间接检测法主要是使用超声波、激光或者CCD摄像机进行姿态测量，需要在机器人上安装多个外部检测元件，结构复杂，成本较高，不适合微小型机器人。直接检测法主要是利用陀螺仪、加速度计以及磁强计等惯性敏感元器件和计算机，实时测量运载体的空间位置、姿态和重力场等参数，惯性测量的好处是响应速度快、成本低，随着MEMS技术的发展和对功耗、体积的要求，低成本的微型惯性组件已经大量的在机器人的姿态测量中使用[4-6]。惯性系统在姿态解算前必须对其进行初始对准，对准的计算值将作为姿态解算的初始值，因此它的精度将直接影响姿态解算的精度。但低成本的惯性器件由于误差较大，即使在静基座下也无法准确辨识地球自转速率等重要信息，需要使用外部信息辅助动态对准[7]。

为解决对机器人运动姿态进行动态检测的问题，本文基于北斗/iNEMO惯性模块，设计了以ARM为核心的嵌入式组合姿态解算平台。在此基础上，对实时姿态解算和初始对准的算法进行了研究。最后通过试验，验证了硬件和算法设计的有效性。

1 硬件系统总体设计

硬件采用ARM为核心的组合姿态解算平台，可采集iNEMO惯性模块输出量，接收北斗模块输出信号，完成姿态数据的解算、融合处理和存储，系统结构如图1所示。其中，iNEMO惯性模块采用的是ST(意法半导体)公司的LSM9DS1芯片，这最一种基于MEMS工艺的9轴运动位置传感器模块，内置一个3轴加速度计、一个3轴陀螺仪和一个3轴磁强计。北斗接收机采用的和芯星通公司的UM220-III N芯片，是一种双系统高性能的GNSS 模块，能够同时支持 BD2 B1、GPS L1 两个频点。主处理器选用的是ST公司的ARM芯片STM32F405，有较强的数字信号处理能力和多任务管理能力，有较多的应用接口。为了避免处理频繁的响应中断，协处理单元由ARM芯片STM32F103和FPGA芯片XC6SLX75共同构成。整个系统以无线指令模块的高电平输出作为测试的启动指令。测试开始后，协处理器ARM从北斗接收机输出的NEMA0813信息中提取姿态解算所需的UTC时间、位置、速度、定位状态等信息并暂存入FPGA中；FPGA内包含一个双口RAM存储器和一个高精度的计数器，其中计数器以北斗接收机有效输出时的1PPS上升沿脉冲作为清零信号；主处理器ARM根据FPGA输出的上升沿信号产生中断，通过FSMC总线、两路SPI总线分别读取FPGA内的北斗导航信息、iNEMO惯性模块的加速度/角速度和地磁信息，利用这些信息完成姿态数据的解算，将结果存入FLASH芯片中。同时，为防止出现时序的竞争和冒险，并协调各个器件的工作，系统以有源晶振的输出作为唯一的频率基准。

图1 硬件系统总体框图

2 实时姿态解算方法

由于iNEMO惯性模块的输出数据速率远高于北斗接收机数据更新率，因此机器人姿态的实时解算主要针对iNEMO惯性模块(陀螺仪、加速度计、磁强计)的组合系统。其中，陀螺仪、加速度计、磁强计都能检测到载体的姿态角，但由于器件自身的局限性和外界环境的干扰，单一惯性器件无法实现对载体姿态角准确而完整的检测，因此必须将这几种惯性传感器的测量数据进行融合。本文实时姿态解算的总体思想是，由陀螺仪单独定姿、加速度计和磁强计组合定姿分别解算出姿态角，采用一维卡尔曼滤波对两类输出的三个方位的姿态角数据分别进行互补融合，进而得到更精确的姿态角，如图2所示。

图2 实时姿态解算原理

2.1 iNEMO陀螺仪解算欧拉角

(1)

式中，ψ∈[-π,π]为方位角， θ∈[-π/2,π/2]为俯仰角，γ∈[-π,π]为横滚角，这三个角称为载体的欧拉角，用于表示载体的运动姿态，此处采用四元数法求解欧拉角。

设载体坐标系相对于导航坐标系的转动四元数为：

(2)

iNEMO陀螺仪x、y、z轴的角速度输出为(ωx、ωy、ωz)，采样周期为T，采用一阶龙格-库塔法更新四元数：

(3)

(4)

若用四元数Q表示载体坐标系相对于导航坐标系的变换，即：

(5)

(6)

2.2iNEMO加速度计、磁强计解算欧拉角

iNEMO加速度计x、y、z轴的线性加速度输出为(ax、ay、az)，则可估算俯仰角θ2、横滚角γ2：

(7)

(8)

iNEMO磁强计x、y、z轴的磁场强度输出为(mx、my、mz)，水平状态下可以估算出方位角的主值：

(9)

机器人在运动过程中由于颠簸或者上下坡度而不会总处于水平位置上，此处采用几何旋转法，通过姿态矩阵建立载体坐标系下iNEMO惯性模块测得的重力矢量和磁场强度矢量的对应方程，用加速计估算出的俯仰角θ、横滚角γ的值对磁强计输出的进行倾角补偿：

(10)

(11)

由于地球的地理北向与磁北方向之间存在一个磁偏角，经过磁偏角修正，就可以得到载体真实的方位角ψ2。

iNEMO加速度计和磁强度计的估算值都通过泰勒级数的展开完成解算。

2.3 卡尔曼滤波的姿态数据融合

由于陀螺仪存在温度漂移，积分运算的累积误差会迅速放大，因此陀螺仪解算出的欧拉角必须进一步融合加速度计和磁强计的数据。以方位角为例， 陀螺仪解算出ψ1、加速度和磁强计组合解算出ψ2，对应的方差δ1和δ2。为减小计算量，采用一维卡尔曼(Kalman)滤波器，方位角的最佳估计值ψ为[8]：

(12)

其方差为：

(13)

3 北斗辅助iNEMO惯性模块精对准

机器人姿态解算时，首先要对系统进行初始化，完成测量平台的导航数据初始对准工作，消除惯性模块的初始误差，并为姿态解算赋予初始值，为解算的迭代过程打下基础。初始对准即在最短的时间内以一定的精度确定从载体坐标系到导航坐标系的初始姿态变换矩阵[13]。

对准分两步进行，粗对准和精对准。在粗对准阶段，将iNEMO加速度计、磁强计输出估算的欧拉角代入方程(1)，即可得到粗对准阶段的初始姿态矩阵。粗对准可以缩短对准的时间，但对准的精度较低，需要精对准进一步提高对准精度。在精对准阶段，本文采用组合对准的方式，应用拓展卡尔曼滤波方法，从惯导参数相对于北斗导航系统提供的参数的偏差中估计出惯性模块的误差量，并进行校正，用实时更新的姿态矩阵替代粗对准阶段得到的姿态矩阵。

为有效地提高计算精度和效率，此处直接采用北斗系统参照的地球坐标系(e系)实现对准。将北斗的导航解直接作为卡尔曼滤波器的输入，这是一种松组合的数据融合方法，因而其数学模型并不需要包含北斗状态方程。由三轴位置误差δre、速度误差δve、姿态误差φ，惯性模块三轴加速度计偏置误差▽b、陀螺仪漂移εb共同组成15维的系统状态向量：

(16)

设理想的地球坐标系为e系(简称地球系)，计算的地球坐标系为c系(简称计算系)。地球系的位置误差由于不涉及坐标系统转换，其微分形式即是速度误差，比较简单。但惯性元件存在测量误差会导致两个坐标系之间存在一个转动误差，即平台欧拉误差角，需要予以考虑。短时间内，三轴陀螺仪漂移、加速度计误差可认为为常值，不随时间变化，其微分为0。在地球坐标系下，任意失准角情况的系统状态误差微分形式为[7]：

(17)

则系统的状态方程可写为：

(18)

式中，F(15×15)为系统的状态转移矩阵，G为系统的动态噪声矩阵，W为系统的过程白噪声矢量。

在惯性模块与北斗的松组合模式中，一般取速度和位置为观测信息。但在静止与匀速运动状态下，系统的方位角误差不可观测。因此，本系统在选取北斗和惯性模块中输出的位置和速度之差的基础上，将惯性模块中陀螺仪与磁强计输出的方位角之差作为观测量，构造量测方程：

(19)

式中，rI和vI为惯性模块输出的载体3维速度和位置信息，rB、vB为北斗接收机输出的载体3维速度和位置信息，ψ1、ψ2分别为惯性模块中陀螺仪与磁强计输出的方位角。H为量测转移矩阵，V为量测白噪声矩阵。

将状态方程(18)和量测方程(19)离散化， 即可利用拓展卡尔曼滤波，通过时间更新和状态更新，反馈修正姿态信息，完成姿态数据的初始对准。

4 试验测试及结果分析

4.1 基于Kalman滤波的姿态信息融合数据分析

使用微系统中的iNEMO惯性模块进行实时姿态测量实验，陀螺仪和加速度计采样频率为100 Hz，磁强计的采用频率为10 Hz，将两种姿态测量结果进行Kalman滤波信息融合，得到的姿态信息如图3所示。其中，上图为陀螺仪输出的欧拉角，中图为加速度计和磁强计组合输出的欧拉角，下图为运用经过Kalman滤波后输出的欧拉角，γ、θ、ψ分别表示系统的横滚角、俯仰角和方位角。由图中可以看出，经过Kalman滤波信息融合，对两种方式测得的姿态误差进行了补偿，经滤波得到的结果较为平滑且与系统真实的姿态变化相吻合。

图3 实时姿态解算Kalman滤波结果

4.2 系统初始对准精度分析

初始对准在静基座条件下进行。实验中，惯组模块陀螺仪和加速度计采样频率为100 Hz、磁强计的采用频率为10 Hz，北斗接收机的数据更新率为1 Hz。采用惯组输出进行粗对准，得到姿态角误差如图4所示。北斗辅助iNEMO惯性模块进行精对准，得到姿态角误差如图5所示。表1列出了两种对准方式下的姿态角误差的均值和标准差。从测试结果可以看出，粗对准中俯仰角误差和方位角误差较大，误差均值在0.7°左右，精对准后，输出的姿态角误差的均值和标准差均变小，得到的

图4 粗对准时静基座的姿态角误差

图5 精对准时静基座的姿态角误差

方位角和俯仰角的误差控制在±0.1°以内，横滚角误差在±0.15°以内，表明基于上述模型的初始对准很好的满足了姿态解算精度的要求。

表1 静态测试时的姿态角误差结果

5 结论

本文设计了一种移动机器人的姿态测量系统，可以完成对机器人运动姿态的实时采集和存储。研究了对iNEMO惯性模块测量数据进行融合的方法，可以较精确地实时解算出姿态数据。利用北斗输出的导航信息辅助iNEMO惯性模块实现精对准，能够有效的改善系统的性能。软硬件的测试结果验证了设计方案的可行性和正确性，该方法方便快捷、功耗低、试验范围不受限制，并且用来支持试验的地面设备少，节约经费，设备可重复使用，能够满足机器人姿态解算所要求的高精度、稳定性，具有很强的通用性和可拓展性。

[1] 徐国保,尹怡欣, 周美娟.智能移动机器人技术现状及展望[J]. 机器人技术与应用,2007(2):29-34.

[2] Hua M D, Ducard G, Hamel T, et al. Introduction to Nonlinear Attitude Estimation for Aerial Robotic Systems[J]. Aerospace Lab, 2014: AL08-04.

[3] 任文强.多面体机器人姿态检测技术研究[D].北京:北京交通大学,2016.

[4] 秦 勇,臧希喆,王晓宇等.基于MEMS 惯性传感器的机器人姿态检测系统的研究[J].传感技术学报,2007,20(2):298-301.

[5] Renaudin V, Combettes C. Magnetic, Acceleration Fields and Gyroscope Quaternion (MAGYQ)-Based Attitude Estimation with Smartphone Sensors for Indoor Pedestrian Navigation[J]. Sensors, 2014, 14(12): 22864-22890.

[6] Di L, Fromm T, Chen Y Q. A Data Fusion System for Attitude Estimation of Low-cost Miniature UAVs[J]. Journal of Intelligent & Robotic Systems, 2012,65(1-4): 621-635.

[7] 张秋昭, 张书毕, 王 坚,等.露天矿卡车低成本GPS/INS组合导航系统动态对准模型[J].煤炭学报,2013,38(8):1362-1367.

[8] 姜 兰.超长航时无人机持久组合导航系统设计[D].南京:南京理工大学,2014.

[9] 杨亚非,谭久彬,邓正隆.惯导系统初始对准技术综述[J]. 中国惯性技术学报,2002,10(2):68-72.

Method of Robot Attitude Calculation Based on Beidou/ iNEMO Inertial Module

Chen Kai1, Wang Guanglong2,Wang Cheng2,Bo Hongbo1

(1.No.66008 Unit of PLA, Tianjin 300250, China; 2.Laboratory of Nanotechnology and Microsystems,Ordnance Engineering College, Shijiazhuang 050003, China)

This paper researches a combined robot posture measurement system and attitude algorithm based on Beidou/iNEMO. Here designed an embedded combination attitude calculation platform cored by ARM, to calculate posture of robot in real-time under high dynamic condition, by Quaternion and Kalman filter technology fusing measured data of iNEMO inertial measurement unit. Meanwhile, using the Beidou receiver outputted 1pps rising edge pulse as synchronous time of subsequent data fusion, as well as its outputted navigation data aid iNEMO inertial measurement unit, fine alignment can be realized. The feasibility and correctness of the designed test has been verified, it provides robot attitude calculation with high precision, high stability, small volume, low power consumption.

attitude calculation; inertial measurement unit(IMU); robot; BeiDou system; initial alignment

2016-04-07；

2016-06-21。

国家自然科学青年基金(61501493)。

陈 凯(1983-)，男，重庆荣昌人，硕士研究生，工程师，主要从事测试计量技术及仪器的应用研究。

1671-4598(2017)02-0130-04

10.16526/j.cnki.11-4762/tp.2017.02.036

TP212.9

A