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

技术特征：
1.一种基于惯导卫导方位融合的姿态解算方法，其特征在于，包括如下步骤：s1：通过惯导imu器件的三轴陀螺仪和三轴加速度计，在载体坐标系中采集载体静止状态下多组陀螺仪输出数据及多组加速度输出数据；s2：通过多组陀螺仪输出数据估算陀螺零偏，并通过多组加速度输出数据计算加速度均值，并根据所述加速度均值进行水平对准，并计算俯仰角和滚转角，并通过所述俯仰角和所述滚转角构建载体坐标系到水平系的初始姿态转换矩阵；s3：采用卫导方位角对载体方位角进行初始化，并构建初始的姿态转换矩阵；s4：根据各组加速度输出数据和投影值计算水平对准后三轴陀螺仪水平方向的误差值，并根据卫导方位角测量值与当前方位角估算陀螺z轴误差值；s5：根据所述水平方向的误差值、陀螺零偏和陀螺z轴误差值估算陀螺真值，并根据所述陀螺真值更新所述姿态转换矩阵；s6：根据更新后的姿态转换矩阵解算得到姿态角。2.根据权利要求1所述的姿态解算方法，其特征在于，所述s2中通过多组陀螺仪输出数据估算陀螺零偏，并通过多组加速度输出数据计算加速度均值，具体包括：通过陀螺仪零偏公式和陀螺仪输出数据估算陀螺零偏，所述陀螺仪零偏公式为：其中，和为相邻两组陀螺仪输出数据的平均值，且与差的绝对值小于设定的阀值，n为正整数，为陀螺仪输出的三轴角速度向量通过加速度均值公式和加速度输出数据计算加速度均值，所述加速度均值公式为：其中，其中，和为相邻两组加速度数据的平均值，且与差的绝对值小于设定的阀值，n为正整数，为加速度输出的三轴加速度向量根据加速度均值进行水平对准，并计算俯仰角和滚转角：其中，θ为俯仰角，φ为滚转角，通过所述俯仰角和所述滚转角构建载体坐标系到水平系的初始姿态转换矩阵，所述初
始姿态转换矩阵为：3.根据权利要求2所述的姿态解算方法，其特征在于，所述s3中采用卫导方位角对载体方位角进行初始化，并构建载体坐标系到导航坐标系的初始姿态转换矩阵具体为：当首次获得卫导方位角或距离上次获得卫导方位角的时间超过设定时间，则初始化或重置姿态方位角重置姿态方位角为卫导方位角，并根据当前俯仰角θ、滚转角φ初始化或重置载体坐标系到导航坐标系的初始姿态转换矩阵其中，4.根据权利要求3所述的姿态解算方法，其特征在于，所述s4中根据各组加速度输出数据和投影值计算水平对准后三轴陀螺仪水平方向的误差值，具体为：根据陀螺误差值计算公式、各组加速度输出数据和投影值计算水平对准后三轴陀螺仪水平方向的误差值，所述陀螺误差值计算公式为：其中，为陀螺仪水平方向x轴、y轴的误差值，kp
a
＝-0.5，ki
a
＝-0.1，为重力加速度在载体坐标系投影矢量g
b
与重力加速度在载体坐标系的测量值ga
b
的叉乘结果，投影矢量g
b
＝[g
bx g
by g
bx
]
t
，测量矢量ga
b
＝[ga
bx ga
by ga
bx
]
t
，对投影矢量和测量矢量进行归一化处理，记归一化后的投影矢量和测量矢量分别记为g
be
、ga
be
，则g
be
＝g
b
/‖g
b
‖，ga
be
＝ga
b
/‖ga
b
‖，记g
be
＝[g
bxe g
bye g
bxe
]
t
，ga
be
＝[ga
bxe ga
bye ga
bxe
]
t
，，5.根据权利要求4所述的姿态解算方法，其特征在于，所述s4中根据卫导方位角测量值与当前方位角估算陀螺z轴误差值，具体包括：根据卫导方位角更新情况，选取case1、case2其中一种执行，所述case1为：姿态方位角已初始化，且当前时刻获得为卫导方位角，根据偏差公式、卫导方位角测量值和误差值估算陀螺仪z轴误差值所述偏差公式为：
其中，所述case2为：姿态方位角已初始化，且当前时刻未更新，更新基于更新后的更新陀螺仪误差值具体为：其中，其中，为的第i行第j列元素。6.根据权利要求5所述的姿态解算方法，其特征在于，所述s5具体包括：根据估算公式所述水平方向的误差值、陀螺零偏和陀螺z轴误差值估算陀螺真值ω
b
，所述估算公式为：其中，为平滑后的陀螺仪输出数据，k-1和k为相邻时刻，为陀螺零偏，中为陀螺零偏，中分别为向量的x轴和y轴分量，为陀螺仪z轴误差，计算一个采样周期δt时间内的ω
b
的角度增量并更新具体为：其中，为更新后的姿态转换矩阵，((ω
b
*δt)
×
表示向量v＝(ω
b
*δt)＝[vx vy vz]
t
构成的反对称矩阵，7.根据权利要求6所述的姿态解算方法，其特征在于，所述s6具体包括：对更新后的姿态转换矩阵进行单位化及正交化处理，得到处理后的姿态转换矩阵根据第二公式组和姿态转换矩阵得到姿态角数据，所述第二公式组为：
其中，其中，其中，其中，表示矩阵的第i行第j列元素。8.一种基于惯导卫导方位融合的姿态解算装置，其特征在于，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，当所述处理器执行所述计算机程序时，实现如权利要求1至7任一项所述的一种基于惯导卫导方位融合的姿态解算方法。

技术总结
本发明涉及一种基于惯导卫导方位融合的姿态解算方法及装置，属于姿态角测量技术领域；本发明采集载体静止状态的陀螺数据估算陀螺零偏、采集静止状态加速度数据进行水平对准，采用卫导方位角进行方位初始化并计算初始姿态转换矩阵；利用加速度输出在载体系的测量值和投影值，计算陀螺水平方向误差，利用卫导方位角测量值与估算值的偏差计算陀螺z轴误差，根据陀螺误差、零偏、陀螺测量值估算陀螺真值，并更新姿态转换矩阵；根据更新后的姿态转换矩阵计算姿态角。本发明对载体环境要求低，能够较好的代替磁力计进行融合解算，获得稳定可靠的姿态角数据。可靠的姿态角数据。可靠的姿态角数据。


技术研发人员：韩绍伟 程健 李廷
受保护的技术使用者：武汉梦芯科技有限公司
技术研发日：2022.01.24
技术公布日：2022/5/17