CN103471594B - 基于ahrs的精对准算法 - Google Patents
基于ahrs的精对准算法 Download PDFInfo
- Publication number
- CN103471594B CN103471594B CN201310439123.0A CN201310439123A CN103471594B CN 103471594 B CN103471594 B CN 103471594B CN 201310439123 A CN201310439123 A CN 201310439123A CN 103471594 B CN103471594 B CN 103471594B
- Authority
- CN
- China
- Prior art keywords
- rightarrow
- axis
- matrix
- ahrs
- accelerometer
- 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.)
- Expired - Fee Related
Links
Landscapes
- Navigation (AREA)
Abstract
本发明公开了一种基于AHRS的精对准算法,主要解决了现有技术中存在的精对准技术收敛较慢、误差估计精度不高,不能满足人们需求的问题。该基于AHRS的精对准算法采用两个方向上的加速计的输出和陀螺仪三个方向上的输出做观测值,并采用卡尔曼滤波求姿态角的误差,进而进行精对准。通过上述方案,本发明达到了收敛较快,精度较高的目的,具有很高的实用价值和推广价值。
Description
技术领域
本发明涉及一种导航领域的对准算法,具体地说,是涉及一种基于AHRS的精对准算法。
背景技术
传统的精对准技术观测量比较少,大都只用东向和北向的速度做观测值或只用东向和北向加速度计做观测值,该种对准方式虽然对姿态角的误差有一定的估计作用,但是收敛很慢,且误差估计精度不高。现有技术中大都采用多传感器的输出做观测值、采用东向和北向的速度和陀螺仪的三个方向的角速度共五个值做观测值,以此提高收敛速度和精度,但这种方式的对准度仍然较为有限,不能满足人们需求。
发明内容
本发明的目的在于提供一种基于AHRS的精对准算法,主要解决现有技术中存在的精对准技术收敛较慢、误差估计精度不高,不能满足人们需求的问题。
为了实现上述目的,本发明采用的技术方案如下:
基于AHRS的精对准算法,包括以下步骤:
(1)采集当前的纬度信息和重力加速度信息;
(2)根据AHRS中加速度计和陀螺仪的输出测得当前的姿态转移矩阵Cbp;
(3)测算出地球相对于惯性坐标系的旋转角速度wie;
(4)确定当前X轴向的角度误差、Y轴向的角度误差、Z轴向的角度误差、陀螺仪在X轴向的常值偏移、陀螺仪在Y轴向的常值偏移、陀螺仪在Z轴向的常值偏移、加速度计在X轴向的常值偏移和加速度计在Y轴向的常值偏移的被估计值;
(5)根据步骤(4)中被估计值前后两个时刻的关系得出连续卡尔曼滤波下的状态转移矩阵A,并根据公式F=eye(8)+A×t;将其转换为离散卡尔曼滤波下的状态转移矩阵,其中,t为滤波间隔;
(6)根据当前噪声和观测值前后两个时刻间的关系求出噪声驱动矩阵G,根据被估计值和观测值之间的关系求出观测矩阵H,根据陀螺仪的随机误差确定AHRS中的***噪声方差矩阵,并根据加速度计的随机误差确定观测噪声方差矩阵;
(7)将被估计值的初始滤波值设置为零;
(8)开始卡尔曼滤波,每次取两个方向上的加速度计的输出和陀螺仪的三个方向上的输出作为观测值;
(9)根据求得的滤波结果和方差更新被估计值和观测噪声方差的数据,并再次取观测值进行卡尔曼滤波;
(10)返回步骤(8),直至两次滤波结果小于用户设定的阈值;
(11)使用求得的X轴向、Y轴向和Z轴向上的姿态角的误差对姿态转移矩阵Cbp进行纠正。
具体地说,所述步骤(2)具体包括以下步骤:
(2a)根据 得出重力加速度在地理坐标系下的投影其中g为重力加速度;根据 得出地球绕惯性坐标系自转角速度在地理坐标系下的投影其中wiε为地球绕惯性轴的自转角速度,L为纬度;
(2b)根据公式构造坐标系下的三个向量和
(2c)根据方程式 解得矩阵其中,t为时间间隔,将以平台坐标系表示;
(2d)根据公式计算出姿态转移矩阵Cbp。
与现有技术相比,本发明具有以下有益效果:
(1)本发明中,巧妙地采用两个方向上的加速计的输出和陀螺仪三个方向上的输出做观测值,并采用卡尔曼滤波求姿态角的误差,从而大大提高了收敛速度,并有效地增加了对准精度,符合技术发展需求,具有突出的实质性特点和显著进步,适合大规模推广应用。
具体实施方式
下面结合实施例对本发明作进一步说明,本发明的实施方式包括但不限于下列实施例。
实施例
为了解决现有技术中存在的精对准技术收敛较慢、误差估计精度不高,不能满足人们需求的问题,本发明公开了一种基于AHRS的精对准算法,通过以下步骤实现:
第一步:通过外部设备,如经纬度测量仪和重力加速度测量仪获得纬度信息和重力加速度信息;
第二步:进行粗对准,即利用AHRS(航姿参考***)中的加速度计和陀螺仪的输出测得当前的姿态转移矩阵Cbp,此时该姿态转移矩阵的精度较差;
根据 得出重力加速度在地理坐标系下的投影其中g为重力加速度;根据 得出地球绕惯性坐标系自转角速度在地理坐标系下的投影其中wiε为地球绕惯性轴的自转角速度,L为纬度;根据构造载体坐标系下的三个向量根据方程式 解得矩阵因为精度不高,这里写为平台坐标系更合适,则姿态转移矩阵
第三步:确定地球相对于惯性坐标系的旋转角速度wie;
第四步:确定被估计值X=[φxφyφzεxεyεz▽x▽y]T,其中,前三个值为要求的三个轴向,即X轴向、Y轴向、Z轴向的角度误差,中间三个值为陀螺仪常值偏移,开机后这个值既可作为定值,但每次开机不一样,可以多次测量取均值,最后两个量为加速度计的常值偏移,为了进行卡尔曼滤波,还需求出以下几个关键矩阵。
(1)求状态转移矩阵:
A=zeros(8,8);
A(1,2)=wie×sin(L);
A(1,3)=-wie×cos(L);
A(2,1)=-wie×sin(L);
A(3,1)=wie×cos(L);
A(1,4)=-Cbp(1,1);
A(1,5)=-Cbp(1,2);
A(1,6)=-Cbp(1,3);
A(2,4)=-Cbp(2,1);
A(2,5)=-Cbp(2,2);
A(2,6)=-Cbp(2,3);
A(3,4)=-Cbp(3,1);
A(3,5)=-Cbp(3,2);
A(3,6)=-Cbp(3,3);根据被估计值前后两个时刻的关系求出连续卡尔曼滤波下的状态转移矩阵A,并将它转换为离散卡尔曼滤波下的状态转移矩阵,以便于运算;
(2)根据F=eye(8)+A×t;求出离散状态下的状态转移矩阵,其中,t为滤波间隔;
(3)根据当前噪声和观测值前后两个时刻间的关系,求出噪声驱动矩阵G,即噪声对状态转移的贡献矩阵,也为连续状态下的矩阵,同样离散化;
G=zeros(8,8);
G(1,1)=-Cbp(1,1);
G(1,2)=-Cbp(1,2);
G(1,3)=-Cbp(1,3);
G(2,1)=-Cbp(2,1);
G(2,2)=-Cbp(2,2);
G(2,3)=-Cbp(2,3);
G(3,1)=-Cbp(3,1);
G(3,2)=-Cbp(3,2);
G(3,3)=-Cbp(3,3);
G(4,4)=1;
G(5,5)=1;
G(6,6)=1;
G(7,7)=1;
G(8,8)=1;
G0=G×t;
(4)根据被估计值与观测值之间的关系求出观测矩阵H;
H=zeros(5,8);
H(1,2)=-g;
H(1,7)=1;
H(2,1)=g;
H(2,8)=1;
H(3,2)=-wie×sin(L);
H(3,3)=wie×cos(L);
H(3,4)=1;
H(4,1)=wie×sin(L);
H(4,5)=1;
H(5,1)=-wie×cos(L);
H(5,6)=1;
(5)根据陀螺仪随机误差确定***噪声方差矩阵;
Q={zeros(8,8)+diag(q2,q2,q2,0,0,0,0,0)}/t其中q为陀螺仪产生的白噪声的方差;
(6)根据加速度计随机误差确定观测噪声方差阵,虽然理论上来说初始方差是随意的,在进行卡尔曼滤波的过程中会趋近真实值,但在具体应用时优选取为和各观测值误差相关的值,以利于加快收敛速度;
R={zeros(5,5)+diag(s2,s2,q2,q2,q2)}/t,其中s为加速度计产生的白噪声的方差。
在测算出上述值的基础上,进行以下步骤便可实现精对准:
第五步:设置被估计值X的初始滤波值为全零;
第六步:开始卡尔曼滤波,每次取两个方向上的加速度计的输出和陀螺仪的三个方向上的输出作为观测值,如
第七步:根据求得的滤波结果和方差更新被估计值X和观测噪声方差R的数据,并再次取观测值进行卡尔曼滤波;
第八步:反复进行第六步和第七部步,直到两次滤波结果小于一个阈值,该阈值由用户根据实际需求自行设定;
第九步:用求得的三个方向上的姿态角的误差纠正姿态转移矩阵Cbp,求得精度较高的姿态矩阵
按照上述实施例,便可很好地实现本发明。
Claims (2)
1.基于AHRS的精对准算法,其特征在于,包括以下步骤:
(1)采集当前的纬度信息和重力加速度信息;
(2)根据AHRS中加速度计和陀螺仪的输出测得当前的姿态转移矩阵Cbp;
(3)测算出地球相对于惯性坐标系的旋转角速度wie;
(4)确定当前X轴向的角度误差、Y轴向的角度误差、Z轴向的角度误差、陀螺仪在X轴向的常值偏移、陀螺仪在Y轴向的常值偏移、陀螺仪在Z轴向的常值偏移、加速度计在X轴向的常值偏移和加速度计在Y轴向的常值偏移的被估计值;
(5)根据步骤(4)中被估计值前后两个时刻的关系得出连续卡尔曼滤波下的状态转移矩阵A,并根据公式F=eye(8)+A×t;将其转换为离散卡尔曼滤波下的状态转移矩阵,其中,t为滤波间隔,eye(8)为维度为8的单位矩阵;
(6)根据当前噪声和观测值前后两个时刻间的关系求出噪声驱动矩阵G,根据被估计值和观测值之间的关系求出观测矩阵H,根据陀螺仪的随机误差确定AHRS中的***噪声方差矩阵,并根据加速度计的随机误差确定观测噪声方差矩阵;
(7)将被估计值的初始滤波值设置为零;
(8)开始卡尔曼滤波,每次取两个方向上的加速度计的输出和陀螺仪的三个方向上的输出作为观测值;
(9)根据求得的滤波结果和方差更新被估计值和观测噪声方差的数据,并再次取观测值进行卡尔曼滤波;
(10)返回步骤(8),直至两次滤波结果小于用户设定的阈值;
(11)使用求得的X轴向、Y轴向和Z轴向上的姿态角的误差对姿态转移矩阵Cbp进行纠正。
2.根据权利要求1所述的基于AHRS的精对准算法,其特征在于,所述步骤(2)具体包括以下步骤:
(2a)根据 得出重力加速度在地理坐标系下的投影其中g为重力加速度;根据 得出地球绕惯性坐标系自转角速度在地理坐标系下的投影其中wiε为地球绕惯性轴的自转角速度,L为纬度;
(2b)根据公式构造坐标系下的三个向量和
(2c)根据方程式 解得矩阵其中,t为时间间隔,将以平台坐标系表示;
(2d)根据公式计算出姿态转移矩阵Cbp。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201310439123.0A CN103471594B (zh) | 2013-09-24 | 2013-09-24 | 基于ahrs的精对准算法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201310439123.0A CN103471594B (zh) | 2013-09-24 | 2013-09-24 | 基于ahrs的精对准算法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN103471594A CN103471594A (zh) | 2013-12-25 |
CN103471594B true CN103471594B (zh) | 2016-01-20 |
Family
ID=49796545
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201310439123.0A Expired - Fee Related CN103471594B (zh) | 2013-09-24 | 2013-09-24 | 基于ahrs的精对准算法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN103471594B (zh) |
Families Citing this family (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104748719B (zh) * | 2015-03-18 | 2017-03-01 | 中国人民解放军空军装备研究院航空装备研究所 | 一种基于ahrs的单轴旋转角测量方法 |
CN104864865B (zh) * | 2015-06-01 | 2017-09-22 | 济南大学 | 一种面向室内行人导航的ahrs/uwb无缝组合导航方法 |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101033973A (zh) * | 2007-04-10 | 2007-09-12 | 南京航空航天大学 | 微小型飞行器微惯性组合导航***的姿态确定方法 |
CN102519460A (zh) * | 2011-12-09 | 2012-06-27 | 东南大学 | 一种捷联惯性导航***非线性对准方法 |
CN102607562A (zh) * | 2012-04-12 | 2012-07-25 | 南京航空航天大学 | 基于载体飞行模态判别的微惯性参数自适应姿态确定方法 |
CN102692225A (zh) * | 2011-03-24 | 2012-09-26 | 北京理工大学 | 一种用于低成本小型无人机的姿态航向参考*** |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US7386395B1 (en) * | 2005-01-18 | 2008-06-10 | Honeywell International Inc. | Systems and methods for shock compensation utilizing an adaptive control technique algorithm |
-
2013
- 2013-09-24 CN CN201310439123.0A patent/CN103471594B/zh not_active Expired - Fee Related
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101033973A (zh) * | 2007-04-10 | 2007-09-12 | 南京航空航天大学 | 微小型飞行器微惯性组合导航***的姿态确定方法 |
CN102692225A (zh) * | 2011-03-24 | 2012-09-26 | 北京理工大学 | 一种用于低成本小型无人机的姿态航向参考*** |
CN102519460A (zh) * | 2011-12-09 | 2012-06-27 | 东南大学 | 一种捷联惯性导航***非线性对准方法 |
CN102607562A (zh) * | 2012-04-12 | 2012-07-25 | 南京航空航天大学 | 基于载体飞行模态判别的微惯性参数自适应姿态确定方法 |
Non-Patent Citations (2)
Title |
---|
一种捷联姿态航向参考***信息融合算法应用;张谦等;《计算机工程与应用》;20070521(第15期);全文 * |
机动加速度辅助的航姿***扩展卡尔曼滤波;郭鹏飞等;《***工程与电子技术》;20090315(第03期);全文 * |
Also Published As
Publication number | Publication date |
---|---|
CN103471594A (zh) | 2013-12-25 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN102486377B (zh) | 一种光纤陀螺捷联惯导***初始航向的姿态获取方法 | |
CN103822633B (zh) | 一种基于二阶量测更新的低成本姿态估计方法 | |
CN105300379B (zh) | 一种基于加速度的卡尔曼滤波姿态估计方法及*** | |
CN103630137B (zh) | 一种用于导航***的姿态及航向角的校正方法 | |
CN101706281B (zh) | 惯性/天文/卫星高精度组合导航***及其导航方法 | |
CN101514900B (zh) | 一种单轴旋转的捷联惯导***初始对准方法 | |
CN103727941B (zh) | 基于载体系速度匹配的容积卡尔曼非线性组合导航方法 | |
CN103900608B (zh) | 一种基于四元数ckf的低精度惯导初始对准方法 | |
CN108458714B (zh) | 一种姿态检测***中不含重力加速度的欧拉角求解方法 | |
CN103299247B (zh) | 用于磁近场的动态追踪及补偿的设备和方法 | |
CN104121905A (zh) | 一种基于惯性传感器的航向角获取方法 | |
CN109870173A (zh) | 一种基于校验点的海底管道惯性导航***的轨迹修正方法 | |
CN105021198B (zh) | 一种基于多传感器综合导航的位置估计方法 | |
CN102519485B (zh) | 一种引入陀螺信息的二位置捷联惯性导航***初始对准方法 | |
CN103512584A (zh) | 导航姿态信息输出方法、装置及捷联航姿参考*** | |
CN106370178B (zh) | 移动终端设备的姿态测量方法及装置 | |
CN106840211A (zh) | 一种基于kf和stupf组合滤波的sins大方位失准角初始对准方法 | |
CN107255474A (zh) | 一种融合电子罗盘和陀螺仪的pdr航向角确定方法 | |
CN107167131A (zh) | 一种微惯性测量信息的深度融合与实时补偿的方法及*** | |
CN106153073A (zh) | 一种全姿态捷联惯导***的非线性初始对准方法 | |
CN105157724B (zh) | 一种基于速度加姿态匹配的传递对准时间延迟估计与补偿方法 | |
CN206074000U (zh) | 一种基于重力辅助的水下潜器惯性导航*** | |
CN103499348A (zh) | Ahrs高精度姿态数据计算方法 | |
CN205940567U (zh) | 一种车载组合导航定位*** | |
CN103398725A (zh) | 一种基于星敏感器的捷联惯导***初始对准的方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
C10 | Entry into substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
C14 | Grant of patent or utility model | ||
GR01 | Patent grant | ||
CF01 | Termination of patent right due to non-payment of annual fee | ||
CF01 | Termination of patent right due to non-payment of annual fee |
Granted publication date: 20160120 Termination date: 20160924 |