GPS与INS组合的便携式轨道类游乐设施轨迹研究

黄辉琼， 项辉宇， 孙九增

(北京工商大学 人工智能学院， 北京 100048)

0 引 言

过山车等轨道游乐设施一般具有轨道蜿蜒曲折、行驶姿态多变的特征，而且由于受起落高度、载客人数等因素影响，其运行轨迹和乘客姿态存在一定差异，增加了设计评价和安全评估的难度。目前，关于轨道类游乐设施轨迹检测方面的研究成果较少，通过实测获取其运行过程空间位置和姿态信息的技术手段不足[1]。因此开发一种便携式适应现场测试评估需求的轨道类游乐设施姿态与轨迹测量装置十分必要。

安卓智能手机内部搭载多种传感器，虽然其中的惯性导航系统(Inertial Navigation System，INS)具有抗干扰性强、可靠性高、自主性好等特点，但往往会存在累积误差，单独使用难以提供高精度的位姿和速度信息；而将其与手机自带的GPS(Global Positioning System)导航组合使用，由于GPS误差不在时间上累积，通过卡尔曼滤波器实现INS与GPS组合，能够有效克服惯导系统自身不足，同时矫正惯导系统累积误差，提高系统准确度，获取速度、位置的最优估计[2～3]。

目前，INS与GPS组合使用有多种方式。采用卡尔曼滤波作为组合算法，通过状态方程及量测方程，以前一个数据参量，推测当前应得数据，通过以当前所测数据参量为校正量进行修正，从而获得数据参量得最佳估算[4]。为收集试验所需的INS、GPS数据，开发Android客户端采集相关数据，在Matlab软件环境下搭建算法平台，对数据进行处理，在试验场地采集数据，将处理完成的数据导入UG，编写拟合程序二次开发，拟合游乐设施运行轨迹，实现可视化，验证方法的可用性及有效性。

1 惯性导航系统

1.1 惯导系统原理

采用捷联惯导系统，系统原理如图1所示。惯性元件安装在运载体上，通过导航计算机进行运算，经过求解加速度计的线运动数据、陀螺仪的角运动数据，计算姿态转换矩阵；进行坐标转换，载体坐标系—oxbybzb固定于运载体上，原点位于运载体重心上，xb轴、yb轴、zb轴分别指向载体前进正右方、载体前进正前方、载体前进正上方，载体运动状态需要通过导航坐标系—oxnynzn进行衡量。捷联惯导系统通常选择地理坐标系(东北天坐标系)作为导航坐标系—oxtytzt，原点与载体重心重合，用于求解运载体各个方向的加速度数值[5]。

图1 捷联式惯性导航系统原理

1.2 惯性导航基本方程

用四元数表示姿态矩阵(方向余弦矩阵)，如式(1)、式(2)所示：

(1)

(2)

参照文献[6]，分别通过式(3)～式(5)用四元数值反求载体的欧拉角。

θ=sin-1(T32)

(3)

(4)

(5)

式中:θ表示俯仰角,γ为横滚角,φ为航向角的值。

将比力fb从载体坐标系转换到导航坐标系fn的形式如式(6)所示：

(6)

速度可通过加速度一次积分求解，速度V的即时修正如式(7)所示：

(7)

姿态速率(载体坐标系相对于导航坐标系的角速度)可通过式(8)计算得到：

(8)

载体位置的计算主要包括载体的经纬度和高度的求解，具体实现过程如式(9)～式(11)所示：

(9)

(10)

(11)

载体坐标系和地理坐标系之间欧拉角的对应关系如式(12)所示：

(12)

2 GPS/INS组合导航系统

2.1 全球定位系统

全球定位系统(GPS)是利用3颗卫星发射时间到接收机(用户)之间的时差及轨道位置进行导航[7]。构建导航位置的3个方程，分别求解观测点的x、y、z坐标，即三角定位法。为保证测量的准确性，不受接收机与卫星时钟偏差影响，需要第四颗卫星，组成4个伪距方程，分别求解x、y、z坐标以及时钟偏差，从而达到导航定位的目的。虽然GPS导航信息输出频率较低，但是输出频率稳定，误差不随时间积累[8]。

2.2 组合导航系统原理

如图2所示，采用松组合模式进行INS/GPS组合测量。通过线性卡尔曼滤波作为融合算法，提取最小方差估计值，对INS进行速度、位置校正。当GPS输出速度、位置信息时，以GPS与INS速度、位置差值作为量测方程，利用INS误差方程构造组合系统状态方程[9]。

图2 INS/GPS松耦合结构图

2.3 组合系统状态方程与量测方程

惯性导航系统的误差包括INS位置误差、INS速度误差、INS姿态误差，通过误差组合，可得系统误差状态方程，如式(13)、式(14)所示，其状态量为18维。

(13)

X(t)=[φE,φN,φU,δvE,δvN,δvU,δL,δλ,δh,εcx,εcy,εcz,εrx,εry,εrz,…,∇rx,∇ry,∇rz]T

(14)

式中：k表示第k时刻，φE、φN、φU分别表示数据测量后的姿态角误差，δvE、δvN、δh表示方向速度误差,δL、δλ、δvU分别表示经度、维度和高度误差，εcx、εcy、εcz表示陀螺仪产生的随机误差,εrx、εry、εrz表示陀螺仪一阶马尔代夫过程，∇rx、∇ry、∇rz分别表示加速度计的零偏,W(t)表示系统陀螺仪、加速度随机白噪声。噪声系数矩阵G(t)如式(15)，INS系统状态转移矩阵如式(16)所示：

(15)

(16)

取INS、GPS速度、位置信息做差，作为量测方程。位置量测信息分别如式(17)、式(18)所示：

(17)

(18)

式中:(L,λ,h)表示位置信息，反映载体真实位置，(LI,λI,hI)表示基于惯性导航系统的经度、纬度和高度信息，(δL,δλ,δh)表示捷联惯导系统的经度、纬度和高度误差。(LG,λG,hG)下标带有G的表示GPS的位置信息，主要是GPS得到的经纬度和高度信息，(δLG,δλG,δhG)则是表示GPS的误差，是GPS定位系统得到的经纬度和高度误差，(NE,NE,NU)同样也表示位置信息，是GPS在东北天方向的位置误差[10]。

惯性导航系统的速度信息和GPS全球定位系统的速度信息如式(19)、式(20)所示：

(19)

(20)

式中：(δvE,δvN,δvU)和上述位置信息相对应，表示载体的真正速度，(vEI,vNI,vUI)表示捷联惯性导航系统的速度信息(东北天向速度信息)，(δvE,δvn,δvU)表示捷联惯导的东北天向速度误差信息，(vEG,vNG,vUG)表示GPS的速度信息，(ME,MN,MU)是GPS在东北天方向的速度误差。

位置、速度量测矢量如式(21)、式(22)所示：

(21)

式中：

(22)

GPS全球定位系统的量测白噪声矢量如式(23)所示：

N(t)=[NNNENhMEMNMU]T

(23)

对应的量测白噪声矢量，方差矩阵如式(24)所示：

(24)

2.4 Matlab软件仿真结果分析

在试验场地进行直线引动加水平回环运动，整合实验数据。通过建立的组合导航状态和量测方程，使用Matlab软件构建算法环境，仿真所得速度、位置误差曲线如图3、图4所示。

图3 组合导航前、后速度误差

图3中虚线表示组合导航前速度误差，实线表示组合导航后速度误差。由图3、图4分析可知，未进行组合导航前，速度误差随时间积累，最终导致瘫痪；而通过组合导航后，误差得到有效控制，速度误差的范围在-0.5～0.5 m/s之间，最大误差在-1～1 m/s之间，准确性明显提高；位置误差的波动也稳定在同一范围内，误差较小，准确度较高，组合导航具有一定的校正效果，使误差降低、精度提高，证明组合导航的合理性。

图4 组合导航位置误差

3 游乐设施轨迹测量及可视化

3.1 轨迹测量流程设计

为确保测量准确，尽可能消除重力分量，需要进行初始对准与捷联矩阵即时修正；采用GPS全球定位系统校正，弥补惯性导航因长时间积分运算而造成的误差累积。具体轨迹测量步骤如下：

(1)初始对准，保证导航坐标系与参考坐标系重合，手机水平固定，重力分量在z轴方向。

(2)捷联矩阵T的初始值为0，在运动过程中，对捷联矩阵即时修正。

(3)加速度数据利用捷联矩阵进行坐标转换。通过四元数关系矩阵将加速度分量从载体坐标系转换到地理坐标系；将一个g的重力分量通过四元数的姿态矩阵转换到平台坐标系下，再用转换后的加速度分量减去转换后的重力分量。由于不同地方的经度和纬度对重力加速度g有一定的影响，g0是赤道表面的加速度(g0=9.780 326 771 4 m/s2)，可通过式(25)计算地球引力加速度g：

g=g0×(1+0.005 217×sin2L)-

3.086×10-6h

(25)

(4)加速度数据经过姿态矩阵变换后，进行二次积分运算。在已知物体初始速度和位置的前提下，计算载体轨迹。

(5)用GPS全球定位系统辅助测量方法，对轨迹测量进行校正。

通过融合GPS，采用Great-Circle距离公式方法，来对轨迹进行校正。计算公式如式(26)所示：

(26)

式中:R为地球半径，Aj、Aw分别表示A点的经度、纬度,Bj、Bw分别表示B点的经度、纬度[11]。

3.2 轨道游乐设施实验测试

轨道游乐设施的现场取样如图5所示。将手机固定在过山车座椅上，使用由Android Studio软件开发的应用程序进行手机加速度传感器、陀螺仪传感器、GPS传感器数据采集。应用程序客户端架构组成与应用程序前端界面如图6、图7所示。

图5 轨道游乐设施数据采集现场

采集到的数据包括三轴加速度、三轴角速度、GPS数据，以及采集点的对应时间，数据被保存至后台文件中。测量完毕后，在手机的后台已经保存了3个文件，在后端程序的设计中，已将文件保存格式设置为.txt格式，保存的部分数据信息如图8所示。

图6 数据采集程序客户端架构组成图

图7 应用程序前端界面

图8 部分数据信息示意图

将数据导入，在Matlab软件中进行整合、求解四元数姿态、运动轨迹解算。载体姿态测得值经过卡尔曼滤波处理后，数值信息如图9～图11所示：

图9 横滚角滤波前后示意图

图10 俯仰角滤波前后示意图

图11 偏航角滤波前后示意图

在前500采样点阶段，手机处于初始对准静止阶段，数值变化不明显；初始对准之后开始运动，在采样点500～1 000之间时，出现最大峰值，在接近1 000点左右时，数据较密集，采样点在1 000～2 000之间时大概呈现均匀变化。图12中，偏航角在前500个采样中大降幅，降幅后趋于稳定，原因是试验开始时，初始化后手机放置位置向右偏移，安装过程中手机位置回偏，后趋于位置正常状态，500～1 000采样点间，有一段大幅波动，后趋于平稳，手机状况符合数据反馈情况，符合载体姿态变化情况。通过对比滤波前后数据，滤波后的角度更加平滑，有效的剔除毛刺，降低误差，提高精度。

将最终组合导航后的位置坐标导入UG NX界面中，拟合运动轨迹曲线 。运动轨迹图如图12、图13所示。

图12 拟合后轨迹图像

图13 回环运动放大部分

可以看出，整体轨迹曲线光滑流畅。轨道拟合的回环运动放大部分(图13)与实际游乐设施中的轨道相比，回环半径偏小。在高难度的回环运动中，由于轨道游乐设施进入回环前的冲击震动干扰了手机内传感器的采集，使得解算轨迹产生误差，总体来说拟合程度良好，基本上还原了轨道游乐设施的运动轨迹，能够真实反映运动轨迹。

为更直观检验三维轨迹拟合，选择一段三维轨迹进行测量拟合，拟合结果如图14所示。

图14 三维拟合轨迹

由此可见，轨道游乐设施下滑阶段的运动轨迹相应的保持一定的弧度，对应的角度与实际轨道地面夹角基本一致，能够正确直观反映轨道游乐设施的运动轨迹，证明了方法的有效性。

4 结束语

针对目前轨道式游乐设施物理样机运动状态研究分析的不足，开发安卓程序采集手机内置惯性元件及GPS的测量数据，通过Matlab软件对数据进行处理，设计姿态求解算法以及轨迹测量算法，进行卡尔曼滤波、姿态求解与组合导航，将处理的轨迹数据通过UG软件二次开发实现运行可视化。

经过试验证明，该方法可用于轨道式游乐设施的加速度、速度、姿态、轨迹测量，弥补轨道类游乐设施物理样机运动数据采集、测量的不足。通过手机测量，提高了数据采集的便携性，为虚拟样机运动分析与物理样机运动分析之间的联系与对比提供桥梁，提高了轨道游乐设施的安全性、可靠性。