申请/专利权人：东南大学

申请日：2022-01-19

公开（公告）日：2024-03-19

公开（公告）号：CN114459478B

主分类号：G01C21/18

分类号：G01C21/18;G01C21/20

优先权：

专利状态码：有效-授权

法律状态：2024.03.19#授权;2022.06.10#实质审查的生效;2022.05.10#公开

摘要：本发明公开了一种基于姿态运动学模型的惯性测量单元数据融合方法，涉及姿态测量传感器数据融合技术领域，其技术方案要点是：将运动载体的惯性测量单元实时输出的陀螺仪测量值、加速度计测量值和磁力计测量值进行归一化处理；根据陀螺仪误差模型对标准陀螺仪测量值进行校正；根据惯性测量单元的量测模型对标准加速度计测量值和标准磁力计测量值进行分层解算获得参考载体四元数姿态；将所得的参考四元数姿态和校正陀螺仪数据输入融合观测器获得姿态估计值；对姿态估计值进行误差评估，进一步获得姿态融合值和估计角速度。本发明最后给出的仿真实例表明，所设计的姿态融合方法可以有效、精确的估计出载体的姿态值和角速度值。

主权项：1.一种基于姿态运动学模型的惯性测量单元数据融合方法，其特征是：包括以下步骤：S1、获取当前时刻载体惯性测量单元的测量数据，并将陀螺仪、加速度计、磁力计进行归一化处理；S2、基于陀螺仪误差模型对惯性测量单元的陀螺仪测量数据进行实时校正；步骤S2的具体方法为：S21、计算陀螺仪的误差矩阵：将惯性测量单元单独放置水平面上，将其内部陀螺仪每个轴都相应的转动n圈，通过下式计算陀螺仪的误差矩阵： 其中，E代表陀螺仪的误差矩阵；BO为陀螺仪输出旋转矩阵；为BO的逆矩阵；S22、校正标准陀螺仪的实时测量数据：利用陀螺仪误差矩阵、比例因子和零点漂移偏差将步骤S13得到的标准陀螺仪实时测量数据进行校正，具体如下： 其中，ωk＝[ω1k,ω2k,ω3k]T校正后的k时刻陀螺仪测量数据；hx,hy,hz分别为陀螺仪x,y,z轴的输出比例因子；bx,by,bz为陀螺仪x,y,z轴的零点漂移偏差；S3、基于惯性测量单元的量测模型，将加速度计和磁力计的测量数据分层解算成实时参考四元数姿态；步骤S3的具体方法为：S31、解算参考俯仰角和参考偏航角：将步骤S11中得到的标准加速度计实时测量数据，通过下式转换为载体的参考实时俯仰角和实时参考滚转角： 其中，θk为k时刻载体参考俯仰角；φk为k时刻载体参考滚转角；S32、解算参考偏航角：将步骤S12中得到的标准磁力计实时测量数据和步骤S31中得到的俯仰角和偏航角数据，通过下式转换为载体的实时参考偏航角： 其中，ψk为载体偏航角；S33、转换载体参考欧拉角为实时参考四元数姿态：将步骤S31中得到的参考俯仰角和参考偏航角以及步骤S32中得到的参考偏航角，通过下式转换为实时参考四元数： 其中，qk＝[q0k,q1k,q2k,q3k]T为k时刻载体参考四元数姿态；S4、针对刚体姿态运动学设计融合观测器，将步骤S2中所得的校正陀螺仪测量数据和步骤S3中所得的参考四元数姿态输入融合观测器，得到实时姿态融合估计数据；S5、对步骤S4所得的实时姿态融合估计数据进行姿态运动学误差评估，更新下一时刻姿态融合值和角速度估计值，并返回步骤S1；步骤S5的具体方法为：S51、通过刚体姿态运动学迭代参考四元数姿态：通过刚体姿态运动学迭代下一时刻的参考四元数姿态，具体如下： 其中，qk+1是k+1时刻的参考四元数姿态；姿态矩阵Eqk，表示如下： S52、迭代得到相对四元数误差：迭代下一时刻的四元数相对误差，具体如下： qek+1为k+1时刻的四元数相对误差；ETqek为姿态矩阵Eqek的转置；S53、评估步骤S43得到的估计四元数：计算下一时刻估计四元数与参考四元数的误差，如下： eqk+1为k+1时刻估计四元数与参考四元数的误差；根据离散系统理论，若eqk+1和qek+1的2-范数满足下式：||eqk+1-qk+1||≤1×10-2；则认定四元数误差qek+1会收敛于极小的误差区域，即k+1时刻的姿态估计值与k+1时刻实际姿态值无限接近；S54、更新姿态融合值：根据步骤S53可知，若eqk+1和qek+1满足：||eqk+1-qk+1||≤1×10-2；则下一时刻估计角速度值和姿态融合值为： 其中，为k+1的融合四元数姿态值；若eqk+1和qek+1满足：||eqk+1-qk+1||＞1×10-2；则下一时刻估计角速度值和姿态融合值为： 最后，输出k+1时刻的估计角速度值和姿态融合值并令k＝k+1；并返回S1。

全文数据：

权利要求：

百度查询： 东南大学 一种基于姿态运动学模型的惯性测量单元数据融合方法

免责声明
1、本报告根据公开、合法渠道获得相关数据和信息，力求客观、公正，但并不保证数据的最终完整性和准确性。
2、报告中的分析和结论仅反映本公司于发布本报告当日的职业理解，仅供参考使用，不能作为本公司承担任何法律责任的依据或者凭证。

阅读全文 双屏查看 官方信息 专利公告 收藏专利 下载PDF 下载WORD