欢迎来到知嘟嘟! 联系电话:13095918853 卖家免费入驻,海量在线求购! 卖家免费入驻,海量在线求购!
知嘟嘟
我要发布
联系电话:13095918853
知嘟嘟经纪人
收藏
专利号: 2017108040833
申请人: 常州大学
专利类型:发明专利
专利状态:已下证
专利领域: 测量;测试
更新日期:2023-12-11
缴费截止日期: 暂无
价格&联系人
年费信息
委托购买

摘要:

权利要求书:

1.一种用于惯性-地磁组合姿态解算的自适应optimal-REQUEST算法,其特征在于:包括陀螺仪、加速度计和地磁传感器,以陀螺仪、加速度计和地磁传感器输出构成的三维观测矢量作为输入变量,并包括以下步骤,步骤一:在每个采样时刻k,计算

其中, 和 分别为k时刻由加速度计和地磁传感器输出构成的三维观测矢量;||·||表示取模;×表示取向量积;asin()表示取反正弦; 代表 和 这两个矢量的夹角;

步骤二:计算

其中,α1和α2为比例因子,abs(·)表示取绝对值,g为当地重力加速度矢量;

步骤三:为了将步骤二中 取值范围单调地转化为[0,1]区间,因此,计算步骤四:根据步骤三得到的 计算姿态的“新息”δKk,

式中,各符号定义如下: 该数值表示optimal-REQUEST算法赋予所有观测向量的权值的和; S=B+BT,

I为单位矩阵; 和 分别为同一观测矢量在载体坐标系和世界坐标系下的表示;其中 在载体坐标系和世界坐标系下的表示分别为 和 而 在载体坐标系和世界坐标系下的表示分别为 和步骤五:计算姿态的“预测”Kk/k-1,如果k=0,即初始计算时刻,则令Kk/k=δKk,mk=δmk,Pk/k=Rk,其中Rk为观测噪声协方差阵,则直接跳至步骤七,否则利用上一时刻的姿态验后估计误差的协方差矩阵Kk-1/k-1根据式(10)计算姿态的“预测”,即4×4维的Kk/k-1矩阵,然后利用式(11)计算姿态“预测”,即姿态先验估计的估计误差的协方差矩阵Pk/k-1,其中,Qk-1为过程噪声协方差阵;Φk-1为状态转移阵,且Φk-1=exp(Ωk-1Δt),Ωk-1为反对称阵,且反对称阵Ωk-1定义为其中,ωk-1为由陀螺仪输出构成的三维矢量;

步骤六:利用姿态的“新息”实现对姿态“预测”的修正,即得到Kk/k;

首先,利用上一时刻的累加权值和mk-1、步骤四得到的权值和δmk和步骤五得到的姿态先验估计误差的协方差矩阵Pk/k-1计算加权因子ρk,然后利用上一时刻的累加权值和mk-1和步骤四得到的权值和δmk计算当前时刻的累加权值和mk,mk=(1-ρk)mk-1+ρkδmk  (14)

有了当前时刻的累加权值和mk和加权因子ρk,就可以实现对姿态的修正,即得到矩阵Kk/k,最后计算当前时刻的姿态验后估计误差的协方差矩阵Pk/k,保存Pk/k和mk,以便于进行下一轮的计算;

步骤七:获得Kk/k矩阵后,将会根据该矩阵计算当前时刻的姿态,计算与矩阵Kk/k的最大特征值相对应的归一化的特征向量,该特征向量为该计算时刻以四元数表示的姿态计算结果;返回至步骤一以便进行下一时刻姿态解算,直至在某个时刻用户停止姿态计算。

2.如权利要求1所述的用于惯性-地磁组合姿态解算的自适应optimal-REQUEST算法,其特征在于:采用滤波技术对式(1)的计算结果进行去噪。