CN116358541A - 一种基于陀螺仪和加速度计的姿态校准方法 - Google Patents
一种基于陀螺仪和加速度计的姿态校准方法 Download PDFInfo
- Publication number
- CN116358541A CN116358541A CN202310336055.9A CN202310336055A CN116358541A CN 116358541 A CN116358541 A CN 116358541A CN 202310336055 A CN202310336055 A CN 202310336055A CN 116358541 A CN116358541 A CN 116358541A
- Authority
- CN
- China
- Prior art keywords
- angular velocity
- attitude
- gyroscope
- smaller
- angle
- 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.)
- Pending
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/18—Stabilised platforms, e.g. by gyroscope
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T90/00—Enabling technologies or technologies with a potential or indirect contribution to GHG emissions mitigation
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Gyroscopes (AREA)
Abstract
本发明公开了一种基于陀螺仪和加速度计的姿态校准方法,包括:S1:采集目标陀螺仪和加速度计的角速度和加速度;S2:获取第一预设时间的角速度数据,计算加速度方差、角速度方差以及角速度积分;S3:判断当前的运动状态,根据运动状态,进行姿态角计算,并采用不同修正系数调整姿态。本发明通过角速度数据和加速度数据,判断设备当前的运动状态,通过对应运动状态调整陀螺仪累积误差的消除速度,实现高精度姿态校准。
Description
技术领域
本发明涉及设备姿态校准技术领域,特别涉及一种基于陀螺仪和加速度计的姿态校准方法。
背景技术
陀螺仪是用高速回转体的动量矩敏感壳体相对惯性空间绕正交于自转轴的一个或二个轴的角运动检测装置,其测量角速度,具有高动态特性,但在实际使用时存在零点漂移误差、白噪声误差和温度影响误差。
基于使用时存在的误差,目前处理方式如下:
使用高精度的陀螺仪,将误差降低,缺陷是成本急剧增高。
使用加速度的倾角姿态作修正,但是加速度在车辆等运动时波动很大及转弯时向心加速度引入的误差,导致设备姿态剧烈波动及不准确,适合静态场景及低速场景,在车辆姿态分析及碰撞分析、摩托车压弯等场景下不可接受。
发明内容
为解决上述问题,本发明提供了一种基于陀螺仪和加速度计的姿态校准方法,具体技术方案如下:
S1:采集目标陀螺仪和加速度计的角速度和加速度;
S2:获取第一预设时间的角速度数据,计算加速度方差、角速度方差以及角速度积分;
S3:判断当前的运动状态,根据运动状态,进行姿态角计算,并采用不同修正系数调整姿态;
所述运动状态包括转弯状态、直线行驶以及静止状态。
进一步的,步骤S2中,所述第一预设时间为2s。
进一步的,步骤S2中,所述角速度数据为航向角和横滚角对应的角速度数据。
进一步的,步骤S2中,角速度积分计算如下:
其中,x为角速度,dt为陀螺仪采用频率倒数,n为当前采集频率下在第一预设时间采集的角速度数据的个数。
进一步的,步骤S3中,所述运动状态判断如下:
角速度方差Sv低于0.1或加速度方差低于0.01时,判定设备处于静止状态;
角速度方差Sv>100或角速度积分最大值Ss大于2时,判定设备处于转弯状态;
当设备处于非静止状态,且角速度积分最大值Ss小于2时,判定设备处于直线行驶状态。
进一步的,步骤S3中,静止状态下,按照如下方法修正姿态;
角速度方差Sv低于0.1时的静止状态下,通过加速度计算姿态角,基于第一修正系数修正设备姿态;
加速度方差低于0.01的静止状态下,通过加速度计算姿态角,基于第二修正系数修正设备姿态;
角速度方差判定的优先级高于加速度方差判定的优先级。
进一步的,步骤S3中,直线行驶状态下,按照如下方法修正姿态;
角速度积分最大值Ss小于0.5,且横滚角和航向角对应角速度最大值小于1,横滚角绝对值r小于5,通过加速度计算姿态角,基于第三修正系数修正设备姿态;
角速度积分最大值Ss小于1,且横滚角和航向角对应角速度最大值小于2,r小于5,通过加速度计算姿态角,基于第四修正系数修正设备姿态;
角速度积分最大值Ss小于1.5,且横滚角和航向角对应角速度最大值小于3,r小于5,通过加速度计算姿态角,基于第五修正系数修正设备姿态;
角速度积分最大值Ss小于2,且横滚角和航向角对应角速度最大值小于4,r小于10,通过加速度计算姿态角,基于第六修正系数修正设备姿态。
进一步的,步骤S3中,设备处于转弯状态时,采用四元数算法通过角速度计算姿态角,不做姿态修正。
进一步的,步骤S2之前,还包括对步骤S1获取的角速度数据按照如下方法进行零漂处理,得到去除零漂后的角速度;
基于第二预设时间,实时计算角速度的方差和均值;将角速度的方差减去平均得到去除零漂后的角速度;步骤S3中所述角速度数据即为去除零漂后的角速度数据。
进一步的,所述第二预设时间为5s。
本发明的有益效果如下:
基于实时加速度数据和角速度数据,计算加速度和角速度方差以及角速度积分,进而判定设备的运动状态,根据不同的运动状态,通过加速度或角速度计算姿态角,采用不同的修正系数进行设备姿态修正,通过对应运动状态调整陀螺仪累积误差的消除速度,实现高精度姿态校准。
附图说明
图1是本发明的方法流程示意图。
具体实施方式
在下面的描述中对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅是本发明的一部分实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例,都属于本发明保护的范围。
在本发明实施例的描述中,需要说明的是,指示方位或位置关系为基于附图所示的方位或位置关系,或者是该发明产品使用时惯常摆放的方位或位置关系,或者是本领域技术人员惯常理解的方位或位置关系,或者是该发明产品使用时惯常摆放的方位或位置关系,仅是为了便于描述本发明和简化描述,而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作,因此不能理解为对本发明的限制。此外,术语“第一”、“第二”仅用于区分描述,而不能理解为指示或暗示相对重要性。
在本发明实施例的描述中,还需要说明的是,除非另有明确的规定和限定,术语“设置”、“连接”应做广义理解,例如,可以是固定连接,也可以是可拆卸连接,或一体地连接;可以是直接连接,也可以通过中间媒介间接连接。对于本领域的普通技术人员而言,可以具体情况理解上述术语在本发明中的具体含义。
实施例1
本发明的实施例1公开了一种基于陀螺仪和加速度计的姿态校准方法,如图1所示,具体如下:
S1:采集目标陀螺仪和加速度计的实时角速度和加速度;
获取第二预设时间的角速度数据,并计算第二预设时间的角速度方差,判断角速度方差是否小于0.3;
若小于则对角速度数据案如下方式进行零漂处理,得到去除零漂后的角速度;
基于对应的第二预设时间,实时计算角速度的均值;将角速度的方差减去平均得到去除零漂后的角速度;
将去除零漂后的角速度数据作为后续步骤S3中参与计算的角速度数据。
本实施例中,所述第二预设时间为5s。
S2:获取第一预设时间的角速度数据,计算加速度方差、角速度方差以及角速度积分;
本实施例中,所述第一预设时间为2s;所述角速度数据为航向角和横滚角对应的角速度数据。
角速度积分计算如下:
其中,x为角速度,dt为陀螺仪采用频率倒数,n为当前采集频率下在第一预设时间采集的角速度数据的个数。
S3:判断当前的运动状态,根据运动状态,进行姿态角计算,并采用不同修正系数调整姿态;
所述运动状态包括转弯状态、直线行驶以及静止状态。
所述运动状态判断如下:
本实施例中,角速度方差Sv低于0.1或加速度方差低于0.01时,判定设备处于静止状态;
本实施例中,静止状态下,按照如下方法修正姿态;
角速度方差Sv低于0.1时的静止状态下,采用四元数AHRS算法通过加速度计算姿态角,基于第一修正系数修正设备姿态;
加速度方差低于0.01的静止状态下,采用四元数AHRS算法通过加速度计算姿态角,基于第二修正系数修正设备姿态;
角速度方差判定的优先级高于加速度方差判定的优先级。
本实施例中,所述角速度方差Sv为三轴角速度方差之和。
本实施例中,所述第一修正系数为2.1,所述第二修正系数为1.1。
本实施例中,当设备处于非静止状态,且角速度积分最大值Ss小于2时,判定设备处于直线行驶状态;
本实施例中,直线行驶状态下,按照如下方法修正姿态;
角速度积分最大值Ss小于0.5,且横滚角和航向角对应角速度最大值小于1,横滚角绝对值r小于5,采用四元数AHRS算法通过加速度计算姿态角,基于第三修正系数修正设备姿态,记为高速修正;
角速度积分最大值Ss小于1,且横滚角和航向角对应角速度最大值小于2,r小于5,采用四元数AHRS算法通过加速度计算姿态角,基于第四修正系数修正设备姿态,记为一般速度修正;
角速度积分最大值Ss小于1.5,且横滚角和航向角对应角速度最大值小于3,r小于5,采用四元数AHRS算法通过加速度计算姿态角,基于第五修正系数修正设备姿态,记为低速修正;
角速度积分最大值Ss小于2,且横滚角和航向角对应角速度最大值小于4,r小于10,采用四元数AHRS算法通过加速度计算姿态角,基于第六修正系数修正设备姿态,记为缓慢修正;
其中,修正的优先级大小依次为高速修正、一般速度修正、低速修正和缓慢修正。
所述角速度积分最大值Ss,为横滚角和航向角对应角速度积分比较的最大值。
本实施例中,所述第三修正系数为0.3,所述第四修正系数为0.2,所述第五修正系数为0.1,所述第六修正系数为0.05。
本实施例中,角速度方差Sv>100或角速度积分最大值Ss大于2时,判定设备处于转弯状态。
本实施例中,设备处于转弯状态时,采用四元数算法通过角速度计算姿态,不做姿态修正。
本发明并不局限于前述的具体实施方式。本发明扩展到任何在本说明书中披露的新特征或任何新的组合,以及披露的任一新的方法或过程的步骤或任何新的组合。
Claims (10)
1.一种基于陀螺仪和加速度计的姿态校准方法,其特征在于,包括:
S1:采集目标陀螺仪和加速度计的角速度和加速度;
S2:获取第一预设时间的角速度数据,计算加速度方差、角速度方差以及角速度积分;
S3:判断当前的运动状态,根据运动状态,进进行姿态角计算,并采用不同修正系数调整姿态;
所述运动状态包括转弯状态、直线行驶以及静止状态。
2.根据权利要求1所述的基于陀螺仪和加速度计的姿态校准方法,其特征在于,步骤S2中,所述第一预设时间为2s。
3.根据权利要求2所述的基于陀螺仪和加速度计的姿态校准方法,其特征在于,步骤S2中,所述角速度数据为航向角和横滚角对应的角速度数据。
5.根据权利要求4所述的基于陀螺仪和加速度计的姿态校准方法,其特征在于,步骤S3中,所述运动状态判断如下:
角速度方差Sv低于0.1或加速度方差低于0.01时,判定设备处于静止状态;
角速度方差Sv>100或角速度积分最大值Ss大于2时,判定设备处于转弯状态;
当设备处于非静止状态,且角速度积分最大值Ss小于2时,判定设备处于直线行驶状态。
6.根据权利要求5所述的基于陀螺仪和加速度计的姿态校准方法,其特征在于,步骤S3中,静止状态下,按照如下方法修正姿态;
角速度方差Sv低于0.1时的静止状态下,通过加速度计算姿态角,基于第一修正系数修正设备姿态;
加速度方差低于0.01的静止状态下,通过加速度计算姿态角,基于第二修正系数修正设备姿态;
角速度方差判定的优先级高于加速度方差判定的优先级。
7.根据权利要求5所述的基于陀螺仪和加速度计的姿态校准方法,其特征在于,步骤S3中,直线行驶状态下,按照如下方法修正姿态;
角速度积分最大值Ss小于0.5,且横滚角和航向角对应角速度最大值小于1,横滚角绝对值r小于5,通过加速度计算姿态角,基于第三修正系数修正设备姿态;
角速度积分最大值Ss小于1,且横滚角和航向角对应角速度最大值小于2,r小于5,通过加速度计算姿态角,基于第四修正系数修正设备姿态;
角速度积分最大值Ss小于1.5,且横滚角和航向角对应角速度最大值小于3,r小于5,通过加速度计算姿态角,基于第五修正系数修正设备姿态;
角速度积分最大值Ss小于2,且横滚角和航向角对应角速度最大值小于4,r小于10,通过加速度计算姿态角,基于第六修正系数修正设备姿态。
8.根据权利要求5所述的基于陀螺仪和加速度计的姿态校准方法,其特征在于,步骤S3中,设备处于转弯状态时,采用四元数算法通过角速度计算姿态角,不做姿态修正。
9.根据权利要求1-8任一所述的基于陀螺仪和加速度计的姿态校准方法,其特征在于,步骤S2之前,还包括对步骤S1获取的角速度数据按照如下方法进行零漂处理,得到去除零漂后的角速度;
基于第二预设时间,实时计算角速度的方差和均值;将角速度的方差减去平均得到去除零漂后的角速度;步骤S3中所述角速度数据即为去除零漂后的角速度数据。
10.根据权利要求9所述的基于陀螺仪和加速度计的姿态校准方法,其特征在于,所述第二预设时间为5s。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202310336055.9A CN116358541A (zh) | 2023-03-31 | 2023-03-31 | 一种基于陀螺仪和加速度计的姿态校准方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202310336055.9A CN116358541A (zh) | 2023-03-31 | 2023-03-31 | 一种基于陀螺仪和加速度计的姿态校准方法 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN116358541A true CN116358541A (zh) | 2023-06-30 |
Family
ID=86915601
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202310336055.9A Pending CN116358541A (zh) | 2023-03-31 | 2023-03-31 | 一种基于陀螺仪和加速度计的姿态校准方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN116358541A (zh) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN116839634A (zh) * | 2023-08-29 | 2023-10-03 | 北京信普尼科技有限公司 | 一种用机械臂标定陀螺仪的方法与机械臂 |
-
2023
- 2023-03-31 CN CN202310336055.9A patent/CN116358541A/zh active Pending
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN116839634A (zh) * | 2023-08-29 | 2023-10-03 | 北京信普尼科技有限公司 | 一种用机械臂标定陀螺仪的方法与机械臂 |
CN116839634B (zh) * | 2023-08-29 | 2023-11-21 | 北京信普尼科技有限公司 | 一种用机械臂标定陀螺仪的方法与机械臂 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN102422166B (zh) | 移动状态检测装置 | |
CN107560613B (zh) | 基于九轴惯性传感器的机器人室内轨迹跟踪系统及方法 | |
CN111578937B (zh) | 同时优化外参数的视觉惯性里程计系统 | |
WO2014010727A1 (ja) | 移動体の進行方向推定装置及び進行方向推定方法 | |
CN107289951B (zh) | 一种基于惯性导航的室内移动机器人定位方法 | |
JP2010078595A5 (zh) | ||
CN116358541A (zh) | 一种基于陀螺仪和加速度计的姿态校准方法 | |
CN108132053B (zh) | 一种行人轨迹构建方法、系统及惯性测量装置 | |
CN108304594B (zh) | 一种基于速度与陀螺仪数据的汽车驾驶稳定性的判定方法 | |
CN110275191A (zh) | 一种静态漂移修正方法、装置、车载设备及存储介质 | |
CN110345936A (zh) | 运动装置的轨迹数据处理方法及其处理系统 | |
CN111504314A (zh) | Imu与刚体的位姿融合方法、装置、设备及存储介质 | |
CN110726511B (zh) | 一种带旋转机构的惯导系统的质心计算方法 | |
CN114201722A (zh) | 基于后处理车体-转向架安装关系动态计算方法 | |
JP2004150973A (ja) | 車両用加速度検出装置 | |
WO2014171227A1 (ja) | 姿勢角推定装置およびそれを備える移動状態検出装置 | |
CN113566828A (zh) | 一种基于多传感器决策融合的抗冲击扫描匹配方法及系统 | |
JP2002023919A (ja) | 姿勢検出装置及び操作入力装置 | |
CN108731675B (zh) | 待定位物航向变化量的测量方法、测量装置和电子设备 | |
CN110567483B (zh) | 一种mems传感器的数据处理方法 | |
CN110243462B (zh) | 运输测振仪振动检测方法 | |
CN112284378A (zh) | 惯性测量单元零漂移的自动驾驶控制方法 | |
CN108627155B (zh) | 一种农业机械非直线作业向心加速度估算及倾角融合方法 | |
JPH10160506A (ja) | 角度計測装置 | |
US11441924B2 (en) | Method and device for detecting the standstill of a vehicle |
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 |