VR机舱人机交互姿态追踪器的算法设计

2018-08-27，

船海工程 2018年4期

，

(大连海事大学轮机工程学院，辽宁大连 116026)

运动载体的倾角检测技术在众多领域均有重要应用，如飞行器航姿、导航系统等，VR技术的出现使得其在人机交互的位置追踪、姿态识别方面得到重视[1]。虚拟三维空间通过大尺寸显示环境如弧幕或者球幕等进行显示，人机交互的位置追踪方式有超声波跟踪器、GPS跟踪器、光学跟踪器等[2]。以此为基础考虑设计精度适用于虚拟现实机舱的追踪器算法，可用于机械跟踪器或惯性跟踪器等穿戴嵌入式设备中。

1 加速度计数据采集

单一的加速度计的输出在动态时精度差，在载体运动时存在动作检测输出延迟，但静止时输出不会随时间漂移；另一方面陀螺仪精度高，但存在零点漂移[3]。借鉴卡尔曼滤波思想[4-6]，采用固定权值的方法融合加速度值与陀螺仪值，属于前馈型数据调节系统。

采用16位ADC加速度检测模块，模块输出值范围是0～655 35，Vref=5 V。由加速度计传感器手册查知零加速度Vzerog，将得到的三轴加速度数据转换为电压值。

(1)

AdcX是实时采集到的加速度计的值，由此得到电压值，该电压值相对于0电压的偏移量为

OffsetX=VolX-Vzerog

(2)

引入加速度计灵敏度S，设S= 478.5 mV/g，将偏移量转化为物理量加速度单位。

根据AccelX=OffsetX/S计算加速度[7]。

(3)

由此求出夹角,由于载体的重力方向与东北天坐标系的天地轴是平行重合的，通过测量载体坐标系各轴与重力方向的夹角以求得其与地理坐标系的角度旋转关系，进一步获知姿态。经过解算角速度计输出的加速度信息，得到载体与地平面的倾角，但是根据加速度计的工作原理可知其测量是惯性力，即便是载体处于相对平稳的状态，它对振动和机械噪声比较敏感。相对静止状态下加速度计的输出原始数据见图1。

图1 单一加速度计和陀螺仪原始数据

由图1可见，加速度计的输出在动态时精度差，但不会随时间漂移；而陀螺仪精度高，但存在零点漂移。因此考虑将双方的优点进行融合，用卡尔曼滤波思想做数据融合，通过计算加速度计和陀螺仪值得到更准确的当前载体姿态的倾角值。

2 数据融合与滤波

陀螺仪输出的是三轴角速率，原理见图2。

图2 模拟陀螺仪原理示意

在表面光滑的圆盘模型上面有一个小球在A点，当小球有一个初速度V指向圆心运动时，经过一段时间t后运动到B点(可见A、B以及O在同一直线上)。当圆盘以角速率时，那么经过相同的时间t则运动到B点的前一个位置B′点，由于圆盘在转动，因此小球离开A时，除了具有径向速度V′外，还有切向速度V1；在B点的切向速度为V2，易知V1

如果以圆盘为参考系，当圆盘转动时，将小球从A点以初速度V′向圆心抛去，对于圆盘上的B点来说，小球并未沿直线达到B点，而是向B点的右前方偏离，对这一结果作分析发现球具有径向速度V′的同时，还具有垂直于这一方向向右的加速度a′。根据牛顿第二定律知，既然球出手后在该方向上没有受到“真实力”的作用，那一定是垂直于V′向右的惯性力Fc。这种在转动参考系中观察到的运动物体的加速现象即科里奥利效应，Fc即科里奥利力。

以转动参考系作为分析参考系，球从A到B′时间为

(4)

球偏离距离AB的距离为

BB′=(V1-V2)· Δt′=

ω·(OA-OB)·Δt′=ω·V′Δt′2

(5)

当Δt′很小时，可以认为沿BB′的运动时匀加速运动而初速度为0，则

(6)

由公式(6)反解出a′，该结果是在Δt′很小时得到的。真实的设计是在陀螺仪内部的1个以一定频率做水平震荡的质量块，当该质量块水平振荡时的频率和位移唯一确定时，此时该质量块的速度可以确定；当陀螺仪旋转时，垂直于该质量块的水平方向会有1个科里奥利力的作用，在该力的作用下会有1个加速度，结合加速度计的工作原理求出该加速度，根据a′=2ωV′求出陀螺仪旋转时的角速率。

设已经获取的加速度计输出惯性矢量为

Raccel=[Rxaccel,Ryaccel,Rzaccel]

(7)

式中：Rxaccel、Ryaccel、Rzaccel表示加速度向量在载体坐标系空间内的分量。这里若初始化加速度传感器配置惯性矢量的长度不是1g，需要对矢量进行标准化处理。

(8)

式中：n为当前时刻。

参考卡尔曼滤波思想[8]，引入数据融合后估算的输出值向量Rout。

Rout=[Rxout,Ryout,Rzout]

(9)

数列初始化时，认为加速度值正确并将加速度的值对应赋值给输出值，即

Rout(0)=Raccel(0)

(10)

Rout(0)为初始的输出估算值。

在嵌入式主控单元内部有时间片Tick，每经过Tick时间做一次测量值接收,于是经过nTick后，便有了前一时刻的Rout(n-1)估算值，初始值Rout(0)=Raccel(0)以及当前时刻加速度的测量值Raccel(n)，前述用陀螺仪数据修正加速度值，因此在估算当前时刻的最佳值Rout(n)之前，引入由陀螺仪当前值和前一个估算值得到的修正中间变量Rgyro。

Rgyro=[Rxgyro,Rygyro,Rzgyro]

(11)

(12)

以其中1个分量Rxgyro为例，见图3。

图3 空间加速度向量分解

φ即为横滚角，θ即为俯仰角。在Rzaccel和Rxz组成的直角三角形中，由tanφ=Rxaccel/Rzaccel反解夹角φ=arctan(Rxaccel/Rzaccel)，根据已经得到的(n-1)时刻的Rout(n-1)，可以得到

(13)

在空间变换后新的角度：

φ(n)=φ(n-1) +Rateφ(n) ×T

(14)

Rateφ表示陀螺仪读取到的横滚角的角速率，为了使数值更准确可以采用平均值法或者提高采用频率，这里采用平均值法

(15)

因此：

φ(n)=φ(n-1)+RateφAvg×T

(16)

θ(n)=θ(n-1) +RateθAvg×T

(17)

由于

(18)

为了简化运算，替换运算符号，以方便在主控单元中计算，优化程序。

Raccel(n)：加速度计读取的当前值；Rgyro(n)：根据 (n-1)和陀螺仪当前值所得。

当前时刻的角度Rout(n)信息更新由以上2项共同完成，Rout(n)信息更新采用固定权值—加权平均值的方法。

(19)

wgyro是信赖程度，可以看出该出使用的权值不同于卡尔曼滤波权重随着加速度计的噪声而改变，这里采用固定权重的方法足够使用，其值的确定在软件设计部分，由实验产生。Rout(n)为

(20)

将Rout进行标准化处理。

(21)

得到Rxout后，进行下一轮ADC数据采集与融合循环。算法流程见图4。

图4 倾角更新算法流程

3 算法仿真

固定权重需要根据实验进行调节，采用二分法在[0,1]调节权重。

采用Matlab仿真的方式，将设计的算法编写成m函数，取值范围为[0,360]。模拟载体坐标系绕原点位置旋转一圈，将其做生成单轴折线图查看噪声与滤波后情况，见图5。最下方曲线表示高斯白噪声干扰，范围为(1，360)之间的随机数，这里权重wgyro初始化为0.25，曲线显示其中的一个倾角偏航角。

图5 算法仿真图形化显示

通过分析步伐姿态识别在机舱虚拟现实系统的应用可行性。按照算法解算出来的数据在数据调试端口看不出任何差别，将其做成折线图5实时显示则可以看出：(1)在最开始一段时间时载体是静止的，实时显示的折线图抖动较大原因是此时算法的作用并不明显，正如前文叙述的此时认为加速度计的输出是可靠的并作为估算值输出，陀螺仪对加速度值的平滑处理作用还未完全开始。(2)随着时间和接收数据的增多，将载体原位置旋转，可以看出其偏航角实时平滑变化。对比完整的卡尔曼滤波变权重的方法，计算出当前的姿态估算值，可以看出本文设计的固定权重wgyro在嵌入式设备中比较合适。