CN109916399B - 一种阴影下的载体姿态估计方法 - Google Patents
一种阴影下的载体姿态估计方法 Download PDFInfo
- Publication number
- CN109916399B CN109916399B CN201910268878.6A CN201910268878A CN109916399B CN 109916399 B CN109916399 B CN 109916399B CN 201910268878 A CN201910268878 A CN 201910268878A CN 109916399 B CN109916399 B CN 109916399B
- Authority
- CN
- China
- Prior art keywords
- axis
- carrier
- angular velocity
- data processor
- initial
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 44
- 238000005070 sampling Methods 0.000 claims description 111
- 230000001133 acceleration Effects 0.000 claims description 60
- 239000011159 matrix material Substances 0.000 claims description 37
- 230000005484 gravity Effects 0.000 claims description 4
- 238000012937 correction Methods 0.000 claims description 3
- 230000010354 integration Effects 0.000 claims description 3
- 230000015572 biosynthetic process Effects 0.000 claims description 2
- 238000001514 detection method Methods 0.000 claims description 2
- 238000003786 synthesis reaction Methods 0.000 claims description 2
- 238000013461 design Methods 0.000 abstract description 4
- 230000015556 catabolic process Effects 0.000 description 3
- 238000006731 degradation reaction Methods 0.000 description 3
- 230000000694 effects Effects 0.000 description 3
- 238000005259 measurement Methods 0.000 description 3
- 238000013459 approach Methods 0.000 description 2
- 238000004364 calculation method Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 238000010923 batch production Methods 0.000 description 1
- 230000000295 complement effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000007812 deficiency Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
Landscapes
- Gyroscopes (AREA)
- Navigation (AREA)
Abstract
本发明公开了一种阴影下的载体姿态估计方法,包括以下步骤:一、建立载体坐标系;二、载体姿态初始参数的获取;三、基于退化四元数的观测器模型的建立并获取载体的姿态估计值。本发明方法步骤简单,设计合理且实现方便,建立基于退化四元数的观测器模型,将航向角与姿态角估计分离出来,在阴影无GPS信息情况下只利用三轴加速度计和三轴陀螺估计载体姿态角,并能在低动态下通过融合三轴加速度计和三轴陀螺输出进行姿态估计,在机动状态下切换到仅依赖三轴陀螺进行姿态估计,具有较高的姿态精度。
Description
技术领域
本发明属于微惯性航姿技术领域,尤其是涉及一种阴影下的载体姿态估计方法。
背景技术
微机械陀螺(即MEMS陀螺)因具有成本低、体积小和便于批量生产等优点,在机器人、无人机等领域具有广泛的应用前景。然而,与激光陀螺和光纤陀螺相比,微机械陀螺存在较大的误差,在许多对精度要求相对较高的领域很难得到应用。
由于微机械陀螺单独工作的精度低,不能长时间提供稳定的姿态,一般需要与加速度计和GPS等辅助信息组合使用,通过姿态估计算法修正陀螺的漂移误差。典型的姿态估计算法大致分为两类:一类是从频率域来分辨和消除噪声,不需考虑信号的统计特性,如互补滤波器;另一类是使用状态空间法在时域内设计滤波器,如扩展卡尔曼滤波器和无迹卡尔曼滤波器等。姿态估计算法能够发挥各个惯性器件的优势,取长补短,提高***精度的同时放宽了各个惯性器件的精度要求。一种典型的方法是采用加速度计和单基线GPS提供复杂观测信息,对陀螺误差实时校正。针对加速度计易受机动加速度的影响,在载体机动时会干扰加速度计对重力的测量。为了克服机动加速度对姿态估计的影响,一种可行的方法是利用GPS测量的加速度信息补偿加速度计非重力加速度的影响。但是GPS对空视要求较高,极易受到阴影遮挡或者干扰影响。上述无法得到应用于无GPS信号或者GPS信号受到干扰的情况。
发明内容
本发明所要解决的技术问题在于针对上述现有技术中的不足,提供一种阴影下的载体姿态估计方法,其方法步骤简单,设计合理且实现方便,建立基于退化四元数的观测器模型,将航向角与姿态角估计分离出来,在阴影无GPS信息情况下只利用三轴加速度计和三轴陀螺估计载体姿态角,并能在低动态下通过融合三轴加速度计和三轴陀螺输出进行姿态估计,在机动状态下切换到仅依赖三轴陀螺进行姿态估计,具有较高的姿态精度,有效地应用于无GPS信号或者GPS信号受到干扰的情况,实用性强。
为解决上述技术问题,本发明采用的技术方案是:一种阴影下的载体姿态估计方法,该方法所采用的装置包括数据处理器、设置在载体上的三轴陀螺和三轴加速度计,其特征在于,该方法包括以下步骤:
步骤一、建立载体坐标系:
以载体的质心为原点o,oxb轴与所述载体的纵轴重合且指向所述载体前进方向,oyb轴与所述载体的横轴重合且指向所述载体前进方向的右侧,ozb轴垂直于所述oxb轴和所述oyb轴形成的平面oxbyb且指向所述载体下方,建立载体坐标系oxbybzb;
步骤二、载体姿态初始参数的获取:
步骤201、三轴加速度计对载体初始时刻的总加速度进行检测,并将检测到的初始时刻总加速度发送至数据处理器,数据处理器根据公式和得到载体的初始俯仰角φ(0)和载体的初始横滚角θ(0);g表示重力加速度,且g=9.8m/s2;其中,fx(0)表示三轴加速度计测量到的初始时刻总加速度在oxb轴上的加速度分量,fy(0)表示三轴加速度计测量到的初始时刻总加速度在oyb轴上的加速度分量,fz(0)表示三轴加速度计测量到的初始时刻总加速度在ozb轴上的加速度分量;
步骤202、采用所述数据处理器根据公式得到初始的虚拟旋转矩阵
步骤203、采用所述数据处理器根据公式得到退化四元数中初始的标量参数q0(0)、退化四元数中初始的第一矢量参数q1(0)和退化四元数中初始的第二矢量参数q2(0);
步骤三、基于退化四元数的观测器模型的建立并获取载体的姿态估计值:
步骤301、j为自然数,即j≥0,当j≥1时,三轴陀螺对载体的角速度进行检测,并将检测到的角速度发送至数据处理器,采用所述数据处理器根据公式得到第j个采样时刻载体在oxb轴上的角速度分量估计值采用所述数据处理器根据公式得到第j个采样时刻载体在oyb轴上的角速度分量估计值采用所述数据处理器根据公式得到第j个采样时刻载体在ozb轴上的角速度分量估计值其中,ωx(j)表示三轴陀螺第j个采样时刻检测到的角速度在oxb轴上的角速度分量,ωy(j)表示三轴陀螺第j个采样时刻检测到的角速度在oyb轴上的角速度分量,ωz(j)表示三轴陀螺第j个采样时刻检测到的角速度在ozb轴上的角速度分量,表示第j-1个采样时刻三轴陀螺在oxb轴上的零偏估计值,表示第j-1个采样时刻三轴陀螺在oyb轴上的零偏估计值,表示第j-1个采样时刻三轴陀螺在ozb轴上的零偏估计值;
当j=0时,三轴陀螺对载体的角速度进行检测,并将检测到的角速度发送至数据处理器,采用所述数据处理器根据公式得到初始时刻载体在oxb轴上的角速度分量估计值采用所述数据处理器根据公式得到初始时刻载体在oyb轴上的角速度分量估计值采用所述数据处理器根据公式得到初始时刻载体在ozb轴上的角速度分量估计值其中,ωx(0)表示三轴陀螺初始时刻检测到的角速度在oxb轴上的角速度分量,ωy(0)表示三轴陀螺初始时刻检测到的角速度在oyb轴上的角速度分量,ωz(0)表示三轴陀螺初始时刻检测到的角速度在ozb轴上的角速度分量,表示三轴陀螺在oxb轴上的初始零偏估计值,表示三轴陀螺在oyb轴上的初始零偏估计值,表示三轴陀螺在ozb轴上的初始零偏估计值;
步骤302、采用所述数据处理器根据公式得到第j个采样时刻载体的径向加速度判断门限τω(j);其中,ξω(j)表示第j个采样时刻的载体径向加速度阈值;
采用所述数据处理器根据公式得到第j个采样时刻载体的切向加速度判断门限τa(j);其中,ξa表示单位采样时刻内俯仰角的变化量阈值,fx(j)表示第j个采样时刻三轴加速度计测量到的总加速度在oxb轴上的加速度分量;
采用所述数据处理器综合和得到第j个采样时刻的综合判断条件其中,τ(j)表示第j个采样时刻的综合判断参数;
步骤303、采用所述数据处理器建立基于退化四元数的观测器模型并输入初始虚拟旋转矩阵和步骤302中第j个采样时刻的综合判断参数τ(j),得到第j+1个采样时刻的虚拟旋转矩阵估计值其中,kp表示观测器比例增益系数,且kP>0,kI表示观测器积分增益系数,且kI>0,km表示观测量权值,km>0,表示当地重力加速度归一化向量,且 表示三轴陀螺第j个采样时刻检测到的角速度分量向量,且 表示三轴陀螺第j个采样时刻的零偏估计向量,且 表示三轴陀螺第j个采样时刻在oxb轴上的零偏估计值,表示三轴陀螺第j个采样时刻在oyb轴上的零偏估计值,表示三轴陀螺第j个采样时刻在ozb轴上的零偏估计值,表示第j个采样时刻的虚拟旋转矩阵估计值的转置,σ(j)表示第j个采样时刻的校正向量;S(·)表示斜对称矩阵,ΔT表示采样时间;
步骤303、采用所述数据处理器建立载体第j+1个采样时刻的虚拟旋转矩阵并带入步骤304中得到的第j+1个采样时刻的虚拟旋转矩阵估计值得到第j+1个采样时刻退化的四元数矩阵其中,其中,q0(j+1)表示退化四元数中第j+1个采样时刻的标量参数,q1(j+1)表示退化四元数中第j+1个采样时刻的第一矢量参数,q2(j+1)表示退化四元数中第j+1个采样时刻的第二矢量参数;
步骤304、采用所述数据处理器根据公式得到第j+1个采样时刻载体的俯仰角估计值和载体的横滚角估计值
上述的一种阴影下的载体姿态估计方法,其特征在于:步骤301中三轴陀螺的初始零偏估计值的获取过程如下:
步骤3011、载体静止时,三轴陀螺对载体的角速度进行检测,并将检测到的角速度发送至数据处理器,数据处理器对三轴陀螺在oxb轴上的多个角速度分量、三轴陀螺在oyb轴上的多个角速度分量和三轴陀螺在ozb轴上的多个角速度分量分别进行采集,并将数据处理器第i次采集到的三轴陀螺在oxb轴上的角速度分量记作b′x(i),数据处理器第i次采集到的三轴陀螺在oyb轴上的角速度分量记作b′y(i),数据处理器第i次采集到的三轴陀螺在ozb轴上的角速度分量记作b′z(i);
步骤3012、采用所述数据处理器根据公式得到三轴陀螺在oxb轴上的初始零偏估计值采用所述数据处理器根据公式得到三轴陀螺在oyb轴上的初始零偏估计值采用所述数据处理器根据公式得到三轴陀螺在ozb轴上的初始零偏估计值其中,N表示采集总次数,i和N均为正整数,且i的取值范围为1~N,N的取值范围为10~20。
上述的一种阴影下的载体姿态估计方法,其特征在于:所述三轴陀螺包括设置在所述载体坐标系中所述oxb轴上且对载体在oxb轴上的角速度分量进行检测的第一微机械陀螺、设置在所述载体坐标系中所述oyb轴上且对载体在oyb轴上的角速度分量进行检测的第二微机械陀螺和设置在所述载体坐标系中所述ozb轴上且对载体在ozb轴上的角速度分量进行检测的第三微机械陀螺,所述第一微机械陀螺、第二微机械陀螺和第三微机械陀螺的输出端均与数据处理器的输入端连接。
上述的一种阴影下的载体姿态估计方法,其特征在于:所述数据处理器为单片机、DSP微控制器或者ARM微控制器。
上述的一种阴影下的载体姿态估计方法,其特征在于:步骤302中第j个采样时刻的载体径向加速度阈值ξω(j)取值为
上述的一种阴影下的载体姿态估计方法,其特征在于:步骤303中单位时间间隔内俯仰角的变化量阈值ξa取值为ωmax表示载体俯仰角的最大变化率,且ωmax的取值范围为2rad/秒~10rad/秒,t表示积分时间。
上述的一种阴影下的载体姿态估计方法,其特征在于:采样时间ΔT的取值范围为0.005秒~0.1秒。
上述的一种阴影下的载体姿态估计方法,其特征在于:步骤303中观测器比例增益系数kp的取值范围为0<kP<10,观测器积分增益系数kI的取值范围为0<kI<10,观测量权值km的取值范围为0<km<10。
本发明与现有技术相比具有以下优点:
1、所采用的阴影下的载体姿态估计方法步骤简单、实现方便且操作简便,确保载体在阴影无法获取GPS信息情况下,准确地得到载体的姿态,即载体的俯仰角估计值和载体的横滚角估计值。
2、所采用的阴影下的载体姿态估计方法操作简便且使用效果好,首先利用三轴加速度计得到载体的初始俯仰角和载体的初始横滚角,进而根据载体的初始俯仰角和载体的初始横滚角得到初始虚拟旋转矩阵和四元数初始三个参数;然后进行载体机动加速度的判断,以便于获取综合判断参数;最后建立基于退化四元数的观测器模型,并输入初始虚拟旋转矩阵和综合判断参数,实现在低动态下通过融合三轴加速度计和三轴陀螺输出进行姿态估计,在机动状态下切换到仅依赖三轴陀螺进行姿态估计,能有效地适应于载体的不同运动状态,准确度高。
3、所采用的载体机动加速度的判断过程中,一方面将载体的在ozb轴上的角速度分量估计值与载体径向加速度阈值进行判断,以便于当载体的在ozb轴上的角速度分量估计值大于载体径向加速度阈值时,使综合判断参数等于零,对基于退化四元数的观测器模型依赖三轴陀螺进行姿态估计;另一方面,当|arcsin(fx(j))+arcsin(2q0(j)q2(j))|>ξa进行判断,以便于载体存在切向加速度时,使综合判断参数等于零,对基于退化四元数的观测器模型依赖三轴陀螺进行姿态估计;其他运动过程中,综合判断参数等于1,实现在低动态下通过融合三轴加速度计和三轴陀螺输出进行姿态估计。
4、所采用的通过基于退化四元数的观测器模型得到退化的四元数矩阵,就是将四元数矩阵中的第四个参数等于0状态下获取,从而能在无法获取GPS信息情况(即航向角的情况下)进行横滚角和俯仰角的获取。
综上所述,本发明步骤简单,设计合理且实现方便,建立基于退化四元数的观测器模型,将航向角与姿态角估计分离出来,在阴影无GPS信息情况下只利用三轴加速度计和三轴陀螺估计载体姿态角,并能在低动态下通过融合三轴加速度计和三轴陀螺输出进行姿态估计,在机动状态下切换到仅依赖三轴陀螺进行姿态估计,具有较高的姿态精度,有效地应用于无GPS信号或者GPS信号受到干扰的情况,实用性强。
下面通过附图和实施例,对本发明的技术方案做进一步的详细描述。
附图说明
图1为本发明所采用装置的电路原理框图。
图2为本发明的方法流程框图。
图3为利用本发明方法获得的载体的姿态角估计值与利用高精度惯导获得姿态角测量值的曲线图。
附图标记说明:
1—数据处理器; 2—三轴加速度计; 3—三轴陀螺;
3-1—第一微机械陀螺; 3-2—第二微机械陀螺; 3-3—第三微机械陀螺。
具体实施方式
如图1和图2所示的一种阴影下的载体姿态估计方法,该方法所采用的装置包括数据处理器1、设置在载体上的三轴陀螺3和三轴加速度计2,该方法包括以下步骤:
步骤一、建立载体坐标系:
以载体的质心为原点o,oxb轴与所述载体的纵轴重合且指向所述载体前进方向,oyb轴与所述载体的横轴重合且指向所述载体前进方向的右侧,ozb轴垂直于所述oxb轴和所述oyb轴形成的平面oxbyb且指向所述载体下方,建立载体坐标系oxbybzb;
步骤二、载体姿态初始参数的获取:
步骤201、三轴加速度计2对载体初始时刻的总加速度进行检测,并将检测到的初始时刻总加速度发送至数据处理器1,数据处理器1根据公式和得到载体的初始俯仰角φ(0)和载体的初始横滚角θ(0);g表示重力加速度,且g=9.8m/s2;其中,fx(0)表示三轴加速度计2测量到的初始时刻总加速度在oxb轴上的加速度分量,fy(0)表示三轴加速度计2测量到的初始时刻总加速度在oyb轴上的加速度分量,fz(0)表示三轴加速度计2测量到的初始时刻总加速度在ozb轴上的加速度分量;
步骤202、采用所述数据处理器1根据公式得到初始的虚拟旋转矩阵
步骤203、采用所述数据处理器1根据公式得到退化四元数中初始的标量参数q0(0)、退化四元数中初始的第一矢量参数q1(0)和退化四元数中初始的第二矢量参数q2(0);
步骤三、基于退化四元数的观测器模型的建立并获取载体的姿态估计值:
步骤301、j为自然数,即j≥0,当j≥1时,三轴陀螺3对载体的角速度进行检测,并将检测到的角速度发送至数据处理器1,采用所述数据处理器1根据公式得到第j个采样时刻载体在oxb轴上的角速度分量估计值采用所述数据处理器1根据公式得到第j个采样时刻载体在oyb轴上的角速度分量估计值采用所述数据处理器1根据公式得到第j个采样时刻载体在ozb轴上的角速度分量估计值其中,ωx(j)表示三轴陀螺3第j个采样时刻检测到的角速度在oxb轴上的角速度分量,ωy(j)表示三轴陀螺3第j个采样时刻检测到的角速度在oyb轴上的角速度分量,ωz(j)表示三轴陀螺3第j个采样时刻检测到的角速度在ozb轴上的角速度分量,表示第j-1个采样时刻三轴陀螺3在oxb轴上的零偏估计值,表示第j-1个采样时刻三轴陀螺3在oyb轴上的零偏估计值,表示第j-1个采样时刻三轴陀螺3在ozb轴上的零偏估计值;
当j=0时,三轴陀螺3对载体的角速度进行检测,并将检测到的角速度发送至数据处理器1,采用所述数据处理器1根据公式得到初始时刻载体在oxb轴上的角速度分量估计值采用所述数据处理器1根据公式得到初始时刻载体在oyb轴上的角速度分量估计值采用所述数据处理器1根据公式得到初始时刻载体在ozb轴上的角速度分量估计值其中,ωx(0)表示三轴陀螺3初始时刻检测到的角速度在oxb轴上的角速度分量,ωy(0)表示三轴陀螺3初始时刻检测到的角速度在oyb轴上的角速度分量,ωz(0)表示三轴陀螺3初始时刻检测到的角速度在ozb轴上的角速度分量,表示三轴陀螺3在oxb轴上的初始零偏估计值,表示三轴陀螺3在oyb轴上的初始零偏估计值,表示三轴陀螺3在ozb轴上的初始零偏估计值;
步骤302、采用所述数据处理器1根据公式得到第j个采样时刻载体的径向加速度判断门限τω(j);其中,ξω(j)表示第j个采样时刻的载体径向加速度阈值;
采用所述数据处理器1根据公式得到第j个采样时刻载体的切向加速度判断门限τa(j);其中,ξa表示单位采样时刻内俯仰角的变化量阈值,fx(j)表示第j个采样时刻三轴加速度计2测量到的总加速度在oxb轴上的加速度分量;
采用所述数据处理器1综合和得到第j个采样时刻的综合判断条件其中,τ(j)表示第j个采样时刻的综合判断参数;
步骤303、采用所述数据处理器1建立基于退化四元数的观测器模型并输入初始虚拟旋转矩阵和步骤302中第j个采样时刻的综合判断参数τ(j),得到第j+1个采样时刻的虚拟旋转矩阵估计值其中,kp表示观测器比例增益系数,且kP>0,kI表示观测器积分增益系数,且kI>0,km表示观测量权值,km>0,表示当地重力加速度归一化向量,且 表示三轴陀螺3第j个采样时刻检测到的角速度分量向量,且 表示三轴陀螺3第j个采样时刻的零偏估计向量,且 表示三轴陀螺3第j个采样时刻在oxb轴上的零偏估计值,表示三轴陀螺3第j个采样时刻在oyb轴上的零偏估计值,表示三轴陀螺3第j个采样时刻在ozb轴上的零偏估计值,表示第j个采样时刻的虚拟旋转矩阵估计值的转置,σ(j)表示第j个采样时刻的校正向量;S(·)表示斜对称矩阵,ΔT表示采样时间;
步骤303、采用所述数据处理器1建立载体第j+1个采样时刻的虚拟旋转矩阵并带入步骤304中得到的第j+1个采样时刻的虚拟旋转矩阵估计值得到第j+1个采样时刻退化的四元数矩阵其中,其中,q0(j+1)表示退化四元数中第j+1个采样时刻的标量参数,q1(j+1)表示退化四元数中第j+1个采样时刻的第一矢量参数,q2(j+1)表示退化四元数中第j+1个采样时刻的第二矢量参数;
步骤304、采用所述数据处理器1根据公式得到第j+1个采样时刻载体的俯仰角估计值和载体的横滚角估计值
本实施例中,步骤301中三轴陀螺3的初始零偏估计值的获取过程如下:
步骤3011、载体静止时,三轴陀螺3对载体的角速度进行检测,并将检测到的角速度发送至数据处理器1,数据处理器1对三轴陀螺3在oxb轴上的多个角速度分量、三轴陀螺3在oyb轴上的多个角速度分量和三轴陀螺3在ozb轴上的多个角速度分量分别进行采集,并将数据处理器1第i次采集到的三轴陀螺3在oxb轴上的角速度分量记作b′x(i),数据处理器1第i次采集到的三轴陀螺3在oyb轴上的角速度分量记作b′y(i),数据处理器1第i次采集到的三轴陀螺3在ozb轴上的角速度分量记作b′z(i);
步骤3012、采用所述数据处理器1根据公式得到三轴陀螺3在oxb轴上的初始零偏估计值采用所述数据处理器1根据公式得到三轴陀螺3在oyb轴上的初始零偏估计值采用所述数据处理器1根据公式得到三轴陀螺3在ozb轴上的初始零偏估计值其中,N表示采集总次数,i和N均为正整数,且i的取值范围为1~N,N的取值范围为10~20。
本实施例中,所述三轴陀螺3包括设置在所述载体坐标系中所述oxb轴上且对载体在oxb轴上的角速度分量进行检测的第一微机械陀螺3-1、设置在所述载体坐标系中所述oyb轴上且对载体在oyb轴上的角速度分量进行检测的第二微机械陀螺3-2和设置在所述载体坐标系中所述ozb轴上且对载体在ozb轴上的角速度分量进行检测的第三微机械陀螺3-3,所述第一微机械陀螺3-1、第二微机械陀螺3-2和第三微机械陀螺3-3的输出端均与数据处理器1的输入端连接。
本实施例中,所述数据处理器1为单片机、DSP微控制器或者ARM微控制器。
本实施例中,步骤302中第j个采样时刻的载体径向加速度阈值ξω(j)取值为
本实施例中,步骤303中单位时间间隔内俯仰角的变化量阈值ξa取值为ωmax表示载体俯仰角的最大变化率,且ωmax的取值范围为2rad/秒~10rad/秒,t表示积分时间。
本实施例中,采样时间ΔT的取值范围为0.005秒~0.1秒。
本实施例中,步骤303中观测器比例增益系数kp的取值范围为0<kP<10,观测器积分增益系数kI的取值范围为0<kI<10,观测量权值km的取值范围为0<km<10。
本实施例中,实际使用时还建立,以载体的质心为原点o,过原点o且指向北为xn轴,过原点o且指向东为yn轴,过原点o且垂直向下指向地球中心为zn轴,建立北东地坐标系oxnynzn。
本实施例中,需要说明的是,四元数中的第一矢量参数表示沿oxn轴方向的旋转参数,四元数中的第二矢量参数表示沿oyn轴方向的旋转参数,四元数中的第三矢量参数表示沿ozn轴方向的旋转参数,退化四元数即将四元数中的第三矢量参数等于零,即退化四元数中的第一矢量参数表示沿oxn轴方向的旋转参数,退化四元数中的第二矢量参数表示沿oyn轴方向的旋转参数。
本实施例中,需要说明的是,虚拟旋转矩阵即在第三矢量参数恒等于零时得到的退化四元数所对应的旋转矩阵,该旋转矩阵是描述一个虚拟的载体的虚拟旋转载体,它与真实的载体有着相同的俯仰角和横滚角,但是却有着不同的航向角。
本实施例中,需要说明的是,当j=0,又称为初始时刻。
本实施例中,需要说明的是,假设带入第j个采样时刻的虚拟旋转矩阵估计值和得到则假设带入kp、τ(j)和σ(j),得到则
本实施例中,所述第一微机械陀螺3-1、第二微机械陀螺3-2和第三微机械陀螺3-3均CRS03微机械陀螺。
本实施例中,进一步地,所述数据处理器1为STM32F407ARM微控制器,所述第一微机械陀螺3-1、第二微机械陀螺3-2、第三微机械陀螺3-3和三轴加速度计2的输出端均与数据处理器1的I/O端口连接。
本实施例中,进一步地,三轴加速度计2为ADXL355三轴加速度计。
本实施例中,需要说明的是,步骤303中观测器模型:当j=0时,代入初始虚拟旋转矩阵以及各个初始时刻的值,得到第一个采样时刻的虚拟旋转矩阵估计值这样依次迭代,从而由上一个采样时刻的虚拟旋转矩阵估计值得到下一个采样时刻的虚拟旋转矩阵估计值,实现各个采样时刻的虚拟旋转矩阵估计值,从而能得到载体的各个采样时刻的姿态角。
本实施例中,如图3所示,利用本发明方法获得的载体的姿态俯仰角和横滚角(即图3的观测器估计值)与利用高精度惯导获得姿态角测量值(即图3的理论参考值)的曲线图,利用本发明方法获得的载体的横滚角的最大误差为0.78°,载利用本发明方法获得的载体的俯仰角最大误差为0.89°,满足载体姿态检测需求。
综上所述,本发明建立基于退化四元数的观测器模型,将航向角与姿态角估计分离出来,在阴影无GPS信息情况下只利用三轴加速度计和三轴陀螺估计载体姿态角,并能在低动态下通过融合三轴加速度计和三轴陀螺输出进行姿态估计,在机动状态下切换到仅依赖三轴陀螺进行姿态估计,具有较高的姿态精度,实用性强。
以上所述,仅是本发明的较佳实施例,并非对本发明作任何限制,凡是根据本发明技术实质对以上实施例所作的任何简单修改、变更以及等效结构变化,均仍属于本发明技术方案的保护范围内。
Claims (8)
1.一种阴影下的载体姿态估计方法,该方法所采用的装置包括数据处理器(1)、设置在载体上的三轴陀螺(3)和三轴加速度计(2),其特征在于,该方法包括以下步骤:
步骤一、建立载体坐标系:
以载体的质心为原点o,oxb轴与所述载体的纵轴重合且指向所述载体前进方向,oyb轴与所述载体的横轴重合且指向所述载体前进方向的右侧,ozb轴垂直于所述oxb轴和所述oyb轴形成的平面oxbyb且指向所述载体下方,建立载体坐标系oxbybzb;
步骤二、载体姿态初始参数的获取:
步骤201、三轴加速度计(2)对载体初始时刻的总加速度进行检测,并将检测到的初始时刻总加速度发送至数据处理器(1),数据处理器(1)根据公式和得到载体的初始俯仰角φ(0)和载体的初始横滚角θ(0);g表示重力加速度,且g=9.8m/s2;其中,fx(0)表示三轴加速度计(2)测量到的初始时刻总加速度在oxb轴上的加速度分量,fy(0)表示三轴加速度计(2)测量到的初始时刻总加速度在oyb轴上的加速度分量,fz(0)表示三轴加速度计(2)测量到的初始时刻总加速度在ozb轴上的加速度分量;
步骤202、采用所述数据处理器(1)根据公式得到初始的虚拟旋转矩阵
步骤203、采用所述数据处理器(1)根据公式得到退化四元数中初始的标量参数q0(0)、退化四元数中初始的第一矢量参数q1(0)和退化四元数中初始的第二矢量参数q2(0);
步骤三、基于退化四元数的观测器模型的建立并获取载体的姿态估计值:
步骤301、j为自然数,即j≥0,当j≥1时,三轴陀螺(3)对载体的角速度进行检测,并将检测到的角速度发送至数据处理器(1),采用所述数据处理器(1)根据公式得到第j个采样时刻载体在oxb轴上的角速度分量估计值采用所述数据处理器(1)根据公式得到第j个采样时刻载体在oyb轴上的角速度分量估计值采用所述数据处理器(1)根据公式得到第j个采样时刻载体在ozb轴上的角速度分量估计值其中,ωx(j)表示三轴陀螺(3)第j个采样时刻检测到的角速度在oxb轴上的角速度分量,ωy(j)表示三轴陀螺(3)第j个采样时刻检测到的角速度在oyb轴上的角速度分量,ωz(j)表示三轴陀螺(3)第j个采样时刻检测到的角速度在ozb轴上的角速度分量,表示第j-1个采样时刻三轴陀螺(3)在oxb轴上的零偏估计值,表示第j-1个采样时刻三轴陀螺(3)在oyb轴上的零偏估计值,表示第j-1个采样时刻三轴陀螺(3)在ozb轴上的零偏估计值;
当j=0时,三轴陀螺(3)对载体的角速度进行检测,并将检测到的角速度发送至数据处理器(1),采用所述数据处理器(1)根据公式得到初始时刻载体在oxb轴上的角速度分量估计值采用所述数据处理器(1)根据公式得到初始时刻载体在oyb轴上的角速度分量估计值采用所述数据处理器(1)根据公式得到初始时刻载体在ozb轴上的角速度分量估计值其中,ωx(0)表示三轴陀螺(3)初始时刻检测到的角速度在oxb轴上的角速度分量,ωy(0)表示三轴陀螺(3)初始时刻检测到的角速度在oyb轴上的角速度分量,ωz(0)表示三轴陀螺(3)初始时刻检测到的角速度在ozb轴上的角速度分量,表示三轴陀螺(3)在oxb轴上的初始零偏估计值,表示三轴陀螺(3)在oyb轴上的初始零偏估计值,表示三轴陀螺(3)在ozb轴上的初始零偏估计值;
步骤302、采用所述数据处理器(1)根据公式得到第j个采样时刻载体的径向加速度判断门限τω(j);其中,ξω(j)表示第j个采样时刻的载体径向加速度阈值;
采用所述数据处理器(1)根据公式得到第j个采样时刻载体的切向加速度判断门限τa(j);其中,ξa表示单位采样时刻内俯仰角的变化量阈值,fx(j)表示第j个采样时刻三轴加速度计(2)测量到的总加速度在oxb轴上的加速度分量;
采用所述数据处理器(1)综合和得到第j个采样时刻的综合判断条件其中,τ(j)表示第j个采样时刻的综合判断参数;
步骤303、采用所述数据处理器(1)建立基于退化四元数的观测器模型并输入初始虚拟旋转矩阵和步骤302中第j个采样时刻的综合判断参数τ(j),得到第j+1个采样时刻的虚拟旋转矩阵估计值其中,kp表示观测器比例增益系数,且kP>0,kI表示观测器积分增益系数,且kI>0,km表示观测量权值,km>0,表示当地重力加速度归一化向量,且 表示三轴陀螺(3)第j个采样时刻检测到的角速度分量向量,且 表示三轴陀螺(3)第j个采样时刻的零偏估计向量,且 表示三轴陀螺(3)第j个采样时刻在oxb轴上的零偏估计值,表示三轴陀螺(3)第j个采样时刻在oyb轴上的零偏估计值,表示三轴陀螺(3)第j个采样时刻在ozb轴上的零偏估计值,表示第j个采样时刻的虚拟旋转矩阵估计值的转置,σ(j)表示第j个采样时刻的校正向量;S(·)表示斜对称矩阵,ΔT表示采样时间;
步骤303、采用所述数据处理器(1)建立载体第j+1个采样时刻的虚拟旋转矩阵并带入步骤304中得到的第j+1个采样时刻的虚拟旋转矩阵估计值得到第j+1个采样时刻退化的四元数矩阵其中,其中,q0(j+1)表示退化四元数中第j+1个采样时刻的标量参数,q1(j+1)表示退化四元数中第j+1个采样时刻的第一矢量参数,q2(j+1)表示退化四元数中第j+1个采样时刻的第二矢量参数;
步骤304、采用所述数据处理器(1)根据公式得到第j+1个采样时刻载体的俯仰角估计值和载体的横滚角估计值
2.按照权利要求1所述的一种阴影下的载体姿态估计方法,其特征在于:步骤301中三轴陀螺(3)的初始零偏估计值的获取过程如下:
步骤3011、载体静止时,三轴陀螺(3)对载体的角速度进行检测,并将检测到的角速度发送至数据处理器(1),数据处理器(1)对三轴陀螺(3)在oxb轴上的多个角速度分量、三轴陀螺(3)在oyb轴上的多个角速度分量和三轴陀螺(3)在ozb轴上的多个角速度分量分别进行采集,并将数据处理器(1)第i次采集到的三轴陀螺(3)在oxb轴上的角速度分量记作b′x(i),数据处理器(1)第i次采集到的三轴陀螺(3)在oyb轴上的角速度分量记作b′y(i),数据处理器(1)第i次采集到的三轴陀螺(3)在ozb轴上的角速度分量记作b′z(i);
步骤3012、采用所述数据处理器(1)根据公式得到三轴陀螺(3)在oxb轴上的初始零偏估计值采用所述数据处理器(1)根据公式得到三轴陀螺(3)在oyb轴上的初始零偏估计值采用所述数据处理器(1)根据公式得到三轴陀螺(3)在ozb轴上的初始零偏估计值其中,N表示采集总次数,i和N均为正整数,且i的取值范围为1~N,N的取值范围为10~20。
3.按照权利要求1所述的一种阴影下的载体姿态估计方法,其特征在于:所述三轴陀螺(3)包括设置在所述载体坐标系中所述oxb轴上且对载体在oxb轴上的角速度分量进行检测的第一微机械陀螺(3-1)、设置在所述载体坐标系中所述oyb轴上且对载体在oyb轴上的角速度分量进行检测的第二微机械陀螺(3-2)和设置在所述载体坐标系中所述ozb轴上且对载体在ozb轴上的角速度分量进行检测的第三微机械陀螺(3-3),所述第一微机械陀螺(3-1)、第二微机械陀螺(3-2)和第三微机械陀螺(3-3)的输出端均与数据处理器(1)的输入端连接。
4.按照权利要求1所述的一种阴影下的载体姿态估计方法,其特征在于:所述数据处理器(1)为单片机、DSP微控制器或者ARM微控制器。
5.按照权利要求1所述的一种阴影下的载体姿态估计方法,其特征在于:步骤302中第j个采样时刻的载体径向加速度阈值ξω(j)取值为
6.按照权利要求1所述的一种阴影下的载体姿态估计方法,其特征在于:步骤303中单位时间间隔内俯仰角的变化量阈值ξa取值为ωmax表示载体俯仰角的最大变化率,且ωmax的取值范围为2rad/秒~10rad/秒,t表示积分时间。
7.按照权利要求1所述的一种阴影下的载体姿态估计方法,其特征在于:采样时间ΔT的取值范围为0.005秒~0.1秒。
8.按照权利要求1所述的一种阴影下的载体姿态估计方法,其特征在于:步骤303中观测器比例增益系数kp的取值范围为0<kP<10,观测器积分增益系数kI的取值范围为0<kI<10,观测量权值km的取值范围为0<km<10。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910268878.6A CN109916399B (zh) | 2019-04-04 | 2019-04-04 | 一种阴影下的载体姿态估计方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910268878.6A CN109916399B (zh) | 2019-04-04 | 2019-04-04 | 一种阴影下的载体姿态估计方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN109916399A CN109916399A (zh) | 2019-06-21 |
CN109916399B true CN109916399B (zh) | 2019-12-24 |
Family
ID=66968517
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910268878.6A Active CN109916399B (zh) | 2019-04-04 | 2019-04-04 | 一种阴影下的载体姿态估计方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN109916399B (zh) |
Families Citing this family (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN112859138B (zh) * | 2019-11-28 | 2023-09-05 | 中移物联网有限公司 | 一种姿态测量方法、装置及电子设备 |
CN114459478B (zh) * | 2022-01-19 | 2024-03-19 | 东南大学 | 一种基于姿态运动学模型的惯性测量单元数据融合方法 |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2007276507A (ja) * | 2006-04-03 | 2007-10-25 | Univ Of Tokyo | 移動体制御装置及び移動体制御方法 |
JP2012173190A (ja) * | 2011-02-23 | 2012-09-10 | Seiko Epson Corp | 測位システム、測位方法 |
CN107525503A (zh) * | 2017-08-23 | 2017-12-29 | 王伟 | 基于双天线gps和mimu组合的自适应级联卡尔曼滤波方法 |
CN108036785A (zh) * | 2017-11-24 | 2018-05-15 | 浙江大学 | 一种基于直接法与惯导融合的飞行器位姿估计方法 |
CN109211230A (zh) * | 2018-09-07 | 2019-01-15 | 东南大学 | 一种基于牛顿迭代法的炮弹姿态和加速度计常值误差估计方法 |
-
2019
- 2019-04-04 CN CN201910268878.6A patent/CN109916399B/zh active Active
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2007276507A (ja) * | 2006-04-03 | 2007-10-25 | Univ Of Tokyo | 移動体制御装置及び移動体制御方法 |
JP2012173190A (ja) * | 2011-02-23 | 2012-09-10 | Seiko Epson Corp | 測位システム、測位方法 |
CN107525503A (zh) * | 2017-08-23 | 2017-12-29 | 王伟 | 基于双天线gps和mimu组合的自适应级联卡尔曼滤波方法 |
CN108036785A (zh) * | 2017-11-24 | 2018-05-15 | 浙江大学 | 一种基于直接法与惯导融合的飞行器位姿估计方法 |
CN109211230A (zh) * | 2018-09-07 | 2019-01-15 | 东南大学 | 一种基于牛顿迭代法的炮弹姿态和加速度计常值误差估计方法 |
Non-Patent Citations (3)
Title |
---|
A Nonlinear Observer for Attitude Estimation of Vehicle-Mounted Satcom-on-the-Move;Xiaowei Shen,et al.;《IEEE SENSORS JOURNAL》;20190915;第19卷(第18期);8057-8066 * |
基于平方根差分滤波的动中通姿态稳定方法;沈晓卫等;《电光与控制》;20120331;第19卷(第3期);60-64 * |
基于扩展卡尔曼滤波的动中通低成本姿态估计;闫林波等;《电光与控制》;20130731;第20卷(第7期);53-87 * |
Also Published As
Publication number | Publication date |
---|---|
CN109916399A (zh) | 2019-06-21 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110207697B (zh) | 基于角加速度计/陀螺/加速度计的惯性导航解算方法 | |
CN109682377B (zh) | 一种基于动态步长梯度下降的姿态估计方法 | |
CN107289930B (zh) | 基于mems惯性测量单元的纯惯性车辆导航方法 | |
CN110887481B (zh) | 基于mems惯性传感器的载体动态姿态估计方法 | |
CN112629538A (zh) | 基于融合互补滤波和卡尔曼滤波的舰船水平姿态测量方法 | |
CN110702113B (zh) | 基于mems传感器的捷联惯导***数据预处理和姿态解算的方法 | |
CN108731676B (zh) | 一种基于惯性导航技术的姿态融合增强测量方法及*** | |
CN108168549B (zh) | 一种卫星动中通姿态检测方法 | |
CN114485641B (zh) | 一种基于惯导卫导方位融合的姿态解算方法及装置 | |
CN106979781A (zh) | 基于分布式惯性网络的高精度传递对准方法 | |
CN109916399B (zh) | 一种阴影下的载体姿态估计方法 | |
TWI689705B (zh) | 一種衛星姿態資料融合系統及其方法 | |
CN111272158A (zh) | 复杂磁扰动场景mems电子罗盘的动态方位角解算方法 | |
CN108871319B (zh) | 一种基于地球重力场与地磁场序贯修正的姿态解算方法 | |
CN108444468B (zh) | 一种融合下视视觉与惯导信息的定向罗盘 | |
CN114526731A (zh) | 一种基于助力车的惯性组合导航方向定位方法 | |
CN113008229A (zh) | 一种基于低成本车载传感器的分布式自主组合导航方法 | |
CN111141283A (zh) | 一种通过地磁数据判断行进方向的方法 | |
KR100648882B1 (ko) | 무인 항공기 자동 항법 시스템의 관성 값 계산 장치 및 그방법 | |
CN113932803B (zh) | 适用于高动态飞行器的惯性/地磁/卫星组合导航*** | |
CN113267183B (zh) | 一种多加速度计惯导***的组合导航方法 | |
CN115754352A (zh) | 一种基于数据优化的卫星旋转机动加速度计定标方法 | |
CN114137592A (zh) | 一种多源传感器融合定位的切换方法及*** | |
Wu et al. | Real-time magnetic disturbance determination for micro air vehicles via gravity and global navigation satellite system measurements | |
Mumtaz et al. | Development of a low cost wireless IMU using MEMS sensors for pedestrian navigation |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |