一种基于惯导卫导方位融合的姿态解算方法及装置与流程

1.本发明主要涉及姿态角测量技术领域，具体涉及一种基于惯导卫导方位融合的姿态解算方法及装置。


背景技术：

2.姿态角是指物体的横滚角、俯仰角和方位角，用来表征载体的姿态，姿态角的测量在控制、机械、通信、航空航天等技术领域得到了广泛的应用，其主要利用惯性测量单元(inertial measurement unit，imu)进行测量。一般而言，imu包含加速度计和陀螺仪，加速度计用于检测载体三轴的加速度，而陀螺仪用于检测载体三轴角速度信息，通过测量载体在三维空间中的角速度和加速度，并以此解算出物体的姿态。
3.在利用imu解算姿态角信息时，陀螺仪自身的工作原理决定了其长时间的积分会导致误差的累积，使测量值随时间漂移，因此需要利用外部信息对陀螺仪的漂移进行修正。目前，在进行姿态解算时，主要使用加速度计和磁力计的数据修正陀螺仪的漂移。磁力计的原理是测量地球磁场，根据地磁在载体三轴上的分量确定载体朝向，但若使用的环境中存在除地球以外的磁场且这些磁场无法有效屏蔽，或离铁、镍、磁铁、发动机或其它磁性物质很近时，那么磁力计的使用就有很大的问题。


技术实现要素：

4.本发明所要解决的技术问题是针对现有技术的不足，提供一种基于惯导卫导方位融合的姿态解算方法及装置。
5.本发明解决上述技术问题的技术方案如下：一种基于惯导卫导方位融合的姿态解算方法，包括如下步骤：
6.s1：通过惯导imu器件的三轴陀螺仪和三轴加速度计，在载体坐标系中采集载体静止状态下多组陀螺仪输出数据及多组加速度输出数据；
7.s2：通过多组陀螺仪输出数据估算陀螺零偏，并通过多组加速度输出数据计算加速度均值，并根据所述加速度均值进行水平对准，并计算俯仰角和滚转角，并通过所述俯仰角和所述滚转角构建载体坐标系到水平系的初始姿态转换矩阵；
8.s3：采用卫导方位角对载体方位角进行初始化，并构建初始的姿态转换矩阵；
9.s4：根据各组加速度输出数据和投影值计算水平对准后三轴陀螺仪水平方向的误差值，并根据卫导方位角测量值与当前方位角估算陀螺z轴误差值；
10.s5：根据所述水平方向的误差值、陀螺零偏和陀螺z轴误差值估算陀螺真值，并根据所述陀螺真值更新所述姿态转换矩阵；
11.s6：根据更新后的姿态转换矩阵解算得到姿态角
12.本发明的有益效果是：对载体环境要求低，能够较好的代替磁力计进行融合解算，修正陀螺积分产生方位解偏差，获得稳定可靠的姿态角数据。
13.本发明解决上述技术问题的另一技术方案如下：一种基于惯导卫导方位融合的姿
态解算装置，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，当所述处理器执行所述计算机程序时，实现如上所述的一种基于惯导卫导方位融合的姿态解算方法。
附图说明
14.图1为本发明实施例提供的姿态解算方法的流程示意图；
15.图2为本发明实施例提供的姿态解算方法的数据流向示意图。
具体实施方式
16.以下结合附图对本发明的原理和特征进行描述，所举实例只用于解释本发明，并非用于限定本发明的范围。
17.近年来卫星导航技术发展迅速，成本大幅下降，高精度卫导定位传感器和惯性测量单元(imu)已经成为户外特定场景低速无人驾驶车辆、辅助驾驶车辆、位置形变监测场景必备的传感器，且这些使用场景中无法满足磁力计使用环境要求或无法满足磁力计校准条件。本发明利用卫导解算出的方位信息代替磁力计进行融合解算，修正陀螺积分产生的方位角漂移，得到准确的姿态角数据。
18.实施例1：
19.如图1-2所示，一种基于惯导卫导方位融合的姿态解算方法，包括如下步骤：
20.s1：通过惯导imu器件的三轴陀螺仪和三轴加速度计，在载体坐标系中采集载体静止状态下多组陀螺仪输出数据及多组加速度输出数据；
21.s2：通过多组陀螺仪输出数据估算陀螺零偏，并通过多组加速度输出数据计算加速度均值，并根据所述加速度均值进行水平对准，并计算俯仰角和滚转角，并通过所述俯仰角和所述滚转角构建载体坐标系到水平系的姿态转换矩阵；
22.s3：采用卫导方位角对载体方位角进行初始化，并构建初始的姿态转换矩阵；
23.s4：根据各组加速度输出数据和投影值计算水平对准后三轴陀螺仪水平方向的误差值，并根据卫导方位角测量值与当前方位角估算陀螺z轴误差值；
24.s5：根据所述水平方向的误差值、陀螺零偏和陀螺z轴误差值估算陀螺真值，并根据所述陀螺真值更新所述姿态转换矩阵；
25.s6：根据更新后的姿态转换矩阵解算得到姿态角。
26.应理解地，惯导指的是imu器件，imu内有三轴陀螺仪和三轴加速度计，用于测量三轴角速度和三轴加速度。
27.上述实施例中，采集载体静止状态的陀螺数据估算陀螺零偏、采集静止状态加速度数据进行水平对准，采用卫导方位角进行方位初始化并计算初始姿态转换矩阵；利用加速度输出在载体系的测量值和投影值，计算陀螺水平方向误差，利用卫导方位角测量值与计算值的偏差估算陀螺z轴误差，根据陀螺误差、零偏、陀螺测量值估算陀螺真值，并更新姿态转换矩阵，根据更新后的姿态转换矩阵计算姿态角；本发明对载体环境的要求低，能够较好的代替磁力计进行融合解算，修正陀螺积分产生方位解偏差，获得稳定可靠的姿态角数据。
28.如图2所示，具体地，所述s2中通过多组陀螺仪输出数据估算陀螺零偏，并通过多
组加速度输出数据计算加速度均值，具体包括：通过陀螺仪偏
29.公式和陀螺仪输出数据估算陀螺零偏，所述陀螺仪零偏公式为：
[0030][0031]
其中，和为相邻两组陀螺仪输出数据的平均值，且与差的绝对值小于设定的阀值，n为正整数，为陀螺仪输出的三轴角速度向量
[0032]
通过加速度均值公式和加速度输出数据计算加速度均值，所述加速度均值公式为：
[0033]
其中，其中，和为相邻两组加速度数据的平均值，且与差的绝对值小于设定的阀值，n为正整数，为加速度输出的三轴加速度向量
[0034]
根据加速度均值进行水平对准，并计算俯仰角和滚转角：
[0035][0036]
其中，θ为俯仰角，φ为滚转角。
[0037]
通过所述俯仰角和所述滚转角构建载体坐标系到水平系的姿态转换矩阵，所述姿态转换矩阵为：
[0038][0039]
应理解地，三个姿态角与姿态转换矩阵为等价关系，有三个姿态角可计算姿态转换矩阵，有姿态转换矩阵可计算三个姿态角。此外这里只有两个角度，没有方位角，方位角记为0计算在没有方位角的情况下，算法可正常运行计算准确的俯仰角和滚转角。
[0040]
具体地，所述s3中构建载体坐标系到导航坐标系的初始姿态转换矩阵具体包括：
[0041]
当首次获得卫导方位角或距离上次获得卫导方位角的时间超过设定时间，则初始化或重置姿态方位角化或重置姿态方位角为卫导方位角，并根据当前俯仰角θ、滚转角φ初始化
或重置载体坐标系到导航坐标系的初始姿态转换矩阵其中，
[0042][0043]
应理解地，采用卫导方位角与惯导imu组合解算姿态角，卫导双天线与imu器件需要固连在同一载体平台，且卫导双天线与imu器件x轴平行，指向同一方向；若卫导双天线与imu器件需要固连，但imu器件方位角与卫导方位角存在固定方位偏差，在初始化和估算陀螺z轴误差值时需要用此固定方位偏差计算出imu器件的方位后再进行初始化和误差估计。
[0044]
具体地，所述s4中根据各组加速度输出数据和投影值计算水平对准后三轴陀螺仪水平方向的误差值，具体为：
[0045]
根据陀螺误差值计算公式、各组加速度输出数据和投影值计算水平对准后三轴陀螺仪水平方向的误差值，所述陀螺误差值计算公式为：
[0046][0047]
其中，其中，为陀螺仪水平方向x轴、y轴的误差值，理想情况下项计算结果为0，但由于实际误差的存在，该项为一个小值，为分离俯仰角、滚转角和航向角通道，避免耦合误差，不计入陀螺z轴误差项；kpa＝-0.5，kia＝-0.1，为重力加速度在载体坐标系投影矢量gb与重力加速度在载体坐标系的测量值gab的叉乘结果，投影矢量gb＝[g
bx g
by g
bx
]
t
，测量矢量gab＝[ga
bx ga
by ga
bx
]
t
，对投影矢量和测量矢量进行归一化处理，记归一化后的投影矢量和测量矢量分别记为g
be
、ga
be
，则g
be
＝gb/‖gb‖，ga
be
＝gab/‖gab‖，
[0048]
记
[0049][0050]
具体地，所述s4中根据卫导方位角测量值与当前方位角估算陀螺z轴误差值，具体包括：
[0051]
根据卫导方位角更新情况，选取case1、case2其中一种执行，
[0052]
所述case1为：
[0053]
姿态方位角已初始化，且当前时刻获得卫导方位角根据偏差公式、卫导方位角测量值和误差值估算陀螺仪z轴误差值所述偏差公式为：
[0054]
[0055]
其中，
[0056]
所述case2为：
[0057]
姿态方位角已初始化，且当前时刻卫导方位角未更新，更新基于更新后的更新陀螺仪误差值具体为：
[0058][0059]
其中，其中，为的第i行第j列元素。
[0060]
应理解地，每一次更新这两种情况的其中一种会发生。第一种是初始化后收到更新的方位角，第二种是因为一般情况下卫导方位角的更新频率低于imu的更新频率，而算法的更新频率与imu更新频率相同，在imu数据更新而卫导方位角没有更新的时刻用case2。
[0061]
如图2所示，具体地，所述s5具体包括：
[0062]
根据估算公式所述水平方向的误差值、陀螺零偏和陀螺z轴误差值估算陀螺真值ωb，所述估算公式为：
[0063][0064]
其中，为平滑后的陀螺仪输出数据，k-1和k为相邻时刻，为陀螺零偏，中为陀螺零偏，中分别为向量的x轴和y轴分量，为陀螺仪z轴误差，
[0065]
计算一个采样周期δt时间内的ωb的角度增量并更新具体为：
[0066][0067]
其中，为更新后的姿态转换矩阵，((ωb*δt)
×
)表示向量v＝(ωb*δt)＝[vx vy vz]
t
构成的反对称矩阵，
[0068]
具体地，所述s6具体包括：
[0069]
对更新后的姿态转换矩阵进行单位化及正交化处理，得到处理后的姿态转换矩阵
[0070]
根据第二公式组和姿态转换矩阵得到姿态角数据，所述第二公式组为：
[0071]
其中，其中，其中，其中，其中，表示矩阵的第i行第j列元素。
[0072]
实施例2：
[0073]
一种基于惯导卫导方位融合的姿态解算装置，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，当所述处理器执行所述计算机程序时，实现如上所述的一种基于惯导卫导方位融合的姿态解算方法。
[0074]
需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。
[0075]
所属领域的技术人员可以清楚地了解到，为了描述的方便和简洁，上述描述的系统和单元的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。
[0076]
在本技术所提供的几个实施例中，应该理解到，所揭露的系统和方法，可以通过其它的方式实现。例如，以上所描述的系统实施例仅仅是示意性的，例如，单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。
[0077]
作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本发明实施例方案的目的。
[0078]
另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以是两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。
[0079]
以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和
原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。