基于小二乘－卡尔曼滤波的wMPS系统跟踪定位算法研究*

端木琼，杨学友，邾继贵

(天津大学精密测试技术及仪器国家重点实验室，天津300072)

随着现代装备制造业、空间技术和机器人技术的发展，静态测量已越来越不能满足部分现代工业的要求，一些航空、航天及高精密制造企业的客户希望对其生产或使用的大型工件以及整套系统进行动态测量。在工业测量领域，动态测量的主要方法有激光跟踪仪测量技术以及摄影测量技术，但是这些方法都有其局限性。摄影测量需要通视条件，视场较小，而激光跟踪仪只能实现单点跟踪测量，姿态测量则需增加其它附件，在环境复杂的现场条件下容易受到干扰［1］。wMPS是一种基于光电扫描的网络式大尺寸测量系统，与其它系统相比，该系统具有测量精度高、并行测量能力强、抗干扰能力强等特点。本文在静态测量的基础［2－6］上对系统的动态测量原理进行了研究，将静态测量与卡尔曼滤波相结合，实现了动态过程中的精密定位，减小了运动导致的测量误差，对大型设备装配过程中的跟踪定位测量具有重要意义。

1 wMPS系统组成

wMPS三维定位系统主要由激光发射器网络、位置传感器、中心计算机和无线通讯系统组成［2］。激光发射器由固定基座和转动头组成，安装有两个一字线激光器和一个脉冲激光器，两个一字线激光器固定于转动头上，激光器产生的光平面分别与垂直方向呈±30°，呈V字形。当发射器工作时，激光器所产生的光平面随转动头一同旋转，对测量空间进行扫描。脉冲激光器用于产生一个计时同步时刻，以该时刻光平面1与发射器水平面之间的交线为发射器X正方向，旋转轴为Z方向，按右手定则确定Y方向。当系统工作时，在工件装配的特定工作空间中的不同区域放置多个发射器(Transmitter)，接收器(Receiver)安装在工件的关键点上，由于发射器与接收器之间的光信号通信是单向广播式(One-Way Broadcast-Style)的，因此多个接收器可以共用这些发射器信号，如图1所示。传感器通过计算与两个(以上)发射器的方位角，通过空间交会原理即可得到传感器的空间三维坐标［2－4］。

图1 wMPS三维坐标系统组成

2 基于最小二乘原理的静态定位方法

wMPS系统的静态定位通常采用基于多面交会的最小二乘估计方法［7－9］。如图2(a)所示，发射器可以抽象为围绕旋转轴以角速度ω旋转的两个光平面。以初始时刻，光平面1与发射器水平面之间的交线为发射器X正方向，旋转轴为Z方向，按右手定则确定Y方向。转台位于初始位置时，对两个光平面进行标定，得到两个光平面的方程为:

即

图2 wMPS系统测量原理

若被测点P的坐标为(x，y，z)，则可得方程组

对于多个发射器，将其方程组写成矩阵形式可得

其最小二乘解为

利用最小二乘法解算接收器的坐标只利用了当前的观测量，不能对观测量进行误差分析，因此解算结果受观测量影响较大，当观测量误差较大或接收器运动时，解算结果精度不高，但最小二乘法收敛速度很快，受初始位置误差影响较小。

3 基于最小二乘－卡尔曼滤波的动态测量方法

如果直接利用接收器测得的与多个发射器之间的方位角信息进行卡尔曼滤波跟踪定位，不可避免要解决非线性估计问题，这将使算法复杂，同时引入非线性误差，降低解算精度。此外，随着观测量维数的增加，计算量也大幅增加，因此我们利用静态坐标解算的方法对接收器坐标进行估计，将其结果作为伪测量值，然后在利用匀速运动模型进行线性卡尔曼滤波，实现高精度定位跟踪［10－12］。

3．1 运动方程和测量方程

在数字化装配过程中，工件所做的多为匀速运动、匀加速运动以及低速转弯等机动性较弱的运动，因此选用匀速运动模型建立运动方程，将加速度作为运动噪声。设状态变量 X=［x，y，z，vx，vy，vz］T，其中 x，y，z分别为接收器的空间三维坐标，vx，vy，vz分别为接收器运动速度在x，y，z方向上的分量，采样间隔为T。状态转移方程为:

其中，状态转移矩阵、状态转移噪声矩阵分别为

状态转移噪声向量为

同时，状态转移噪声满足

系统的观测量 分别为接收器的坐标(x，y，z)，观测方程为

其中，观测矩阵为

观测噪声满足

3．2 线性卡尔曼滤波

采用最小二乘计算所得的坐标作为伪观测量，系统转移方程和观测方程均为线性，采用卡尔曼滤波对测量结果进行最优化估计。卡尔曼滤波步骤如下:

(1)计算状态变量及其协方差初值。k=1，k=2时，直接利用观测值Z1，Z2通过最小二乘原理计算出接收器的位置分别为(x1，y1，z1)和(x2，y2，z2)。求出系统状态变量初始值为

(2)利用状态转移方程及前一步的状态变量对当前状态进行最优估值。当k≥3时，系统状态变量Xk|k－1和方差 Pk|k－1的先验估计为:

(3)通过观测量对估值进行修正。残余增益K及后验估计Xk|k和后验方差Pk|k分别为:

(4)将后验估计Xk|k和Pk|k作为初值代入第2步，再次进行滤波。

4 试验验证

为了验证最小二乘－卡尔曼定位跟踪方法的有效性，并与静态跟踪方法进行比较，本文在精密测试技术及仪器国家重点实验研制的wMPS原型平台上进行了验证，试验平台如图3所示。

图3 wMPS系统实验平台

试验采用了四个发射站，分别布置在工作区域的四个角上，工作区域约为8 m×6 m×2 m，测量前利用Lecia激光跟踪仪作为基准对wMPS系统进行了标定，标定方法与文献［7］相同。接收器在测量空间中分别沿x、y、z方向做匀速移动，移动速度分别为 0．05 m/s和 0．2 m/s，测量采样频率为 20 Hz。

图4给出了接收器以速度0．05 m/s做匀速直线运动时利用四个发射器数据跟踪估值曲线。在测量数据质量较好，数据冗余较多的情况下，最小二乘－卡尔曼滤波法的测量误差小于±2 mm，效果远好于最小二乘法估值。

图4 0．05 m/s匀速直线运动时估值误差对比

图5给出了接收器以速度0．2 m/s匀速直线运动时的测量结果。测量结果表明，随着运动速度的加快，两种估值方法的测量误差都有所增大，最小二乘－卡尔曼滤波法的估值误差增加至±3 mm，而最小二乘法的估值误差增大到±10 mm。

图5 0．2 m/s匀速直线运动时估值误差对比

5 结论

本文将卡尔曼滤波引入到了wMPS系统的动态跟踪中来，并与静态坐标估算想结合，克服了采用扫描角进行滤波带来的非线性误差及计算量增大的问题。试验结果表明，最小二乘－卡尔曼滤波法在测量误差较大、可利用发射站数量较少的情况下仍然能够较好的跟踪目标，跟踪精度始终好于2 mm，能够满足工业数字化装配的应用，具有良好的应用前景。

［1］ 张靓，李明，郁菁菁，等．物体空间动态测量方法的研究［J］．机械制造，2007，45(12):54－56．

［2］ 端木琼，杨学友，邾继贵，等．基于光电扫描的网络式大尺寸测量系统定位算法研究［J］．传感技术学报，2011，24(9):1－5．

［3］ Yang Linghui，Yang Xueyou，Zhu Jigui，et al，Novel Method for Spatial Angle Measurement Based on Rotating Planar Laser Beams［J］．Chinese Journal of Mechanical engineering，2010，23(6):758－764．

［4］ 杨凌辉，杨学友，劳达宝，等．采用光平面交汇的大尺寸坐标测量方法［J］．红外与激光工程，2010，39(6):1829－1833．

［5］ 劳达宝，杨学友，邾继贵，等．网络式激光扫描空间定位系统标定技术研究［J］．机械工程学报，2011，47(6):1829－1833．

［6］ 杨凌辉，杨学友，邾继贵，等．基于光电扫描的工作空间测量定位系统误差分析［J］．光电子·激光，2010，21(12):1829－1833．

［7］ 邾继贵，王鑫，王大为，等．光学坐标测量系统技术研究［J］．传感技术学报，2007，20(4):778－780．

［8］ 张滋黎，邾继贵，耿娜，等．双经纬仪三维坐标测量系统设计［J］．传感技术学报，2010，23(5):660－664．

［9］ 李广云．非正交系坐标测量系统原理及进展［J］．测绘信息与工程，2003，28(1):4－10．

［10］邱玲，沈振康．三维纯角度被动跟踪定位的最小二乘－卡尔曼滤波算法［J］．红外与激光工程，2001，30(2):83－86．

［11］宋骊平，姬红兵，高新波．多站测角的机动目标最小二乘自适应跟踪算法［J］．电子与信息学报，2005，27(5):793－796．

［12］宋骊平，姬红兵．多站测角的最小二乘交互多模型跟踪算法［J］．西安电子科技大学学报，2008，35(2):242－247．