CN108762528B - 适用于空中飞鼠的姿态解算方法 - Google Patents

Abstract

本发明涉及一种适用于空中飞鼠的姿态解算方法，所述空中飞鼠基于六轴传感器，包括：获取角速度ω和加速度a；估计水平方向的加速度的大小，确定增益因子α的大小；归一化加速度a，并求出加速度a与水平方向夹角的正弦余弦值；定义D是上次姿态解算估计出的重力矢量

φ为翻滚角，从而计算误差矢量e＝axD；误差矢量累加计算误差积分In，I_n＝I_n‑1+Δte，Δt是算法执行周期，e是上一步计算出来的误差；角速度融合ω′＝ω+α(K_pe+K_iI_n)。上述适用于空中飞鼠的姿态解算方法，提高空中飞鼠姿态解算的准确性，从而降低漂移感，降低姿态解算的复杂性从而提高姿态解算的实时性，减少空中飞鼠的滞后感和成本。

Description

适用于空中飞鼠的姿态解算方法

技术领域

本发明涉及空中飞鼠，特别是涉及适用于空中飞鼠的姿态解算方法。

背景技术

国内外对空中飞鼠均有一定的研究。早在2002年，Alan H.F.Lam等人就研制出了基于二维加速度计的虚拟键盘鼠标系统,操控者只需将加速度计装在指环上，数据信息就可以通过戴在手腕上的无线传输模块传送至电脑，然后通过解析和显示的软件实时解算这些数据，最后完成输入控制。2003年，英国伯明翰大学设计了一种可以控制电脑屏幕上三维立体旋转的三维鼠标。

然而无线空中飞鼠最大的问题就是鼠标轨迹的算法。起初，是利用加速度二次积分，偏移非常的大，例如2010年苏州大学的王海红设计的三维鼠标，也有利用加速度的数据计算鼠标的翻滚角，来对应鼠标的位置，如上海交通大学的姜晓波制作的AIR-MOUSE，这样做出来的效果体验感并不好。后来的空中鼠标大都用上了陀螺仪传感器，情况有所改善，比如罗技的MXAir利用两轴陀螺仪进行控制，实际效果就已经比较好了。目前大部分方案使用六轴传感器之后姿态算法更加精确，轨迹也更加准确。只是把加速度计换成了六轴传感器，比如2013年华侨大学周获制作的无线空中鼠标。

滞后感和漂移感的问题依旧存在，于是更多的传感器用来解决这个问题。比如2011年上海交通大学的刘刚开发的激光笔辅助教学系统，是通过摄像头识别投影屏幕上的激光点的位置，来确定鼠标的位置，这样可以达到像鼠标一样指哪点哪的操作体验，但是系统的便捷性和成本限制了这种方案的发展。宁波大学的赵士龙发表的无线手持控制器是借助触控屏来移动鼠标指针的，想法也比较新颖，但并不实用。

目前的空中鼠标用的方案大多是六轴传感器的方案，鼠标控制也从轨迹算法转为了姿态算法，用角速度去代替实际运动速度。而姿态算法方面却没有集中的研究，采用的大多是飞行器的姿态算法和传感器自带的运动处理单元。但是飞行器场景是一个趋于稳定的控制过程，对实时的角度要求不高，而手持场景是高动态的过程，需要更加实时准确的解算角度。

传统技术存在以下技术问题：

1、mahony互补滤波：由于将加速度方向作为重力方向，而空中飞鼠一直处于运动的过程中，对重力方向估计错误，导致漂移问题严重；

2、卡尔曼滤波：计算量很大，速度慢，成本较高；

3、六轴传感器内置算法，存在姿态解算速度的限制，存在滞后感，且不开源，不能够正确评估算法的性能。

发明内容

基于此，有必要针对上述技术问题，提供一种适用于空中飞鼠的姿态解算方法，提高空中飞鼠姿态解算的准确性，从而降低漂移感；降低姿态解算的复杂性从而提高姿态解算的实时性，减少空中飞鼠的滞后感和成本。

一种适用于空中飞鼠的姿态解算方法，所述空中飞鼠基于六轴传感器，包括：

获取角速度ω和加速度a；

估计水平方向的加速度的大小，确定增益因子α的大小；

归一化加速度a，并求出加速度a与水平方向夹角的正弦余弦值；

定义D是上次姿态解算估计出的重力矢量[cosφ，sinφ]^T，φ为翻滚角，从而计算误差矢量e＝a x D；

误差矢量累加计算误差积分In

I_n＝I_n-1+eΔt，Δt是算法执行周期，e是上一步计算出来的误差；

角速度融合

ω′＝ω+α(K_pe+K_iI_n)；K_p是误差的比例增益，Ki是误差的积分增益，e是上一步计算出来的误差，I_n是误差积分；

得出估计的重力矢量D_n，并进行归一化

D_n＝D_n-1+ω′ΔtD′，D′是D的导数。

上述适用于空中飞鼠的姿态解算方法，提高空中飞鼠姿态解算的准确性，从而降低漂移感，降低姿态解算的复杂性从而提高姿态解算的实时性，减少空中飞鼠的滞后感和成本。

在另外的一个实施例中，“估计水平方向的加速度的大小，确定增益因子α的大小；”具体包括：

根据陀螺仪得到的原始数据：偏航角速度ω1和俯仰角速度为ω2，以及上次解算得到的重力矢量[cosφ,sinφ]^T，φ为翻滚角，求出此时的水平加速度ωx＝ω1sinφ+ω2cosφ；

根据水平加速度的大小来调节增益因子α，当水平加速度的大小未达到第一阈值时，增益因子α是恒定的并且等于1；如果水平加速度增大并且幅度超过第一阈值，则增益因子α随着幅度误差的增加而线性地减小，直到大于第二阈值时增益因子α减小到0；

根据陀螺仪得到的原始数据偏航角速度ω1和俯仰角速度为ω2去判断是否处于静止状态，如果偏航角速度ω1的绝对值和俯仰角速度为ω2的绝对值之和小于第三阈值，则认为处于静止状态，增益因子α为1；其余状态增益因子保持不变。

在另外的一个实施例中，根据一阶龙格库塔法得出估计的重力矢量D_n。

一种空中飞鼠，所述空中飞鼠基于六轴传感器，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现任一项所述方法的步骤。

一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现任一项所述方法的步骤。

一种处理器，所述处理器用于运行程序，其中，所述程序运行时执行任一项所述的方法。

附图说明

图1为本申请实施例提供的一种适用于空中飞鼠的姿态解算方法中欧拉角的示意图。

图2为本申请实施例提供的一种适用于空中飞鼠的姿态解算方法中垂直平面上加速度方向的示意图。

图3为本申请实施例提供的一种适用于空中飞鼠的姿态解算方法中计算水平加速度大小的示意图。

图4为本申请实施例提供的一种适用于空中飞鼠的姿态解算方法中根据水平加速度大小调节增益因子。

图5为本申请实施例提供的一种适用于空中飞鼠的姿态解算方法中实验验证画圆时不同算法姿态角比较的示意图。

图6为本申请实施例提供的一种适用于空中飞鼠的姿态解算方法中实验验证部分水平循环移动时不同算法轨迹显示的示意图。(位移值是相对比例，无单位)。

图7为本申请实施例提供的一种适用于空中飞鼠的姿态解算方法中实验验证水平循环移动时姿态角的示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

一种适用于空中飞鼠的姿态解算方法，所述空中飞鼠基于六轴传感器，包括：

获取角速度ω和加速度a；

估计水平方向的加速度的大小，确定增益因子α的大小；

归一化加速度a，并求出加速度a与水平方向夹角的正弦余弦值；

定义D是上次姿态解算估计出的重力矢量[cosφ,sinφ]^T，φ为翻滚角，从而计算误差矢量e＝a x D；

误差矢量累加计算误差积分In

I_n＝I_n-1+eΔt，Δt是算法执行周期，e是上一步计算出来的误差；

角速度融合

ω′＝ω+α(K_pe+K_iI_n)；K_p是误差的比例增益，K_i是误差的积分增益，e是上一步计算出来的误差，I_n是误差积分；

得出估计的重力矢量D_n，并进行归一化

D_n＝D_n-1+ω′ΔtD′，D′是D的导数。

在另外的一个实施例中，“估计水平方向的加速度的大小，确定增益因子α的大小；”具体包括：

根据陀螺仪得到的原始数据偏航角速度ω1和俯仰角速度为ω2，以及上次解算得到的重力矢量[cosφ，sinφ]^T，φ为翻滚角，求出此时的水平加速度ωx＝ω1sinφ+ω2cosφ；

根据水平加速度的大小来调节增益因子α，当水平加速度的大小未达到第一阈值时，增益因子α是恒定的并且等于1；如果水平加速度增大并且幅度超过第一阈值，则增益因子α随着幅度误差的增加而线性地减小，直到大于第二阈值时增益因子α减小到0；

根据陀螺仪得到的原始数据偏航角速度ω1和俯仰角速度为ω2去判断是否处于静止状态，如果偏航角速度ω1的绝对值和俯仰角速度为ω2的绝对值之和小于第三阈值，则认为处于静止状态，增益因子α为1；其余状态增益因子保持不变。

上述适用于空中飞鼠的姿态解算方法，提高空中飞鼠姿态解算的准确性，从而降低漂移感，降低姿态解算的复杂性从而提高姿态解算的实时性，减少空中飞鼠的滞后感和成本。

在另外的一个实施例中，根据一阶龙格库塔法得出估计的重力矢量D_n。

一种空中飞鼠，所述空中飞鼠基于六轴传感器，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现任一项所述方法的步骤。

一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现任一项所述方法的步骤。

一种处理器，所述处理器用于运行程序，其中，所述程序运行时执行任一项所述的方法。

下面给出本发明的一个具体应用场景：

四元数定义为超复数：

Q＝q₀+q₁i+q₂j+q₃k

它的运算规则在这里就不详细介绍了，可以用四元数表示坐标系的旋转。

由于这个矩阵里不包含正弦余弦值，计算效率比较高。更重要的是四元数的旋转不存在万向节锁问题。不过接下来我们可以简化一下3D无线鼠标的姿态。

其实四元数就是为了解决三维空间内的旋转，而3D无线鼠标水平和垂直两个方向上的运动情况根本不需要加速度计去校正，所以其实姿态解算只需要得到3D无线鼠标的翻滚角φ，但是用欧拉角的方法需要计算sinφ和cosφ，这就需要用到泰勒公式，运算中带来了很多的乘法，造成了效率低下。

本发明没有采用传统的四元数的方法，仅仅是对翻滚角进行姿态解算，(如图3中的φ)，并对计算过程加以优化，省去了将角度转换为正余弦值的步骤，大大简化计算。

同时由于在四旋翼的姿态算法中，加速度计会对偏航角和俯仰角造成干扰。为了减少加速度计偏航角和俯仰角的影响，本发明采取的方案是完全相信陀螺仪来确定偏航角和俯仰角的角速度，只对翻滚角进行解算。

初始化

刚上电测到的加速度a₀可以近似为重力加速度，令初始重力矢量D₀＝a₀，初始误差积分I₀＝0。由于在姿态算法中，加速度计会不可避免的对偏航角和俯仰角造成干扰。为了减少加速度计对偏航角和俯仰角造成的影响，本发明采取的方案是完全用陀螺仪来确定偏航角和俯仰角的角速度，只对翻滚角进行姿态解算。同时也将三维运动简化为二维运动，计算更加简单。

调节增益因子α：

由于手持设备和飞行器使用场景不同，有着手持的特殊性，可以近似为绕着一个中心(比如手腕，手肘)的圆弧运动，在很短的时间内可以认为半径不变，因此空中飞鼠受到的加速度可以分为向心加速度、切向加速度和重力加速度。向心加速度与手臂方向平行，不影响翻滚角的测量。重力方向上的切向加速度与重力加速度平行，不会对重力方向造成影响。但是当空中飞鼠在水平方向做加速运动，切向加速度与重力加速度垂直，且大小不能忽略，此时加速度方向和重力方向不平行。Mahony互补滤波是利用加速度计数据计算重力方向的，这样会严重的干扰对重力方向的判断，本发明的方法是降低此时加速度计的增益。(参阅图2)

具体增益调节的过程：

首先根据陀螺仪计算得到的角加速度和上次姿态解算得出的角度得出此次水平加速度的大小。陀螺仪得到的原始角速度数据为ω1和ω2，上次解算得到的重力矢量[cosφ,sinφ]^T(φ为上一次估算出来的重力矢量与空中飞鼠x方向的夹角)，可以求出此时的水平加速度ωx＝ω1sinφ+ω2cosφ。(参阅图3)

其次水平加速度的大小来调节增益因子α，如图4所示，当水平加速度的大小未达到第一阈值时，增益因子α是恒定的并且等于1。如果水平加速度增大并且幅度超过第一阈值，则增益因子α随着幅度误差的增加而线性地减小，直到大于第二阈值时增益因子α减小到0。

最后通过原始陀螺仪得到的角速度去判断是否处于静止状态，如果角速度绝对值之和小于第三阈值，则认为处于静止状态，增益因子α为1。其余状态增益因子保持不变。这是为了防止初始化错误的情况和由于长时间的高动态情况积累的误差导致误差未收敛的情况。

这里的第一、第二和第三阈值是由具体实验和经验得到的。

误差矢量的计算

这个误差是上一次陀螺仪得到的姿态估算的重力方向和此时实际加速度方向之间的误差。这里θ_a是角速度方向和空中飞鼠x方向的夹角。φ为上一次估算出来的重力矢量与空中飞鼠x方向的夹角。(x方向如图3)这里用叉积来表示两个矢量之间的误差，这个修正对于误差来说是收敛的，误差会越来越小。两个角之差的正弦值又可以通过和差公式拆解成估计的翻滚角和加速度计测量的翻滚角之间的关系。

e＝a×D＝sin(θ_a-φ)＝sinθ_acosφ-cosθ_asinφ

角速度融合

ω为翻滚角的角速度，角速度融合是为了融合陀螺仪和加速度的数据，让翻滚角的数据更加准确。K_p和K_i分别为比例增益和积分增益，用比例积分的方法去控制陀螺仪和加速度计之间的误差。此处的K_i和K_p同样需要根据经验和具体实验得出，一般K_i可以设置为0.05K_p ²到0.1K_p ²。增益因子α是由步骤2得出，根据陀螺仪估算的水平加速度大小去确定加速度数据的增益。

角速度积分得出翻滚角

龙格库塔方法是一种应用广泛的高精度单步算法，可以用于数值求解微分方程，本发明利用它进行积分，更新重力矢量。D＝[cosφ,sinφ]^T，D′＝[-sinφ，cosφ]^T。

运算量

本发明中的正余弦变量全部可以直接参与运算，不需要额外的对角度的正余弦值进行计算，大大减少了运算量。整个算法步骤仅有十一次乘法(除去归一化的步骤)，大大减少了计算量。

附：理论推导过程

互补滤波实现

此发明中数据融合通过P控制器和积分过程完成，其中“加速度计”角度a变为设定点，旋转速率变为ω扰动(类型d₁)。通过PI控制器来减小陀螺仪的漂移。

因此相应的传递函数为

其中φ为翻滚角(算法解算出来的重力方向与x方向的夹角)，Kp为比例增益，Ki为积分增益，a为加速度角度(加速度与x方向的夹角)，ω为翻滚角的角速度。

令误差角度e＝a-φ，误差积分

因此

将其用backward difference方法离散化，得到：

e_k＝a_k-φ_k-1

I_k＝I_k-1+Δte_k

φ_k＝φ_k-1+α(K_pe_k+K_iI_k+ω_k)Δt

e＝a-φ，由于误差需要逐渐收敛的，sin(a-φ)同样可以做到，而且可以让计算更加简便，避免了角度的直接计算。

e_k＝sin(a_k-φ_k-1)＝sin a_k cosφ_k-1-cos a_k sinφ_k-1

ω＝α(K_pe_k+K_iI_k)+ω_k

由一阶龙格库塔法可得微分方程

的解为

即：D_n＝D_n-1+ω′ΔtD′，拆分开来写作

sinφ_n＝sinφ_n-1cosωΔt+cOsφ_n-1sinωΔt

cosφ_n＝cosφ_n-1cosωΔt-sinφ_n-1sinωΔt

角加速度和加速度的关系

在手持演讲环境中可以将手的运动简化为绕着支点圆弧运动的模型，实时的速度为

V＝ωr，ω为角速度，r为圆弧运动的半径

此时加速度a＝dV/dt＝dω*r/dt＝(ωk-ωk-1)*r/Δt

令b＝r/Δt，可知，加速度和角速度之差成线性关系，因此本发明可以用水平方向上的角速度之差代替水平加速度的大小。

实验效果验证：

在本节中，本发明尝试评估增加了自适应增益的姿态算法的性能。由于卡尔曼滤波等算法姿态解算出的结果和互补滤波得到的结果的差距很小，我们用Mahony互补滤波代替传统的四旋翼姿态解算算法。本发明通过两个实验，将传统Mahony互补滤波得到的结果，和增加了自适应增益的互补滤波得到的结果进行比较。

姿态角跟踪实验

本节实验，主要是将Mahony互补滤波算法和本发明姿态算法解算出的姿态角进行实时对比。实验数据是通过手顺时针画圆得到的，实验刚开始的姿态角和实验结束时的姿态角是相同的。

由于本发明没有仪器去实时的测量姿态角，所以通过比较不同曲线的趋势去分析结果。在图5中，绿色的曲线是加速度计得到的姿态角，红色的曲线是陀螺仪得到的姿态角，蓝色的曲线是采用Mahony互补滤波得出的姿态角，黑色的为本发明采用的算法得出的姿态角。

本实验的条件开始和结束的姿态角相同，从图中可以看出，陀螺仪(红线)得到的姿态角从1.60rad的位置起始，到1.62rad的位置结束，可以看出存在漂移，漂移量为0.02rad。加速度计(绿线)得到的姿态角开始和结束时都是在1.60rad，不存在漂移。另一方面，陀螺仪(红线)得到的姿态角曲线噪声很小，加速度计(绿线)得到的姿态角噪声很大。所以姿态算法需要结合两者的优点。

从图中可以看出，Mahony互补滤波和本发明的算法都很好的控制了姿态角积分的漂移，曲线比较平滑，噪声控制较好。而Mahony互补滤波相对于本发明采用的算法来说，在某些动作(如在采样次数为250-300次)时，受到加速度噪声的影响更大，因此在图中体现出来的是姿态变化的滞后性。比如在实际情况中，当水平运动加速度较大的情况下，姿态角会因为加速度的噪声而漂移严重。根据对实验数据的分析，初步得出这就是漂移感的原因的结论，下面的实验将对轨迹的漂移现象更好的呈现。

高非重力加速度实验

本发明利用快速的水平循环移动代替专业的高非重力加速度实验，水平循环移动能够让水平方向上的加速度很大，从而达到了高非重力加速度的要求，我们可以用这个实验去测试它的动态特性。实验数据是用手在水平方向上反复移动得到的，并保持姿态角基本不变。但是由于手部运动并不统一，有一定的噪声影响，因此描绘出来的轨迹并不是标准的直线。

图6是用不同的算法，将每次处理后计算出的位置描绘出来，可以显示鼠标的运动轨迹。红色的曲线是用陀螺仪原始数据画出的未量化的轨迹，因为不受加速度的影响，短时间内偏移并不严重，理论上我们可以将它作为参考标准。

加速度计测量的加速度是水平加速度和实际重力加速度的合加速度，当水平移动较快，水平方向上的加速度大小较大时，水平方向上的加速度不能够忽略。而Mahony互补滤波认为加速度测量的只是重力加速度，因此导致算法估计出的重力方向和实际的重力方向有偏差，如果不降低此时加速度计数据的增益，会使轨迹向上偏移。从图6看出原始的Mahony互补滤波(黄线)偏移很严重，画直线都会向上翘，同时也可以很明显的看出本发明的方法得出的轨迹更加不容易向上漂移。

图7显示的是在水平移动的过程中不同的算法解算出来的姿态角，红色的曲线是陀螺仪积分出的姿态角，绿色的曲线是加速度计得到的姿态角，蓝色的曲线是原始Mahony滤波，黑色的曲线是本发明采用的方法。

因为在水平方向快速移动时，水平方向上的加速度较大，影响了合加速度的方向。往返运动中水平方向上存在一个先加速后减速，再反向加减速的过程，因此加速度计(绿线)计算出来的角度呈现出一个震荡的趋势。而陀螺仪的数据和加速度没有关系，更加可靠，姿态角基本上没有变化。

Mahony互补滤波(蓝线)认为加速度测量的只是重力，这会让姿态角随着加速度方向的变化而变化，形成震荡较大。我们可以看到相比于原始的Mahony滤波，本发明采用的方法，姿态角受到加速度噪声的影响更小。

其以上所述实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。

以上所述实施例仅表达了本发明的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。因此，本发明专利的保护范围应以所附权利要求为准。

Claims (4)

1.一种适用于空中飞鼠的姿态解算方法，所述空中飞鼠基于六轴传感器，其特征在于，包括：

获取角速度ω和加速度a；

估计水平方向的加速度的大小，确定增益因子α的大小；

归一化加速度a，并求出加速度a与水平方向夹角的正弦余弦值；

定义D是上次姿态解算估计出的重力矢量[cosφ,sinφ]^T，φ为翻滚角，从而计算误差矢量e＝a x D；

误差矢量累加计算误差积分In

I_n＝I_n-1+eΔt，Δt是算法执行周期，e是上一步计算出来的误差；

角速度融合

ω′＝ω+α(K_pe+K_iI_n)；K_p是误差的比例增益，K_i是误差的积分增益，e是上一步计算出来的误差，I_n是误差积分；

得出估计的重力矢量D_n，并进行归一化

D_n＝D_n-1+ω′ΔtD′，D'是D的导数；

“估计水平方向的加速度的大小，确定增益因子α的大小；”具体包括：

根据陀螺仪得到的原始数据偏航角速度ω1和俯仰角速度为ω2，以及上次解算得到的重力矢量[cosφ,sinφ]^T，求出此时的水平加速度ωx＝ω1sinφ+ω2cosφ；

根据水平加速度的大小来调节增益因子α，当水平加速度的大小未达到第一阈值时，增益因子α是恒定的并且等于1；如果水平加速度增大并且幅度超过第一阈值，则增益因子α随着幅度误差的增加而线性地减小，直到大于第二阈值时增益因子α减小到0；

根据陀螺仪得到的原始数据偏航角速度ω1和俯仰角速度为ω2去判断是否处于静止状态，如果偏航角速度ω1的绝对值和俯仰角速度为ω2的绝对值之和小于第三阈值，则认为处于静止状态，增益因子α为1。

2.一种空中飞鼠，所述空中飞鼠基于六轴传感器，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述程序时实现权利要求1所述方法的步骤。

3.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该程序被处理器执行时实现权利要求1所述方法的步骤。

4.一种处理器，其特征在于，所述处理器用于运行程序，其中，所述程序运行时执行权利要求1所述的方法。

Images