买专利卖专利找龙图腾，真高效！ 查专利查商标用IPTOP,全免费！专利年费监控用IP管家,真方便！

申请/专利权人：北京理工大学

摘要：本发明公开了一种角加速度传感器辅助陀螺仪的姿态解算方法，利用角加速度传感器数据提供观测信息校正陀螺仪误差，提升陀螺仪姿态解算的精度；本发明在陀螺仪测量的角速度原始数据基础上，引入角加速度传感器直接测量的角加速度数据作为观测信息，设计一种基于卡尔曼滤波的角加速度传感器与陀螺仪的信息融合方法；该方法考虑了角加速度传感器和陀螺仪两种惯性器件的数据特征，设计了两种不同的传感器数据组合方案，提升角速度测量的精度，进而提高载体姿态解算的精度。本发明的方法只需同时将角加速度传感器和陀螺仪与载体固连，安装方式较简单，对环境的适应性较强。同时，引入的角加速度传感器成本较低，便于开展大规模应用。

主权项：1.一种角加速度传感器辅助陀螺仪的姿态解算方法，其特征在于，包括如下步骤：步骤1、数据采集：利用陀螺仪和角加速度传感器，分别采集载体运动时的角速度和角加速度原始数据；步骤2、数据预处理：依据步骤1中采集到原始数据的时间戳信息，通过数据插值的方式，实现两种传感器数据的数据对齐；预处理后的角速度数据记为wgyro，角加速度数据记为aaas。步骤3、根据两种惯性传感器的数据特点，设计两种系统状态量，用卡尔曼滤波实现传感器的信息融合：对于组合模式1，状态量为陀螺仪和角加速度传感器的测量误差，公式如下：xk＝[δwgyroδaaas]T其中，xk代表k时刻的状态量，δwgyro代表陀螺仪的测量误差，δaaas代表角加速度传感器的测量误差；对于组合模式2，选取陀螺仪连续两个时刻之间角速度的增量作为系统状态量，公式如下： 其中，xk代表k时刻的状态量，和分别代表k+1时刻和k时刻陀螺仪采集的角速度；步骤4、根据步骤3中选取的两种系统状态量，设计与之对应的系统观测量：对于组合模式1，根据陀螺仪和角加速度传感器当前时刻获得的角速度和角加速度，得到下一时刻的角速度；该数据与陀螺仪在相应时刻收集的角速度之间的差值被用作系统观测量，公式如下： 其中，zk代表k时刻的状态量，和分别代表k时刻和k-1时刻陀螺仪采集的角速度，代表k-1时刻角加速度传感器采集的角加速度，T代表数据对齐后的时间间隔；对于组合模式2，利用角加速度数据一次积分，获取角加速度传感器采集的角速度，进一步计算连续两个时刻之间角速度的增量，选为系统的观测量，公式如下： 其中，zk代表k时刻的状态量，和分别代表k时刻和k-1时刻角加速度传感器数据积分获取的角速度；步骤5、基于步骤3中选取的两种系统状态量，定义系统状态转移矩阵F，进行卡尔曼滤波的时间更新：对于组合模式1，定义系统状态转移矩阵如下式： 其中，τgyro和τaas分别是陀螺仪和角加速度传感器的马尔科相关时间，03×3是3阶的零矩阵；对于组合模式2，定义系统状态转移矩阵如下式：F＝I3×3其中，I3×3是3阶的单位阵；时间更新过程如下式： 其中，代表系统状态预测，xk-1代表k-1时刻的系统状态，Pk∣k-1代表系统状态协方差矩阵预测，Pk-1代表k-1时刻的系统状态协方差矩阵，Qd代表系统过程噪声方差矩阵；步骤6、基于步骤4中设计的两种系统观测量，定义系统观测矩阵H，进行卡尔曼滤波的量测更新：对于组合模式1，定义系统观测矩阵如下式：H＝[I3×303×3]其中，I3×3是3阶的单位阵，03×3是3阶的零矩阵；对于组合模式2，定义系统观测矩阵如下式：H＝I3×3其中，I3×3是3阶的单位阵。量测更新过程如下式： 其中，K代表卡尔曼滤波增益，Pk∣k-1代表系统状态协方差矩阵预测，Pk代表k时刻的系统状态协方差矩阵，R代表系统过程噪声方差矩阵，xk代表k时刻的系统状态，代表系统状态预测，zk代表k时刻的系统观测，I代表单位阵；步骤7、误差补偿：经过步骤6的卡尔曼滤波量测更新，获取角加速度数据的补偿值，进一步利用其校正陀螺仪原始角速度数据中的误差，获取校正角加速度wfusion＝[wxwywz]T；步骤8、姿态解算；使用四元数Q表示载体姿态，利用步骤7获取的校正过的角速度数据wfusion，得到四元数关于角速度的实时更新的微分方程，公式如下： 其中， 利用四阶龙格库塔法解算上述四元数微分方程，可获取连续时刻的四元数数据，将四元数转换为姿态角参数，即可完成姿态解算；步骤9、依据当前场景下的姿态角解算精度选择其中一种模式实现传感器组合；重复步骤1到步骤8，获取连续的载体运动的姿态角数据。

全文数据：

权利要求：

百度查询： 北京理工大学 一种角加速度传感器辅助陀螺仪的姿态解算方法