CN105910606B - 一种基于角速度差值的方向修正方法 - Google Patents

Abstract

本发明公开了一种基于角速度差值的方向修正方法，包括以下步骤：将东北天坐标系中输出的加速度转化到载体坐标系中，获取加速度计的第一误差向量；将电子罗盘在载体坐标的输出经过旋转到东北天坐标系中，获取磁场向量的第二误差向量；将经过加速度数据和磁场数据补偿之后的第一误差向量和第二误差向量相加获取误差；根据误差修正陀螺仪的角速度值，获取修正后的角速度，利用修正后的角速度更新四元数；通过修正后的角速度，采用积分方法计算载体的方向角度信息。本发明通过结合四元数互补滤波和角速度差值计算方向角度，减小算法的复杂度，并通过改进积分方法，减小传感器本身存在随机常值偏移对角度计算造成的影响，提高角度计算的精度，进而提高室内定位的精确度。

Description

一种基于角速度差值的方向修正方法

技术领域

本发明涉及定位技术领域，尤其涉及一种惯性导航方面一种姿态计算方法，在室内环境下优化方向角度获取方法，属于移动计算和信号处理的交叉技术应用领域。

背景技术

人们的日常生活和工作大多处于室内环境，基于室内定位技术的应用有着迫切的需求和广泛的使用前景。室内定位是指采用无线通讯、基站定位、惯性导航定位等多种技术，在室内环境中进行位置定位，从而实现对室内人员、物体等的位置监控。

室内环境不同于室外环境，相比于室外环境，室内环境存在着环境更加复杂、直接到达的传输道路容易缺失、定位范围也相对小、多径传播现象、室内环境经常容易发生变化等一系列问题。

室内定位技术经过很多年的发展，各种不同的技术被应用到此领域中，例如：无线信号定位技术、计算机视觉定位技术、磁场定位技术以及惯性导航定位技术等，这些不同的技术各自存在着优势，但也有一定的局限性。例如：无线信号定位技术中的红外技术穿透性差，超声波技术发射测距时受多径效应和非视距传播影响很大，射频定位技术不具有通信能力，抗干扰性差等。

为了解决上述的缺陷，现有技术中又提出了惯性导航定位技术，该技术是利用运动传感器来获取运动载体自身的运动信息，通过融合运动信息得到方向和位置信息，从而实现运载体的导航定位功能。因为组成惯性导航系统的运动传感器都安装在载体内，运行时可以不依赖外界环境，而且也不向外界辐射能量，因而此种定位方法不易受到干扰，是基于载体本身的自主导航系统。然而由于方向和位置信息经过积分而产生，系统误差会随时间而增大，所以长期使用惯性导航会存在精度较差的问题。

发明内容

本发明提供了一种基于角速度差值的方向修正方法，本发明减小了惯性导航系统中由于时间增大，带来的较大积累误差，提高了方向角度的计算精度，详见下文描述：

一种基于角速度差值的方向修正方法，所述方法包括以下步骤：

将东北天坐标系中输出的加速度转化到载体坐标系中，获取加速度计的第一误差向量；

将电子罗盘在载体坐标的输出经过旋转到东北天坐标系中，获取磁场向量的第二误差向量；

将经过加速度数据和磁场数据补偿之后的第一误差向量和第二误差向量相加获取误差；

根据误差修正陀螺仪的角速度值，获取修正后的角速度，利用修正后的角速度更新四元数；

通过修正后的角速度，采用积分方法计算载体的方向角度信息。

所述方法还包括：

根据初始状态下加速度三轴数据、陀螺仪三轴数据以及电子罗盘三轴数据，获取载体的俯仰角、偏航角和滚转角，并获取初始四元数；

将经过滤波处理后的加速度计、陀螺仪和电子罗盘器获取的三轴加速度、三轴角速度以及三轴磁场数据转化为单位向量。

所述方法还包括：

对于运动传感器获取的原始数据，转化为载体计算位置信息和方向信息所需要的运动数据，并加以滤波处理。

本发明提供的技术方案的有益效果是：多传感器数据融合过程中，由于卡尔曼滤波、小波分析等姿态解算算法的复杂增加算法的复杂度和计算量，而这不利于实时计算，导致这些算法复杂度高，计算密度大，收敛速度相对较慢，而本发明通过结合四元数互补滤波和角速度差值计算方向角度，减小算法的复杂度，并通过改进积分方法，减小传感器本身存在随机常值偏移对角度计算造成的影响，提高角度计算的精度，进而提高室内定位的精确度。

附图说明

图1为基于角速度差值的方向修正方法的流程图；

图2为传感器原始数据和处理后数据的对比示意图(以加速度计y轴为例)；

图3为角速度差值法积分的推导过程；

图4为角速度变化曲线示意图；

图5为角度变化曲线示意图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面对本发明实施方式作进一步地详细描述。

载体的姿态是惯性导航中重要的组成部分，载体的角度主要通过结合运动传感器的加速度信息、角速度信息和磁场信息而得到，惯性导航定位采用卡尔曼滤波、小波分析等姿态解算算法对采集的多种传感器数据进行融合，由于这些算法复杂度高，计算密度大，收敛速度相对较慢，而姿态结算中的俯仰角和横滚角的值并不需要解算得到，通过三角函数解算各个方向上的角度又会增加算法的复杂度，使计算量增多，而这不利于实时计算。

本发明实施例提出了基于角速度差值的方向修正方法，本方法利用基于四元数互补滤波[1-2]解算过程中的纠正后的陀螺仪信息积分获取载体在横向的偏转角，而角速度数据虽然经过加速度计和磁场数据的补偿，但是由于本身存在随机常值偏移使得角速度会在一定范围内震荡，本发明实施例通过改进积分方法来计算载体的方向角度信息，提高载体方向角度的精确度。

实施例1

参见图1，本发明实施例提供了一种基于角速度差值的方向修正方法，该方法包括：运动传感器数据优化、四元数滤波修正陀螺仪数据、角速度差值积分，通过该三部分以此获取载体方向角度，具体步骤如下：

101：将东北天坐标系中输出的加速度转化到载体坐标系中，获取加速度计的第一误差向量；

102：将电子罗盘在载体坐标的输出经过旋转到东北天坐标系中，获取磁场向量的第二误差向量；

103：将经过加速度数据和磁场数据补偿之后的第一误差向量和第二误差向量相加获取误差；

104：根据误差修正陀螺仪的角速度值，获取修正后的角速度，利用修正后的角速度更新四元数；通过修正后的角速度，采用积分方法计算载体的方向角度信息。

其中，在步骤101之前，该方法还包括以下步骤：

对于运动传感器获取的原始数据，转化为载体计算位置信息和方向信息所需要的运动数据，并加以滤波处理；

根据初始状态下加速度三轴数据、陀螺仪三轴数据以及电子罗盘三轴数据，获取载体的俯仰角、偏航角和滚转角，并获取初始四元数；

将经过滤波处理后的加速度计、陀螺仪和电子罗盘器获取的三轴加速度、三轴角速度以及三轴磁场数据转化为单位向量。

综上所述，本发明实施例通过上述步骤101-步骤104结合了四元数互补滤波和角速度差值计算方向角度，减小了计算复杂度，并通过改进积分方法，减小传感器本身存在随机常值偏移对角度计算造成的影响，提高了角度计算的精度，进而提高了室内定位的精确度。

实施例2

下面结合具体的计算公式、图2和图3对实施例1中的方案进行进一步地介绍，详见下文描述：

201：对于运动传感器获取的原始数据，转化为载体计算位置信息和方向信息所需要的运动数据，并加以限幅滤波，均值滤波以及简易的卡尔曼滤波处理；

其中，运动传感器获取到的原始数据存在较大误差，因为运动传感器中会掺杂很多其他的信号，比如：磁场、抖动之类的自然的或人为不可避免的信号，这些都要滤掉，便于结果分析，为此本发明实施例需要尽可能的消除运动传感器的误差，减小干扰和漂移，从而减小累计误差。

本发明实施例将从加速度计、陀螺仪和电子罗盘获取的加速度、角速度和磁场数据进行限幅滤波、均值滤波以及简易卡尔曼滤波处理。具体滤波处理的步骤为本领域技术人员所公知，本发明实施例对此不做赘述。

202：根据初始状态下加速度三轴数据、陀螺仪三轴数据以及电子罗盘三轴数据，获取载体的俯仰角、偏航角和滚转角，并获取初始四元数；

基于四元数的互补滤波姿态解算方法的基本原理是对于不同的坐标系中，当表示同一个向量时，向量的大小和方向一定是相同的，当不同坐标系进行转化过程中，由于旋转矩阵存在误差，因此在旋转过程中会与理论值存在偏差，因此可以采用四元数来表示旋转矩阵，通过修正偏差来修正旋转矩阵，进而修正载体的方向角度信息。

本发明实例中涉及到的坐标系包括载体坐标系(b)和东北天坐标系(n)，载体坐标系(b)的原点在载体的质心，OX轴指向载体的右侧，OY轴沿载体纵轴方向并指向前，OZ轴垂直于载体竖直向上；东北天坐标系原点处于导航系统所处的位置，Z轴与椭球法线重合，向上为正(天向)，y轴与椭球短半轴重合(北向)，x轴与地球椭球的长半轴重合(东向)。其中，载体坐标系(b)和东北天坐标系(n)为本领域技术人员所公知，本发明实施例对此不做赘述。

互补滤波中，定义四元数q如公式(1)所示：

q＝[q₀ q₁ q₂ q₃][1 i j k]^T (1)

其中，q₀、q₁、q₂、q₃表示实数；i，j，k表示虚数单位；T表示转置。

初始四元数的确定是根据载体的初始角度得到，初始角度是在载体坐标系(b)中得到，包括载体的俯仰角、偏航角和滚转角。

载体的俯仰角、偏航角和滚转角可以根据初始状态下加速度三轴数据、陀螺仪三轴数据以及电子罗盘三轴数计算得到，得到三个角度后，计算得到初始四元数。

203：将经过滤波处理后的加速度计、陀螺仪和电子罗盘器获取的三轴加速度ax，ay，az，三轴角速度wx，wy，wz以及三轴磁场数据mx，my，mz转化为单位向量；

204：将东北天坐标系(n)中输出的加速度转化到载体坐标系(b)中得到(vx,vy,vz)^T，载体坐标系(b)中加速度计的实际测量值为(ax,ay,az)^T，向量间的误差可以用向量叉积来表示，得到第一误差向量(ex1,ey1,ez1)^T＝(vx,vy,vz)^T差乘(ax,ay,az)^T；

对于磁场数据对误差的修正，(mx,my,mz)^T为电子罗盘在载体坐标系(b)的输出，(bx,by,bz)^T为东北天坐标系(n)中磁场向量，将(mx,my,mz)^T经过旋转到东北天坐标系(n)中得到(hx,hy,hz)^T，因为磁场向量的大小在不同坐标系中相同，通过转化得到载体坐标系(b)中的磁场向量(wx,wy,wz)^T，第二误差向量(ex2,ey2,ez2)^T＝(mx,my,mz)^T差乘(wx,wy,wz)^T。

205：将经过加速度数据和磁场数据补偿之后的第一误差向量和第二误差向量相加获取误差e＝(ex,ey,ez)^T；

206：根据误差修正陀螺仪的角速度值，获取修正后的角速度w；

计算公式如(2)(3)所示，其中，K_p和K_i是控制加速度计修正陀螺仪积分姿态的速度，是根据实验定义的两个常量(本发明实施例对此不做限制)，δ为经过K_p和K_i修正后的误差，w_g是陀螺仪的测量值，w为经过修正后的角速度，e＝(ex,ey,ez)^T为经过加速度数据和磁场数据补偿之后得到的误差。

δ＝K_pe+K_i∫e (2)

w＝w_g+δ (3)

然后利用修正后的角速度数据来更新四元数中的q₀、q₁、q₂、q₃。

207：通过纠正后的角速度，采用积分方法计算载体的方向角度信息。

在室内定位中，载体在行驶过程中会横向偏移，室内定位利用偏转的角度来计算载体在实际地形中方向角度，因而姿态结算中的俯仰角和横滚角并不需要解算得到，而通过三角函数解算各个方向的角度会增加算法的复杂度，不利于实时计算。

本发明实施例利用四元数互补滤波解算过程得到纠正后的角速度，进而通过纠正后的角速度积分得到方向角度，获取载体在横向的偏转角，而角速度数据虽然经过加速度计和磁场数据的补偿，但是由于本身存在随机常值偏移使得角速度会在一定范围内震荡，本发明实施例通过改进积分方法来计算载体的方向角度信息，改进算法如公式(4)所示：

其中，Δt为采样间隔，Δω_n,n-1为n点角速度ω_n与n-1点角速度ω_n-1之差，ω_n-1即为经过修正后载体在n-1点的角速度数据，θ_n载体为在n点的角度值，θ_n-1为载体在n-1点的角度值，θ_n-2为载体在n-2点的角度值，ω_i为载体在i点的角速度。角速度差值法积分的推导过程如图3所示。

综上所述，本发明实施例通过上述步骤201-步骤207结合了四元数互补滤波和角速度差值计算方向角度，减小了计算复杂度，并通过改进积分方法，减小了运动传感器本身存在随机常值偏移对角度计算造成的影响，提高了角度计算的精度，进而提高了室内定位的精确度。

实施例3

下面结合图4、图5对实施例1和2中的方案进行可行性验证，详见下文描述：

为了验证本方法的有效性，模拟载体向左、向右、向左、向右、向右、向左、向右的运动过程，在此过程中载体角速度的变化过程如图4所示，载体角度的相应变化过程如图5所示，图中所示曲线中，上部曲线代表利用角速度直接积分方法获得的角度，下部曲线代表利用互补滤波方法得到的角度，而中间曲线则代表经过本方法修正得到的角度。

图4表示角速度经历了负向变大变小，正向变大变小等几个过程，图5表示载体方向角度的变化，从图5可以看出，利用角速度直接积分方法获得的角度，在短期内基本能描述载体转动时的角度变化，但是随着测试时间的增加，精度明显下降；利用互补滤波方法得到的角度相对于直接积分获得有了一定的改进，漂移现象得到一定改善，但是测量精度依然欠佳；而通过本方法得到的角度不仅可以精确反应载体转动时角度的变化，并且数据的静态稳定性也显著增加，明显抑制了数据的偏移，得到的曲线也更为平滑，测量精度大大提高。误差计算方法如公式(5)所示：

平均绝对误差：

经过试验分析，角速度直接积分得到角度的平均误差为6.11°，互补滤波得到角度的平均误差为2.05°，本方法得到的角度平均误差为0.83度，相比互补滤波提高了59.51％。

从上述实验可以看出，本方法的主要优点在于利用经过磁场和加速度数据修正过的陀螺仪数据，运用角速度差值法来计算得到载体的方向角度，融合多传感器的信息，减小算法的复杂度，提高算法实时计算的能力，增加角度计算的精确度，进而提高定位的精确度。

参考文献

[1]吕印新,肖前贵,胡寿松.基于四元数互补滤波的无人机姿态解算[J].燕山大学学报，02:175-180

[2]陈伟.基于四元数和卡尔曼滤波的姿态角估计算法研究与应用[D].燕山大学,2015.

本发明实施例对各器件的型号除做特殊说明的以外，其他器件的型号不做限制，只要能完成上述功能的器件均可。

本领域技术人员可以理解附图只是一个优选实施例的示意图，上述本发明实施例序号仅仅为了描述，不代表实施例的优劣。

以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (3)

1.一种基于角速度差值的方向修正方法，其特征在于，所述方法包括以下步骤：

将东北天坐标系中输出的加速度转化到载体坐标系中，获取加速度计的第一误差向量；

将电子罗盘在载体坐标的输出经过旋转到东北天坐标系中，获取磁场向量的第二误差向量；

将经过加速度数据和磁场数据补偿之后的第一误差向量和第二误差向量相加获取误差；

根据误差修正陀螺仪的角速度值，获取修正后的角速度，利用修正后的角速度更新四元数；

通过修正后的角速度，采用积分方法计算载体的方向角度信息；

其中，所述方法通过改进积分方法来计算载体的方向角度信息，改进算法公式所下示：

其中，Δt为采样间隔，Δω_n,n-1为n点角速度ω_n与n-1点角速度ω_n-1之差，θ_n载体为在n点的角度值，θ_n-1为载体在n-1点的角度值，θ_n-2为载体在n-2点的角度值，ω_i为载体在i点的角速度。

2.根据权利要求1所述的一种基于角速度差值的方向修正方法，其特征在于，所述方法还包括：

根据初始状态下加速度三轴数据、陀螺仪三轴数据以及电子罗盘三轴数据，获取载体的俯仰角、偏航角和滚转角，并获取初始四元数；

将经过滤波处理后的加速度计、陀螺仪和电子罗盘器获取的三轴加速度、三轴角速度以及三轴磁场数据转化为单位向量。

3.根据权利要求1所述的一种基于角速度差值的方向修正方法，其特征在于，所述方法还包括：

对于运动传感器获取的原始数据，转化为载体计算位置信息和方向信息所需要的运动数据，并加以滤波处理。