基于光电鼠标与惯性导航数据融合的定位系统*

房海基 高向东 叶国良 郭建文 杨宇辉

(①广东工业大学机电工程学院，广东 广州 510006; ②东莞理工学院机械工程学院，广东 东莞 523000)

超声检测具有穿透能力强、灵敏度高、设备轻便及对人体无害的特点，是应用最广泛且发展最快的无损检测技术之一，能完成对金属、非金属和复合材料等的宏观缺陷、几何特性及力学变化等方面的检测。在超声无损检测人工操作超声探头过程中面临着无法自动定位和记录检测点位置而导致错检漏检的问题，目前的定位方法有利用传动装置与编码器进行定位精度控制，操作数控工作台或者机械手获取实时的扫查位置，结合超声检测信号和成像技术，得到和实际的缺陷分布有一一对应关系的图像，定位精度较高，但这也存在检测系统成本提高和灵活性降低的问题[1-2]。文献[3]针对手持超声成像，提出一种在光学定位条件下基于立体标定模板的超声探头标定方法，能同时采集超声图像和探头在摄像机中的图像，求解两者之间的空间变换关系，在标定图像数量较少时也有较高的精度。参考移动机器人的室内定位方法，惯性导航定位是更加灵活、简便的自主定位方法，它利用运动传感器不断从外界获取物体的位置、姿态信息，通过数据融合算法，更新实时位姿。文献[4]提出利用加速度传感器和陀螺仪数据推算超声检测过程的运动轨迹，并通过姿态转换和卡尔曼滤波修正运动信息。但在推算位移时要解决加速度传感器零点漂移的积累误差，而且在实际的惯性导航应用中系统噪声模型往往是时变的，卡尔曼滤波数据融合存在局限性。文献[5]在系统噪声统计特性为时变时，提出在拓展卡尔曼滤波基础上加入自适应过程，能抑制滤波发散现象，算法稳定性和实时性更好。针对积累误差问题，文献[6]提出光电鼠标里程计获取位移信息，而不是通过不断获取加速度数据计算位移。光电鼠标能够直接记录运动过程，价格低廉且能在航迹推算中消除大部分误差，精度很高。

针对上述众多问题，本文提出利用光电鼠标位移传感器与惯性测量单元(internal measurement unit，IMU)组合导航的定位方法，经过自适应卡尔曼滤波算法融合，得到定位系统的姿态角最优估计，结合光电鼠标输出的位置坐标解算出定位点的世界坐标。设备简易轻便，有望能够快速实现超声无损检测过程中超声探头的准确定位。

1 定位方法

本文研究的基于光电鼠标和惯性导航数据融合的定位系统是以超声探头为目标载体，在传统手动操作超声探头进行无损检测的基础上，利用光电鼠标输出的二维坐标值和惯性测量单元的航推位置信息，达到实时位置标定的目的。光电鼠标获取移动过程中的鼠标坐标系下的坐标，IMU获取实时的运动姿态信息，包括加速度、角速度、磁分量等。由于陀螺仪角速度数据积分过程中存在漂移现象，时间越长就会产生越大的定位误差，利用地磁计数据补偿加速度数据解算出姿态角，经过自适应卡尔曼滤波算法，能够得到更为准确的更新后的姿态角，提高定位精度与可靠度。最后根据光电鼠标、IMU与超声探头之间的运动学几何关系，利用光电鼠标的输出坐标和姿态角变化解算出世界坐标系下超声探头的位姿，正确标定超声探头检测实时位置。如图1所示为系统整体架构。

1.1 地磁计和加速度计姿态更新算法

当运动物体处在静止或匀速运动状态时，物体只受当前状态下的重力加速度影响。导航坐标系(n系)和载体坐标系(b系)下的加速度关系为：

式中：ab为加速度计输出[ax,ay,az]T，an为重力矢量[0,0,g]T，方向余弦矩阵

可以算出运动物体的俯仰角和横滚角：

(1)

运动物体在n系下的磁感应强度为

(2)

式中：φm为相对于磁北的航向角，Δφ为真北和磁北的磁偏角。

当运动过程中不能保证始终与水平面平行时，需要旋转roll和pitch角，把地磁计的数据转换到水平面，即

(3)

超声无损检测过程一般会保持在水平面，只有航向角变化，那么就只需要根据式(2)地磁计水平方向上磁分量求得航向角。

1.2 陀螺仪角度数据融合算法

加速度计和地磁计可以测得3个姿态角的关系，但容易受到外部环境的干扰，其动态响应较慢，长时间内测量值比较准确。陀螺仪可以积分得到3个姿态角的关系，且不容易受到外部环境的干扰，动态响应较快，短时间内测量值比较准确，但长时间积分会造成累积误差[7]。所以要根据加速度计、陀螺仪和地磁计数据进行陀螺仪角度数据融合，得到目标载体的姿态角最优估计。

在实际的惯性导航应用中，系统噪声的噪声方差Q和观测噪声误差R往往是未知的或者是变动的，随着时间推移，将会使估计误差超出理想滤波范围，甚至产生发散现象[8]。所以可以使用自适应卡尔曼滤波算法，在卡尔曼滤波的基础上，加入自适应过程，利用量测数据自动更新时变噪声参数Q和R，提高滤波过程的精确度，得到最优估计。

Sage-Husa卡尔曼滤波算法是Sage和Husa提出的一种自适应滤波算法，可以在线计算系统噪声和量测噪声，减少由模型误差带来的影响[9]。在此基础上引入遗忘因子，逐步发展出简化Sage-Husa算法以及改进Sage-Husa卡尔曼滤波算法。研究者通过噪声模型的统计特性分析发现当系统噪声和观测噪声都未知的情况下，并不能同时把它们估计出来，必须要有一个已知项。通常认为组合导航定位模型下，系统噪声具有稳定性而只考虑观测测噪声的更新[10]。与传统卡尔曼滤波相比，Sage-Husa自适应卡尔曼滤波具有更好的实时性、适应性和鲁棒性，能够提高滤波精度，并且得到了广泛应用。

首先建立系统状态方程和量测方程：

(4)

式中：Xk为k时刻的状态向量：A为状态转移矩阵；B为控制矩阵；H为量测矩阵；U为输入控制向量，在该模型中为陀螺仪输出的角速度；Zk为k时刻的量测向量，在该模型中为加速度计与地磁计测量的航向角；wk和vk分别为系统过程噪声和量测噪声。

具体的算法流程如下：

①k时刻状态一步预测方程：

②一步预测协方差方程：

③滤波增益方程：

⑥k时刻协方差：Pk=(I-KkHk)Pk|k-1

⑧权重系数：dk=(1-b)/(1-bk+1) 0

其中，b为遗忘因子，取值范围[0.95,0.99]。遗忘因子通过改变加权系数，更多地接受新数据对滤波的影响，并使滤波过程保持在理想误差范围内。

1.3 光电鼠标与IMU定位运动学分析

光电鼠标是基于光学导航技术，通过光学反射获取连续的表面图像，利用数学关系确定运动的方向和幅度来测量位置的变化[11]。影响光电鼠标定位精度的要素有两个：分辨率和刷新率。

(1)当一个光电鼠标的分辨率固定时，其分辨率越高，移动相同物理距离时获得更多的坐标数量，位置信息更精确。

(2)刷新率是指单位时间内采集和处理图像的数量，刷新率越高则CMOS成像信息也越连贯，通过信号处理器处理得到的移动距离和方向也越准确，能处理到更为微小的移动。

通过每一帧图像的像素分析，计算出位移增量，但这个增量是基于鼠标坐标系的绝对位移，而非基于某一个确定的世界坐标系的绝对位移。本文的定位方法是要得到世界坐标系下超声探头的绝对位置坐标，所以需要对鼠标的输出进行坐标变换。

如图2为传感器坐标关系图[12]。只考虑二维平面直角坐标系，对光电鼠标、IMU和超声探头的检测过程进行建模分析。定义OwXwYw为世界坐标系，OtXtYt为超声探头坐标系，OsXsYs为鼠标坐标系。其中Ot和Os分别为超声探头和鼠标传感器的几何中心，二者之间的安装距离为L;超声探头坐标系与世界坐标系的夹角为θ，可以认为是每次旋转运动时的旋转角;鼠标坐标系与超声探头坐标系的夹角φ为安装偏差角，L与Xt方向的夹角为ψ。

在世界坐标系下，光电鼠标和超声探头的相对位置满足

(5)

(6)

(7)

即

上式是在鼠标和超声探头存在安装距离和安装角情况下的定位公式。假设超声探头坐标系与鼠标坐标系的原点重合，即L=0且ψ=0，但安装角φ存在，根据光电鼠标和IMU的输出，在世界坐标系下，超声探头的定位公式为

(8)

根据上式，θi通常会随旋转运动而改变，而安装角偏差φ为固定值。为了求出此安装偏差角，可以在鼠标移动过程中保持旋转角θi=0恒定，通过实验能够拟合出此偏差角：

(9)

2 实验与结果

2.1 光电鼠标与惯性测量单元的软硬件开发与实验装置设计

本次实验主要分为光电鼠标和惯性导航两部分的数据采集与分析。前者硬件平台主要包括Arduino UNO、Arduino USB Host Shield和光电鼠标。利用Arduino拓展板让Arduino UNO设备连接一个USB光电鼠标，通过Arduino实时获取鼠标移动过程中输出的像素坐标，并转换为真实的物理坐标封装起来，方便后续处理。这里需要安装Arduino USB Host Shield2.0和ThreadController开发库。后者硬件平台为IMU模块，该模块的核心为MPU6050传感器，集成了高精度的陀螺仪、加速度计、地磁计，能够快速解算出所需的实时加速度、角速度和磁分量数据。在实验前需在Arduino上测试鼠标输出坐标的准确性，在IMU自带调试软件中设置回传速率、波特率、输出物理量等参数。如图3所示为硬件连接框图，图4为实验装置图。

本次实验光电鼠标的分辨率为1 200 DPI，设置数据回传速率100 Hz；IMU设置的主要参数有回传速率100 Hz、设备地址0x50、波特率115 200 bit/s等。预制一个116 mm×96 mm的矩形环，保证当鼠标在环内运动时有准确的世界坐标。在保证Arduino输出的鼠标数据和IMU输出的姿态数据准确的前提下，根据串口通信协议，分析设备地址、数据传输方式和计算公式，通过MATLAB软件编写上述两个传感器的数据采集接口，使两个传感器能保持通信同步，得到的位置坐标更精确。

2.2 角度数据融合结果与分析

利用上述的实验装置，在移动过程中保持与水平面平行，不间断地改变航向角的方向，通过MATLAB建立的数据采集接口统一采集保存为离线数据，利用 MATLAB软件编写的Sage-Husa自适应卡尔曼滤波算法程序对其进行数据融合来获取准确姿态角。

图5是传感器在移动过程中通过角速度积分和使用Sage-Husa自适应卡尔曼滤波算法得到的航向角曲线。图6是图5中角速度积分与自适应卡尔曼滤波数据融合的航向角差值曲线。选取的遗忘因子数值为0.95，由图5和图6可以看出，陀螺仪积分得到的航向角在35 s左右发生了0.3°的漂移，且在移动过程中存在着累积误差，特别是在运动方向发生变化时，产生的误差更明显。经过自适应卡尔曼滤波算法进行数据融合，既能很好地消除陀螺仪的积分飘移，也能明显地降低数据的噪声影响，使估计的姿态角更平滑。

2.3 定位结果与分析

通过上述的实验测试，在预制的矩形环内移动，对采集的鼠标坐标和角度离线数据进行坐标变换处理，通过MATLAB软件编程绘制了预定轨迹、坐标转换前后的运动轨迹对比图，如图7所示。图8是鼠标在矩形环内移动时经过自适应卡尔曼滤波的航向角变化图。从运动轨迹上看，坐标转换前鼠标的输出坐标仅仅是相对于鼠标坐标系的绝对坐标，其轨迹并不能反映实际运动轨迹。结合每个数据点的角度变化和鼠标输出的坐标增量进行坐标变换，得到解算后的世界坐标系下的运动轨迹与预定轨迹重合度很高，有少许的波动，但能更好地反映检测点的实际位置。

表1 预定轨迹移动解算坐标与真实坐标结果对比

具体分析，表1是在预定轨迹中移动的四个定位点的解算坐标和真实坐标结果对比。起始点为(0，0)，沿图8中1-2-3-4顺序移动，从解算轨迹和坐标可以看出，在整个过程中x轴方向的位置偏差较小，而y轴方向特别是1-2过程的数据缺失出现了0.132 cm的位置偏差，在3-4过程虽然y轴方向并未出现数据波动，但由于前面检测过程的位置偏差的累积，最终定位点4还是有0.179 cm的位置偏差。出现数据偏离的原因可能是实验装置在实际移动过程中受到振动或冲击产生了误差，特别是光电鼠标，在光学反射过程中可能存在丢失帧的情况，导致数据的缺失，从而影响定位效果。

3 结语

本文为解决超声波无损检测人工操作超声探头时无法自动标定位置而导致错检漏检的问题，提出利用光电鼠标与惯性测量单元组合导航的定位方法。通过自适应卡尔曼滤波算法，经过地磁计的补偿，消除了陀螺仪的积分漂移，获得姿态角的最优估计。根据传感器运动学几何关系，利用鼠标输出数据和姿态角变化解算出超声探头在世界坐标下的绝对位置，能实现检测过程的准确定位。但在定位结果对比中出现数据缺失现象导致细微地偏离预定轨迹，说明在实验装置设计制作或者位置估计中存在不足，今后将在这两个方面继续深入研究，实现设备简易轻便、检测过程快速精确且实时位置标定的目标。