CN113566849B - 惯性测量单元的安装角度标定方法、装置和计算机设备 - Google Patents
惯性测量单元的安装角度标定方法、装置和计算机设备 Download PDFInfo
- Publication number
- CN113566849B CN113566849B CN202110864781.9A CN202110864781A CN113566849B CN 113566849 B CN113566849 B CN 113566849B CN 202110864781 A CN202110864781 A CN 202110864781A CN 113566849 B CN113566849 B CN 113566849B
- Authority
- CN
- China
- Prior art keywords
- running
- information
- time
- speed
- predicted
- 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
- 238000005259 measurement Methods 0.000 title claims abstract description 61
- 238000009434 installation Methods 0.000 title claims abstract description 44
- 238000000034 method Methods 0.000 title claims abstract description 44
- 230000008859 change Effects 0.000 claims abstract description 199
- 230000001133 acceleration Effects 0.000 claims abstract description 160
- 238000006073 displacement reaction Methods 0.000 claims abstract description 47
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 21
- 238000012937 correction Methods 0.000 claims abstract description 16
- 238000004590 computer program Methods 0.000 claims description 20
- 230000005484 gravity Effects 0.000 claims description 19
- 238000012545 processing Methods 0.000 claims description 4
- 230000036544 posture Effects 0.000 claims 5
- 239000000758 substrate Substances 0.000 claims 1
- 238000004364 calculation method Methods 0.000 description 6
- 238000001914 filtration Methods 0.000 description 6
- 230000008569 process Effects 0.000 description 5
- 230000008878 coupling Effects 0.000 description 4
- 238000010168 coupling process Methods 0.000 description 4
- 238000005859 coupling reaction Methods 0.000 description 4
- 238000010586 diagram Methods 0.000 description 4
- 239000011159 matrix material Substances 0.000 description 3
- 230000000977 initiatory effect Effects 0.000 description 2
- 230000003068 static effect Effects 0.000 description 2
- 238000007796 conventional method Methods 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 230000003993 interaction Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
- G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
Landscapes
- Engineering & Computer Science (AREA)
- Manufacturing & Machinery (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Navigation (AREA)
Abstract
本申请涉及一种惯性测量单元的安装角度标定方法、装置、计算机设备和存储介质。所述方法包括:获取每个信息采集时刻的导航定位信息、速度变化信息以及实际行驶参数;采用组合定位算法,根据导航定位信息、速度变化信息以及实际行驶参数,得到估计姿态信息以及估计零偏;根据导航定位信息、估计姿态以及估计零偏进行加速度修正,得到每个信息采集时刻的修正后加速度;根据修正后加速度,得到对应的预测行驶速度;根据行驶终止时间的预测行驶速度进行速度修正,得到与信息采集时刻对应的修正后行驶速度;根据修正后行驶速度,得到与行驶终止时间对应的行驶位移,根据行驶位移,得到惯性测量单元的安装角度。采用本方法能够提高安装角度的标定精度。
Description
技术领域
本申请涉及自动驾驶技术领域,特别是涉及一种惯性测量单元的安装角度标定方法、装置和计算机设备。
背景技术
惯性测量单元由三轴加速度计与三轴陀螺仪组成,其坐标系与车辆坐标系之间存在一定的安装角度,当惯性测量单元应用于自动驾驶车辆时,需要对存在的安装角度进行准确标定。
传统技术中,对惯性测量单元的安装角度进行标定的方式为:一是根据全球导航卫星系统、惯性测量单元或轮速等测量的相关关系,将安装角度作为估计参数建立状态方差,使用卡尔曼滤波进行估计;二是使用车辆直线行驶时的加速度计测量进行安装角度估计。
然而,传统方法,第一种方式由于其估算的安装角度与惯性测量单元的姿态误差耦合,存在标定精度低的问题,第二种按时由于没有考虑加速度计偏零的影响,且其倾斜角和俯仰角由加速度计测量来估计,噪声较大,也会存在标定精度低的问题。
发明内容
基于此,有必要针对上述技术问题,提供一种能够提高安装角度的标定精度的惯性测量单元的安装角度标定方法、装置、计算机设备和存储介质。
一种惯性测量单元的安装角度标定方法,所述方法包括:
获取行驶时间段内每个信息采集时刻的导航定位信息、速度变化信息以及实际行驶参数;
采用组合定位算法,根据导航定位信息、速度变化信息以及实际行驶参数,得到估计姿态信息以及估计零偏;
根据导航定位信息、估计姿态以及估计零偏,对速度变化信息中加速度进行修正,得到每个信息采集时刻的修正后加速度;
根据修正后加速度,得到与每个信息采集时刻对应的预测行驶速度;
根据行驶终止时间的预测行驶速度,对与信息采集时刻对应的预测行驶速度进行修正,得到与信息采集时刻对应的修正后行驶速度;
根据修正后行驶速度,得到与行驶终止时间对应的行驶位移,根据行驶位移,得到惯性测量单元的安装角度。
在一个实施例中,采用组合定位算法,根据导航定位信息、速度变化信息以及实际行驶参数,得到估计姿态信息以及估计零偏包括:
根据行驶初始时刻的导航定位信息、速度变化信息以及实际行驶参数,得到第一预测时刻的第一车辆状态参数,第一预测时刻为与行驶初始时刻对应的下一时刻;
根据第一预测时刻的导航定位信息以及第一车辆状态参数,得到与第一预测时刻对应的行驶参数误差、导航定位误差以及速度变化零偏;
根据与第一预测时刻对应的行驶参数误差、导航定位误差以及速度变化零偏,迭代计算每个信息采集时刻对应的车辆状态参数以及速度变化零偏,得到估计姿态信息以及估计零偏。
在一个实施例中,实际行驶参数包括实际行驶姿态以及实际行驶速度;
根据行驶初始时刻的导航定位信息、速度变化信息以及实际行驶参数,得到第一预测时刻的第一车辆状态参数包括:
根据行驶初始时刻的实际行驶姿态以及速度变化信息中陀螺角速度,得到第一预测姿态信息;
根据第一预测姿态信息、行驶初始时刻的实际行驶速度以及速度变化信息中加速度计测量的比力,得到第一预测速度信息;
根据第一预测速度信息、行驶初始时刻的导航定位信息以及实际行驶速度,得到第一预测位置信息;
归集第一预测姿态信息、第一预测速度信息以及第一预测位置信息,得到第一预测时刻的第一车辆状态参数。
在一个实施例中,根据与第一预测时刻对应的行驶参数误差、导航定位误差以及速度变化零偏,迭代计算每个信息采集时刻对应的车辆状态参数以及速度变化零偏,得到估计姿态信息以及估计零偏包括:
将第一预测时刻作为当前时刻;
根据当前时刻的导航定位信息、速度变化信息、实际行驶参数以及与当前时刻对应的行驶参数误差、导航定位误差以及速度变化零偏,得到第二预测时刻的第二车辆状态参数,第二预测时刻为当前时刻的下一时刻;
根据第二预测时刻的导航定位信息以及第二车辆状态参数,得到第二预测时刻对应的行驶参数误差、导航定位误差以及速度变化零偏;
将第二预测时刻更新为当前时刻,返回根据当前时刻的导航定位信息、速度变化信息、实际行驶参数以及与当前时刻对应的行驶参数误差、导航定位误差以及速度变化零偏,得到第二预测时刻的第二车辆状态参数的步骤;
直到更新后的第二预测时刻为行驶时间段内的行驶终止时刻,根据行驶终止时刻的第二车辆状态参数得到估计姿态信息,根据行驶终止时刻的速度变化零偏得到估计零偏。
在一个实施例中,根据当前时刻的导航定位信息、速度变化信息、实际行驶参数以及与当前时刻对应的行驶参数误差、导航定位误差以及速度变化零偏,得到第二预测时刻的第二车辆状态参数包括:
根据与当前时刻对应的速度变化零偏,修正当前时刻的速度变化信息,得到修正后速度变化信息;
根据当前时刻的导航定位信息、实际行驶参数以及修正后速度变化信息,得到第二预测时刻的待调整车辆状态参数;
根据与当前时刻对应的行驶参数误差以及导航定位误差对待调整车辆状态参数进行修正,得到第二预测时刻的第二车辆状态参数。
在一个实施例中,根据导航定位信息、根据导航定位信息、估计姿态以及估计零偏,对速度变化信息中加速度进行修正,得到每个信息采集时刻的修正后加速度包括:
根据估计零偏中加速度计零偏,对速度变化信息中加速度进行修正,得到一次修正加速度,并确定与导航定位信息对应的重力加速度;
根据估计姿态以及重力加速度,确定待修正加速度;
根据待修正加速度对一次修正加速度进行修正,得到修正后加速度。
在一个实施例中,根据行驶终止时间的预测行驶速度,对与信息采集时刻对应的预测行驶速度进行修正,得到与信息采集时刻对应的修正后行驶速度包括:
根据行驶终止时间的预测行驶速度以及行驶时间段,确定与信息采集时刻对应的补偿速度;
将补偿速度叠加至与信息采集时刻对应的预测行驶速度,得到与信息采集时刻对应的修正后行驶速度。
一种惯性测量单元的安装角度标定装置,所述装置包括:
获取模块,用于获取行驶时间段内每个信息采集时刻的导航定位信息、速度变化信息以及实际行驶参数;
估计模块,用于采用组合定位算法,根据导航定位信息、速度变化信息以及实际行驶参数,得到估计姿态信息以及估计零偏;
第一修正模块,用于根据导航定位信息、估计姿态以及估计零偏,对速度变化信息中加速度进行修正,得到每个信息采集时刻的修正后加速度;
预测模块,用于根据修正后加速度,得到与每个信息采集时刻对应的预测行驶速度;
第二修正模块,用于根据行驶终止时间的预测行驶速度,对与信息采集时刻对应的预测行驶速度进行修正,得到与信息采集时刻对应的修正后行驶速度;
处理模块,用于根据修正后行驶速度,得到与行驶终止时间对应的行驶位移,根据行驶位移,得到惯性测量单元的安装角度。
一种计算机设备,包括存储器和处理器,所述存储器存储有计算机程序,所述处理器执行所述计算机程序时实现以下步骤:
获取行驶时间段内每个信息采集时刻的导航定位信息、速度变化信息以及实际行驶参数;
采用组合定位算法,根据导航定位信息、速度变化信息以及实际行驶参数,得到估计姿态信息以及估计零偏;
根据导航定位信息、估计姿态以及估计零偏,对速度变化信息中加速度进行修正,得到每个信息采集时刻的修正后加速度;
根据修正后加速度,根据修正后加速度,得到与每个信息采集时刻对应的预测行驶速度;
根据行驶终止时间的预测行驶速度,对与信息采集时刻对应的预测行驶速度进行修正,得到与信息采集时刻对应的修正后行驶速度;
根据修正后行驶速度,得到与行驶终止时间对应的行驶位移,根据行驶位移,得到惯性测量单元的安装角度。
一种计算机可读存储介质,其上存储有计算机程序,所述计算机程序被处理器执行时实现以下步骤:
获取行驶时间段内每个信息采集时刻的导航定位信息、速度变化信息以及实际行驶参数;
采用组合定位算法,根据导航定位信息、速度变化信息以及实际行驶参数,得到估计姿态信息以及估计零偏;
根据导航定位信息、估计姿态以及估计零偏,对速度变化信息中加速度进行修正,得到每个信息采集时刻的修正后加速度;
根据修正后加速度,得到与每个信息采集时刻对应的预测行驶速度;
根据行驶终止时间的预测行驶速度,对与信息采集时刻对应的预测行驶速度进行修正,得到与信息采集时刻对应的修正后行驶速度;
根据修正后行驶速度,得到与行驶终止时间对应的行驶位移,根据行驶位移,得到惯性测量单元的安装角度。
上述惯性测量单元的安装角度标定方法、装置、计算机设备和存储介质,通过获取行驶时间段内每个信息采集时刻的导航定位信息、速度变化信息以及实际行驶参数,采用组合定位算法,根据导航定位信息、速度变化信息以及实际行驶参数,得到估计姿态信息以及估计零偏,由于在估计的过程中不需要安装角度与姿态耦合,能够得到准确的估计姿态信息以及估计零偏,通过根据导航定位信息、估计姿态以及估计零偏,对速度变化信息中加速度进行修正,能够降低与导航定位信息对应的重力加速度和零偏对速度变化信息中加速度的影响,得到准确的修正后加速度,通过根据修正后加速度,得到与每个信息采集时刻对应的预测行驶速度,根据行驶终止时间的预测行驶速度,对与信息采集时刻对应的预测行驶速度进行修正,能够降低加速度计的累计误差,得到准确的与信息采集时刻对应的修正后行驶速度,最后根据修正后行驶速度,得到与行驶终止时间对应的行驶位移,利用行驶位移,计算惯性测量单元的安装角度,能够有效降低加速度计噪声的影响,得到高精度的惯性测量单元的安装角度,提高安装角度的标定精度。
附图说明
图1为一个实施例中惯性测量单元的安装角度标定方法的流程示意图;
图2为一个实施例中安装角度的示意图;
图3为一个实施例中组合定位的示意图;
图4为另一个实施例中惯性测量单元的安装角度标定方法的流程示意图;
图5为一个实施例中惯性测量单元的安装角度标定装置的结构框图;
图6为一个实施例中计算机设备的内部结构图。
具体实施方式
为了使本申请的目的、技术方案及优点更加清楚明白,以下结合附图及实施例,对本申请进行进一步详细说明。应当理解,此处描述的具体实施例仅仅用以解释本申请,并不用于限定本申请。
在一个实施例中,如图1所示,提供了一种惯性测量单元的安装角度标定方法,本实施例以该方法应用于服务器进行举例说明,可以理解的是,该方法也可以应用于终端,还可以应用于包括终端和服务器的系统,并通过终端和服务器的交互实现。其中,终端可以但不限于是各种个人计算机、笔记本电脑、智能手机、平板电脑和便携式可穿戴设备,服务器可以用独立的服务器或者是多个服务器组成的服务器集群来实现。本实施例中,该方法包括以下步骤:
步骤102,获取行驶时间段内每个信息采集时刻的导航定位信息、速度变化信息以及实际行驶参数。
其中,行驶时间段是指车辆完成一次完整的行驶所需要的时间。比如,行驶时间段具体可以是指车辆由静止到运动,再由运动到静止所需要的时间。信息采集时刻是指采集导航定位信息、速度变化信息以及实际行驶参数的时刻。比如,信息采集时刻可以是根据预先设置的采集间隔所确定的采集时间点。导航定位信息是指用于对车辆进行定位的定位信息。比如,导航定位信息具体可以是指根据全球导航卫星系统等定位系统所得到的经纬度位置信息。速度变化信息用于表征车辆的速度变化情况。比如速度变化信息具体可以是指惯性测量单元的加速度计以及陀螺仪所输出的信息。实际行驶参数用于表征车辆实际行驶过程中的状态。比如,实际行驶参数具体可以是指实际行驶姿态以及实际行驶速度。
具体的,全球导航卫星系统接收机可以获取到导航定位信息并输出,惯性测量单元可以获取到速度变化信息并输出,车辆上设置的传感器可以获取到实际行驶参数并输出,当车辆完成一次行驶后,需要对惯性测量单元的安装角度进行标定时,服务器会直接获取行驶时间段内每个信息采集时刻的导航定位信息、速度变化信息以及实际行驶参数。
步骤104,采用组合定位算法,根据导航定位信息、速度变化信息以及实际行驶参数,得到估计姿态信息以及估计零偏。
其中,组合定位算法是指融合导航定位信息、速度变化信息以及实际行驶参数,实现对惯性测量单元的姿态、速度、位置、加速度计零偏以及陀螺仪零偏的估计。估计姿态信息是指通过组合定位算法所估计的惯性测量单元的姿态。估计姿态信息包括侧倾角、俯仰角、航向角等。估计零偏是指通过组合定位算法所估计的惯性测量单元的零偏。估计零偏包括加速度计零偏以及陀螺仪零偏。
具体的,服务器会先根据行驶初始时刻的导航定位信息、速度变化信息以及实际行驶参数,预测与行驶初始时刻对应的下一时刻的车辆状态参数,再根据下一时刻的导航定位信息以及车辆状态参数,利用误差状态卡尔曼滤波,得到与下一时刻对应的行驶参数误差、导航定位误差以及速度变化零偏,最后根据与下一时刻对应的行驶参数误差、导航定位误差以及速度变化零偏,迭代计算每个信息采集时刻对应的车辆状态参数以及速度变化零偏,得到估计姿态信息以及估计零偏。
步骤106,根据导航定位信息、估计姿态以及估计零偏,对速度变化信息中加速度进行修正,得到每个信息采集时刻的修正后加速度。
其中,加速度是通过惯性测量单元中的加速度计得到的,惯性测量单元中的加速度计为三轴加速度计,该加速度包括侧向加速度、前向加速度等。
具体的,服务器会先根据估计零偏中的加速度计零偏,对速度变化信息中加速度进行修正,得到一次修正加速度,再根据导航定位信息确定对应的重力加速度,利用重力加速度以及估计姿态对一次修正加速度进行修正,得到修正后加速度。
步骤108,根据修正后加速度,得到与每个信息采集时刻对应的预测行驶速度。
其中,修正后加速度是指修正过后所得到的加速度,包括修正后侧向加速度和修正后前向加速度。预测行驶速度是指利用修正后加速度进行预测得到的与每个信息采集时刻对应的行驶速度,与修正后加速度对应,预测行驶速度包括侧向行驶速度和前向行驶速度。这里需要说明的是,由于在行驶初始时刻车辆是静止的,与行驶初始时刻对应的预测行驶速度为0。
具体的,服务器会根据修正后加速度进行迭代计算,依次对每个信息采集时刻对应的行驶速度进行预测,得到与每个信息采集时刻对应的预测行驶速度。依次对每个信息采集时刻对应的行驶速度进行预测的方式可以为:从与行驶初始时刻对应的下一时刻开始,根据该下一时刻的修正后加速度,以及与该下一时刻对应的当前时刻已经得到的预测行驶速度,得到该下一时刻的预测行驶速度。
举例说明,修正后加速度包括修正后侧向加速度和修正后前向加速度,与修正后加速度对应,预测行驶速度包括侧向行驶速度和前向行驶速度,具体的预测公式可以为:v_r(k+1)=v_r(k)+acce_r(k)*deltaT、v_f(k+1)=v_f(k)+acce_f(k)*deltaT,其中,v_r(k+1)表示下一时刻的侧向行驶速度,v_r(k)表示当前时刻的侧向行驶速度,acce_r(k)表示修正后侧向加速度,deltaT表示当前时刻与下一时刻之间的时间间隔,v_f(k+1)表示下一时刻的前向行驶速度,v_f(k)表示当前时刻的前向行驶速度,acce_f(k)表示修正后前向加速度。
步骤110,根据行驶终止时间的预测行驶速度,对与信息采集时刻对应的预测行驶速度进行修正,得到与信息采集时刻对应的修正后行驶速度。
具体的,当车辆从运动到静止时,侧向行驶速度和前向行驶速度的数值应当为零。但是实际上由于模型误差等原因,在行驶终止时刻得到的预测行驶速度非零,因此需要利用行驶终止时刻得到的预测行驶速度,分别对每个信息采集时刻对应的非零速度按时间进行线性补偿。
其中,具体的补偿公式可以为:V1=V0+t/T*Vend,其中,Vend为行驶终止时间的预测行驶速度,t为速度补偿的时刻,可通过信息采集时刻与行驶初始时刻的差值得到,T为整个行驶时间段,V0为预测行驶速度,V1为修正后行驶速度。
步骤112,根据修正后行驶速度,得到与行驶终止时间对应的行驶位移,根据行驶位移,得到惯性测量单元的安装角度。
其中,与修正后行驶速度对应的,行驶位移包括侧向位移和前向位移。
具体的,服务器会根据修正后行驶速度进行迭代计算,依次对每个信息采集时刻对应的行驶位移进行预测,得到与行驶终止时刻对应的行驶位移。此处需要说明的是,行驶初始时刻的行驶位移为0。依次对每个信息采集时刻对应的行驶位移进行预测的方式可以为:从与行驶初始时刻对应的下一时刻开始,根据该下一时刻的修正后行驶速度,以及与该下一时刻对应的当前时刻已经得到的行驶位移,得到该下一时刻的行驶位移。根据与行驶终止时刻对应的修正后行驶速度,以及与行驶终止时刻对应的上一时刻的行驶位移,即可得到与行驶终止时间对应的行驶位移。在得到行驶位移后,服务器会利用安装角度计算公式以及行驶位移,计算惯性测量单元的安装角度。其中,安装角度计算公式具体可以为:α=tan-1(s_r/s_f),其中α为安装角度,s_r为侧向位移,s_f为前向位移,举例说明,本实施例中的侧向位移具体可以是指右向位移,安装角度的示意图如图2所示。
上述惯性测量单元的安装角度标定方法,通过获取行驶时间段内每个信息采集时刻的导航定位信息、速度变化信息以及实际行驶参数,采用组合定位算法,根据导航定位信息、速度变化信息以及实际行驶参数,得到估计姿态信息以及估计零偏,由于在估计的过程中不需要安装角度与姿态耦合,能够得到准确的估计姿态信息以及估计零偏,通过根据导航定位信息、估计姿态以及估计零偏,对速度变化信息中加速度进行修正,能够降低与导航定位信息对应的重力加速度和零偏对速度变化信息中加速度的影响,得到准确的修正后加速度,通过根据修正后加速度,得到与每个信息采集时刻对应的预测行驶速度,根据行驶终止时间的预测行驶速度,对与信息采集时刻对应的预测行驶速度进行修正,能够降低加速度计的累计误差,得到准确的与信息采集时刻对应的修正后行驶速度,最后根据修正后行驶速度,得到与行驶终止时间对应的行驶位移,利用行驶位移,计算惯性测量单元的安装角度,能够有效降低加速度计噪声的影响,得到高精度的惯性测量单元的安装角度,提高安装角度的标定精度。
在一个实施例中,采用组合定位算法,根据导航定位信息、速度变化信息以及实际行驶参数,得到估计姿态信息以及估计零偏包括:
根据行驶初始时刻的导航定位信息、速度变化信息以及实际行驶参数,得到第一预测时刻的第一车辆状态参数,第一预测时刻为与行驶初始时刻对应的下一时刻;
根据第一预测时刻的导航定位信息以及第一车辆状态参数,得到与第一预测时刻对应的行驶参数误差、导航定位误差以及速度变化零偏;
根据与第一预测时刻对应的行驶参数误差、导航定位误差以及速度变化零偏,迭代计算每个信息采集时刻对应的车辆状态参数以及速度变化零偏,得到估计姿态信息以及估计零偏。
其中,车辆状态参数是指用于表征车辆状态的参数,包括车辆姿态、车辆速度以及车辆位置。行驶参数误差用于表征在对车辆姿态和车辆速度进行预测时可能出现的偏差,包括姿态误差和速度误差。导航定位误差用于表征在对车辆位置进行预测时可能出现的偏差。速度变化零偏是指与速度变化对应的物理量的零偏,包括加速度计零偏和陀螺零偏。
具体的,服务器会先根据行驶初始时刻的导航定位信息、速度变化信息以及实际行驶参数,对与行驶初始时刻对应的下一时刻(即第一预测时刻)的车辆状态进行预测,得到第一预测时刻的第一车辆状态参数,再根据第一预测时刻的导航定位信息以及第一车辆状态参数中车辆位置,计算位置误差,利用误差状态卡尔曼滤波,根据位置误差以及第一车辆状态参数,得到第一预测时刻对应的行驶参数误差、导航定位误差以及速度变化零偏,最后根据与第一预测时刻对应的行驶参数误差、导航定位误差以及速度变化零偏,迭代计算每个信息采集时刻对应的车辆状态参数以及速度变化零偏,得到估计姿态信息以及估计零偏。
其中,利用误差状态卡尔曼滤波,根据位置误差以及第一车辆状态参数,得到第一预测时刻对应的行驶参数误差、导航定位误差以及速度变化零偏是指根据位置误差以及第一车辆状态参数,得到误差状态卡尔曼滤波对应的状态量,基于状态量进行误差预测,得到第一预测时刻对应的行驶参数误差、导航定位误差以及速度变化零偏。
本实施例中,通过采用组合定位算法,根据导航定位信息、速度变化信息以及实际行驶参数,得到估计姿态信息以及估计零偏,不需要使用安装角度作为状态量,将安装角度与姿态耦合,能够实现对姿态信息和零偏的准确估计。
在一个实施例中,实际行驶参数包括实际行驶姿态以及实际行驶速度;
根据行驶初始时刻的导航定位信息、速度变化信息以及实际行驶参数,得到第一预测时刻的第一车辆状态参数包括:
根据行驶初始时刻的实际行驶姿态以及速度变化信息中陀螺角速度,得到第一预测姿态信息;
根据第一预测姿态信息、行驶初始时刻的实际行驶速度以及速度变化信息中加速度计测量的比力,得到第一预测速度信息;
根据第一预测速度信息、行驶初始时刻的导航定位信息以及实际行驶速度,得到第一预测位置信息;
归集第一预测姿态信息、第一预测速度信息以及第一预测位置信息,得到第一预测时刻的第一车辆状态参数。
具体的,实际行驶参数包括实际行驶姿态以及实际行驶速度,在得到第一预测时刻的第一车辆状态参数时,服务器会先根据行驶初始时刻的实际行驶姿态以及速度变化信息中陀螺角速度,得到第一预测姿态信息,再利用第一预测姿态信息、行驶初始时刻的实际行驶速度以及速度变化信息中加速度计测量的比力,得到第一预测速度信息,再根据第一预测速度信息、行驶初始时刻的导航定位信息以及实际行驶速度,得到第一预测位置信息,最后通过归集第一预测姿态信息、第一预测速度信息以及第一预测位置信息,得到第一预测时刻的第一车辆状态参数。
下面对得到第一预测姿态信息进行举例说明。
具体的,可以利用矩阵链乘得到第一预测姿态信息:
其中,为第一预测姿态信息,式中的/>由地球自转角速度/>计算得到(如公式(2)所示),/>由陀螺角速度/>计算得到(如公式(3)所示);/>和分别为tk-1时刻(即行驶初始时刻)和tk时刻(即第一预测时刻)的姿态矩阵,Δt为行驶初始时刻与第一预测时刻之间的时间间隔。
下面对得到第一预测速度信息进行举例说明。
在导航坐标系下的速度微分方程可以表示为:
其中,为第一预测姿态信息,/>为加速度计测量的比力,gn为重力矢量,/>代表了载体运动导致的向心加速度,/>代表了由于地球自转和载体运动导致的科里奥利力。其中:
其中,分别为n系(此处具体可以为导航坐标系)下的东向速度与北向速度(可通过行驶初始时刻的实际行驶速度得到),e代表地球扁平率,L为纬度,h为海拔高度,RM和RN则分别表示地球子午圈和卯酉圈曲率半径。
则第一预测速度信息v(k):
其中,v(k-1)为行驶初始时刻的实际行驶速度,Δt为行驶初始时刻与第一预测时刻之间的时间间隔。
下面对得到第一预测位置信息进行举例说明。
惯性导航系统的位置微分方程为:
其中,为n系(此处具体可以为导航坐标系)下的北向速度,/>为n系(此处具体可以为导航坐标系)下的东向速度(可通过行驶初始时刻的实际行驶速度得到),λ为经度,L为纬度(可通过行驶初始时刻的导航定位信息得到)。
使用矩阵乘法可以表示为:
采用梯形积分法计算位置更新递推方程:
其中,为第一预测位置信息,/>为行驶初始时刻的导航定位信息,Δt为行驶初始时刻与第一预测时刻之间的时间间隔。
本实施例中,通过先利用实际行驶姿态以及陀螺角速度,得到第一预测姿态信息,再利用第一预测姿态信息、实际行驶速度以及加速度计测量的比力,得到第一预测速度信息,再利用第一预测速度信息、导航定位信息以及实际行驶速度,得到第一预测位置信息,能够实现通过归集第一预测姿态信息、第一预测速度信息以及第一预测位置信息,得到第一车辆状态信息。
在一个实施例中,根据与第一预测时刻对应的行驶参数误差、导航定位误差以及速度变化零偏,迭代计算每个信息采集时刻对应的车辆状态参数以及速度变化零偏,得到估计姿态信息以及估计零偏包括:
将第一预测时刻作为当前时刻;
根据当前时刻的导航定位信息、速度变化信息、实际行驶参数以及与当前时刻对应的行驶参数误差、导航定位误差以及速度变化零偏,得到第二预测时刻的第二车辆状态参数,第二预测时刻为当前时刻的下一时刻;
根据第二预测时刻的导航定位信息以及第二车辆状态参数,得到第二预测时刻对应的行驶参数误差、导航定位误差以及速度变化零偏;
将第二预测时刻更新为当前时刻,返回根据当前时刻的导航定位信息、速度变化信息、实际行驶参数以及与当前时刻对应的行驶参数误差、导航定位误差以及速度变化零偏,得到第二预测时刻的第二车辆状态参数的步骤;
直到更新后的第二预测时刻为行驶时间段内的行驶终止时刻,根据行驶终止时刻的第二车辆状态参数得到估计姿态信息,根据行驶终止时刻的速度变化零偏得到估计零偏。
具体的,服务器会将第一预测时刻作为当前时刻,根据与当前时刻对应的速度变化零偏,修正当前时刻的速度变化信息,根据修正后速度变化信息、当前时刻的导航定位信息以及实际行驶参数,得到第二预测时刻的第二车辆状态参数。得到第二车辆状态参数后,服务器会根据第二预测时刻的导航定位信息以及第二车辆状态参数中车辆位置,计算位置误差,利用误差状态卡尔曼滤波,根据位置误差以及第二车辆状态参数,得到第二预测时刻对应的行驶参数误差、导航定位误差以及速度变化零偏,将第二预测时刻更新为当前时刻,返回根据当前时刻的导航定位信息、速度变化信息、实际行驶参数以及与当前时刻对应的行驶参数误差、导航定位误差以及速度变化零偏,得到第二预测时刻的第二车辆状态参数的步骤,直到更新后的第二预测时刻为行驶时间段内的行驶终止时刻,将行驶终止时刻的第二车辆状态参数中的车辆姿态作为估计姿态信息,将行驶终止时刻的速度变化零偏作为估计零偏。
其中,利用误差状态卡尔曼滤波,根据位置误差以及第二车辆状态参数,得到第二预测时刻对应的行驶参数误差、导航定位误差以及速度变化零偏是指根据位置误差以及第二车辆状态参数,得到误差状态卡尔曼滤波对应的状态量,基于状态量进行误差预测,得到第二预测时刻对应的行驶参数误差、导航定位误差以及速度变化零偏。举例说明,迭代计算每个信息采集时刻对应的车辆状态参数以及速度变化零偏,得到估计姿态信息以及估计零偏的过程可以如图3所示,其中,第二预测时刻的导航定位信息是指观测位置,当前时刻的速度变化信息是指惯性测量单元中加速度计以及陀螺输出的速度变化信息,车辆状态参数是指姿态、速度以及位置(具体为图3中的预测位置),速度变化零偏是指陀螺零偏以及加速度计零偏,行驶参数误差是指姿态误差以及速度误差,导航定位误差是指位置误差。
本实施例中,通过根据与第一预测时刻对应的行驶参数误差、导航定位误差以及速度变化零偏,迭代计算每个信息采集时刻对应的车辆状态参数以及速度变化零偏,得到估计姿态信息以及估计零偏,不需要使用安装角度作为状态量,将安装角度与姿态耦合,能够实现对姿态信息和零偏的准确估计。
在一个实施例中,根据当前时刻的导航定位信息、速度变化信息、实际行驶参数以及与当前时刻对应的行驶参数误差、导航定位误差以及速度变化零偏,得到第二预测时刻的第二车辆状态参数包括:
根据与当前时刻对应的速度变化零偏,修正当前时刻的速度变化信息,得到修正后速度变化信息;
根据当前时刻的导航定位信息、实际行驶参数以及修正后速度变化信息,得到第二预测时刻的待调整车辆状态参数;
根据与当前时刻对应的行驶参数误差以及导航定位误差对待调整车辆状态参数进行修正,得到第二预测时刻的第二车辆状态参数。
其中,待调整车辆状态参数是指根据当前时刻的导航定位信息、实际行驶参数以及修正后速度变化信息预测得到的车辆状态参数。此处的预测方式与根据行驶初始时刻的导航定位信息、速度变化信息以及实际行驶参数,得到第一预测时刻的第一车辆状态参数的过程类似,本实施例在此处不再赘述。
具体的,服务器会根据与当前时刻对应的速度变化零偏,修正当前时刻的速度变化信息,得到修正后速度变化信息,根据当前时刻的导航定位信息、实际行驶参数以及修正后速度变化信息,得到第二预测时刻的待调整车辆状态参数,根据与当前时刻对应的行驶参数误差以及导航定位误差对待调整车辆状态参数进行修正,得到第二预测时刻的第二车辆状态参数。
继续以图3为例进行说明,服务器会根据陀螺零偏对陀螺角速度进行修正,并根据加速度计零偏对加速度进行修正,得到修正后速度变化信息(包括修正后陀螺角速度以及修正后加速度),根据当前时刻的导航定位信息、实际行驶参数以及修正后速度变化信息,得到第二预测时刻的待调整车辆状态参数,根据与当前时刻对应的行驶参数误差(姿态误差以及速度误差)以及导航定位误差(位置误差)对待调整车辆状态参数进行修正(姿态更新、速度更新、位置更新),得到第二预测时刻的第二车辆状态参数。
本实施例中,能够根据当前时刻的导航定位信息、速度变化信息、实际行驶参数以及与当前时刻对应的行驶参数误差、导航定位误差以及速度变化零偏,实现对第二预测时刻的第二车辆状态参数的获取。
在一个实施例中,根据导航定位信息、根据导航定位信息、估计姿态以及估计零偏,对速度变化信息中加速度进行修正,得到每个信息采集时刻的修正后加速度包括:
根据估计零偏中加速度计零偏,对速度变化信息中加速度进行修正,得到一次修正加速度,并确定与导航定位信息对应的重力加速度;
根据估计姿态以及重力加速度,确定待修正加速度;
根据待修正加速度对一次修正加速度进行修正,得到修正后加速度。
具体的,服务器会用速度变化信息中加速度减去估计零偏中加速度计零偏,得到一次修正加速度,并采用重力加速度计算公式计算与导航定位信息对应的重力加速度,根据估计姿态中的侧倾角以及俯仰角,对重力加速度导致的加速度进行修正,得到待修正加速度,将待修正加速度叠加至一次修正加速度,得到修正后加速度。
其中,重力加速度计算公式具体可以为:
gL=9.780325×(1+0.00530240×sin2L-0.00000582×sin2L)其中,
L为导航定位信息中的纬度信息。
其中,根据估计姿态中的侧倾角以及俯仰角,对重力加速度导致的加速度进行修正,得到待修正加速度具体可以为:分别得到与侧倾角(roll)对应的第一待修正加速度(gL*tan(roll))以及与俯仰角(pitch)对应的第二待修正加速度(gL*tan(pitch))。将待修正加速度叠加至一次修正加速度,得到修正后加速度包括:用一次修正加速度中的侧向加速度减去第一待修正加速度,得到修正后侧向加速度,用一次修正加速度中的前向加速度减去第二待修正加速度,得到修正后前向加速度。
本实施例中,通过根据导航定位信息、根据导航定位信息、估计姿态以及估计零偏,对速度变化信息中加速度进行修正,得到每个信息采集时刻的修正后加速度,能够降低重力加速度和零偏对加速度计测量的影响,提高安装角度标定的精度。
在一个实施例中,根据行驶终止时间的预测行驶速度,对与信息采集时刻对应的预测行驶速度进行修正,得到与信息采集时刻对应的修正后行驶速度包括:
根据行驶终止时间的预测行驶速度以及行驶时间段,确定与信息采集时刻对应的补偿速度;
将补偿速度叠加至与信息采集时刻对应的预测行驶速度,得到与信息采集时刻对应的修正后行驶速度。
具体的,服务器会计算信息采集时刻与行驶初始时刻的差值,根据差值、行驶终止时间的预测行驶速度以及行驶时间段,得到与信息采集时刻对应的补偿速度,将补偿速度叠加至与信息采集时刻对应的预测行驶速度,得到与信息采集时刻对应的修正后行驶速度。
其中,在进行修正时所依据的公式可以为:V1=V0+t/T*Vend,其中,Vend为行驶终止时间的预测行驶速度,t为速度补偿的时刻,可通过信息采集时刻与行驶初始时刻的差值得到,T为整个行驶时间段,V0为预测行驶速度,V1为修正后行驶速度。
本实施例中,通过根据行驶终止时间的预测行驶速度以及行驶时间段,确定与信息采集时刻对应的补偿速度,能够利用补偿速度实现对与信息采集时刻对应的预测行驶速度的修正,降低了加速度计积分的累计误差,得到与信息采集时刻对应的修正后行驶速度。
在一个实施例中,通过如图4所示的流程示意图来说明本申请的惯性测量单元的安装角度标定方法。
服务器获取行驶时间段内每个信息采集时刻的导航定位信息、速度变化信息以及实际行驶参数,采用组合定位算法(图4中的GNSS/IMU组合定位),根据导航定位信息、速度变化信息以及实际行驶参数,得到估计姿态信息(图4中的IMU侧倾角、俯仰角)以及估计零偏(包括图4中的加速度计零偏),根据估计零偏中加速度计零偏,对速度变化信息中加速度(对应图4中的IMU加速度输出)进行修正(对应图4中的加速度计零偏修正),得到一次修正加速度,并确定与导航定位信息对应的重力加速度,根据估计姿态(图4中的IMU侧倾角、俯仰角)以及重力加速度,确定待修正加速度,根据待修正加速度对一次修正加速度进行修正,得到修正后加速度(对应图4中的重力加速度修正,得到右向加速度和前向加速度),根据修正后加速度(对应图4中的右向加速度和前向加速度),得到与每个信息采集时刻对应的预测行驶速度(对应图4中的计算右向速度和计算前向速度),根据行驶终止时间的预测行驶速度,对与信息采集时刻对应的预测行驶速度进行修正,得到与信息采集时刻对应的修正后行驶速度(对应图4中的右向速度误差补偿和前向速度误差补偿),根据修正后行驶速度,得到与行驶终止时间对应的行驶位移(对应图4中的计算右向位移和计算前向位移),根据行驶位移,得到惯性测量单元的安装角度。
应该理解的是,虽然上述实施例涉及的各流程图中的各个步骤按照箭头的指示依次显示,但是这些步骤并不是必然按照箭头指示的顺序依次执行。除非本文中有明确的说明,这些步骤的执行并没有严格的顺序限制,这些步骤可以以其它的顺序执行。而且,上述实施例涉及的各流程图中的至少一部分步骤可以包括多个步骤或者多个阶段,这些步骤或者阶段并不必然是在同一时刻执行完成,而是可以在不同的时刻执行,这些步骤或者阶段的执行顺序也不必然是依次进行,而是可以与其它步骤或者其它步骤中的步骤或者阶段的至少一部分轮流或者交替地执行。
在一个实施例中,如图5所示,提供了一种惯性测量单元的安装角度标定装置,包括:获取模块502、估计模块504、第一修正模块506、预测模块508、第二修正模块510和处理模块512,其中:
获取模块502,用于获取行驶时间段内每个信息采集时刻的导航定位信息、速度变化信息以及实际行驶参数;
估计模块504,用于采用组合定位算法,根据导航定位信息、速度变化信息以及实际行驶参数,得到估计姿态信息以及估计零偏;
第一修正模块506,用于根据导航定位信息、估计姿态以及估计零偏,对速度变化信息中加速度进行修正,得到每个信息采集时刻的修正后加速度;
预测模块508,用于根据修正后加速度,得到与每个信息采集时刻对应的预测行驶速度;
第二修正模块510,用于根据行驶终止时间的预测行驶速度,对与信息采集时刻对应的预测行驶速度进行修正,得到与信息采集时刻对应的修正后行驶速度;
处理模块512,用于根据修正后行驶速度,得到与行驶终止时间对应的行驶位移,根据行驶位移,得到惯性测量单元的安装角度。
上述惯性测量单元的安装角度标定方法,通过获取行驶时间段内每个信息采集时刻的导航定位信息、速度变化信息以及实际行驶参数,采用组合定位算法,根据导航定位信息、速度变化信息以及实际行驶参数,得到估计姿态信息以及估计零偏,由于在估计的过程中不需要安装角度与姿态耦合,能够得到准确的估计姿态信息以及估计零偏,通过根据导航定位信息、估计姿态以及估计零偏,对速度变化信息中加速度进行修正,能够降低与导航定位信息对应的重力加速度和零偏对速度变化信息中加速度的影响,得到准确的修正后加速度,通过根据修正后加速度,得到与每个信息采集时刻对应的预测行驶速度,根据行驶终止时间的预测行驶速度,对与信息采集时刻对应的预测行驶速度进行修正,能够降低加速度计的累计误差,得到准确的与信息采集时刻对应的修正后行驶速度,最后根据修正后行驶速度,得到与行驶终止时间对应的行驶位移,利用行驶位移,计算惯性测量单元的安装角度,能够有效降低加速度计噪声的影响,得到高精度的惯性测量单元的安装角度,提高安装角度的标定精度。
在一个实施例中,估计模块还用于根据行驶初始时刻的导航定位信息、速度变化信息以及实际行驶参数,得到第一预测时刻的第一车辆状态参数,第一预测时刻为与行驶初始时刻对应的下一时刻,根据第一预测时刻的导航定位信息以及第一车辆状态参数,得到与第一预测时刻对应的行驶参数误差、导航定位误差以及速度变化零偏,根据与第一预测时刻对应的行驶参数误差、导航定位误差以及速度变化零偏,迭代计算每个信息采集时刻对应的车辆状态参数以及速度变化零偏,得到估计姿态信息以及估计零偏。
在一个实施例中,实际行驶参数包括实际行驶姿态以及实际行驶速度,估计模块还用于根据行驶初始时刻的实际行驶姿态以及速度变化信息中陀螺角速度,得到第一预测姿态信息,根据第一预测姿态信息、行驶初始时刻的实际行驶速度以及速度变化信息中加速度计测量的比力,得到第一预测速度信息,根据第一预测速度信息、行驶初始时刻的导航定位信息以及实际行驶速度,得到第一预测位置信息,归集第一预测姿态信息、第一预测速度信息以及第一预测位置信息,得到第一预测时刻的第一车辆状态参数。
在一个实施例中,估计模块还用于将第一预测时刻作为当前时刻,根据当前时刻的导航定位信息、速度变化信息、实际行驶参数以及与当前时刻对应的行驶参数误差、导航定位误差以及速度变化零偏,得到第二预测时刻的第二车辆状态参数,第二预测时刻为当前时刻的下一时刻,根据第二预测时刻的导航定位信息以及第二车辆状态参数,得到第二预测时刻对应的行驶参数误差、导航定位误差以及速度变化零偏,将第二预测时刻更新为当前时刻,返回根据当前时刻的导航定位信息、速度变化信息、实际行驶参数以及与当前时刻对应的行驶参数误差、导航定位误差以及速度变化零偏,得到第二预测时刻的第二车辆状态参数的步骤,直到更新后的第二预测时刻为行驶时间段内的行驶终止时刻,根据行驶终止时刻的第二车辆状态参数得到估计姿态信息,根据行驶终止时刻的速度变化零偏得到估计零偏。
在一个实施例中,估计模块还用于根据与当前时刻对应的速度变化零偏,修正当前时刻的速度变化信息,得到修正后速度变化信息,根据当前时刻的导航定位信息、实际行驶参数以及修正后速度变化信息,得到第二预测时刻的待调整车辆状态参数,根据与当前时刻对应的行驶参数误差以及导航定位误差对待调整车辆状态参数进行修正,得到第二预测时刻的第二车辆状态参数。
在一个实施例中,第一修正模块还用于根据估计零偏中加速度计零偏,对速度变化信息中加速度进行修正,得到一次修正加速度,并确定与导航定位信息对应的重力加速度,根据估计姿态以及重力加速度,确定待修正加速度,根据待修正加速度对一次修正加速度进行修正,得到修正后加速度。
在一个实施例中,第二修正模块还用于根据行驶终止时间的预测行驶速度以及行驶时间段,确定与信息采集时刻对应的补偿速度,将补偿速度叠加至与信息采集时刻对应的预测行驶速度,得到与信息采集时刻对应的修正后行驶速度。
关于惯性测量单元的安装角度标定装置的具体实施例可以参见上文中对于惯性测量单元的安装角度标定方法的实施例,在此不再赘述。上述惯性测量单元的安装角度标定装置中的各个模块可全部或部分通过软件、硬件及其组合来实现。上述各模块可以硬件形式内嵌于或独立于计算机设备中的处理器中,也可以以软件形式存储于计算机设备中的存储器中,以便于处理器调用执行以上各个模块对应的操作。
在一个实施例中,提供了一种计算机设备,该计算机设备可以是服务器,其内部结构图可以如图6所示。该计算机设备包括通过系统总线连接的处理器、存储器和网络接口。其中,该计算机设备的处理器用于提供计算和控制能力。该计算机设备的存储器包括非易失性存储介质、内存储器。该非易失性存储介质存储有操作系统、计算机程序和数据库。该内存储器为非易失性存储介质中的操作系统和计算机程序的运行提供环境。该计算机设备的数据库用于存储行驶时间段内每个信息采集时刻的导航定位信息、速度变化信息以及实际行驶参数等数据。该计算机设备的网络接口用于与外部的终端通过网络连接通信。该计算机程序被处理器执行时以实现一种惯性测量单元的安装角度标定方法。
本领域技术人员可以理解,图6中示出的结构,仅仅是与本申请方案相关的部分结构的框图,并不构成对本申请方案所应用于其上的计算机设备的限定,具体的计算机设备可以包括比图中所示更多或更少的部件,或者组合某些部件,或者具有不同的部件布置。
在一个实施例中,提供了一种计算机设备,包括存储器和处理器,存储器中存储有计算机程序,该处理器执行计算机程序时实现以下步骤:
获取行驶时间段内每个信息采集时刻的导航定位信息、速度变化信息以及实际行驶参数;
采用组合定位算法,根据导航定位信息、速度变化信息以及实际行驶参数,得到估计姿态信息以及估计零偏;
根据导航定位信息、估计姿态以及估计零偏,对速度变化信息中加速度进行修正,得到每个信息采集时刻的修正后加速度;
根据修正后加速度,得到与每个信息采集时刻对应的预测行驶速度;
根据行驶终止时间的预测行驶速度,对与信息采集时刻对应的预测行驶速度进行修正,得到与信息采集时刻对应的修正后行驶速度;
根据修正后行驶速度,得到与行驶终止时间对应的行驶位移,根据行驶位移,得到惯性测量单元的安装角度。
在一个实施例中,处理器执行计算机程序时还实现以下步骤:根据行驶初始时刻的导航定位信息、速度变化信息以及实际行驶参数,得到第一预测时刻的第一车辆状态参数,第一预测时刻为与行驶初始时刻对应的下一时刻;根据第一预测时刻的导航定位信息以及第一车辆状态参数,得到与第一预测时刻对应的行驶参数误差、导航定位误差以及速度变化零偏;根据与第一预测时刻对应的行驶参数误差、导航定位误差以及速度变化零偏,迭代计算每个信息采集时刻对应的车辆状态参数以及速度变化零偏,得到估计姿态信息以及估计零偏。
在一个实施例中,处理器执行计算机程序时还实现以下步骤:根据行驶初始时刻的实际行驶姿态以及速度变化信息中陀螺角速度,得到第一预测姿态信息;根据第一预测姿态信息、行驶初始时刻的实际行驶速度以及速度变化信息中加速度计测量的比力,得到第一预测速度信息;根据第一预测速度信息、行驶初始时刻的导航定位信息以及实际行驶速度,得到第一预测位置信息;归集第一预测姿态信息、第一预测速度信息以及第一预测位置信息,得到第一预测时刻的第一车辆状态参数。
在一个实施例中,处理器执行计算机程序时还实现以下步骤:将第一预测时刻作为当前时刻;根据当前时刻的导航定位信息、速度变化信息、实际行驶参数以及与当前时刻对应的行驶参数误差、导航定位误差以及速度变化零偏,得到第二预测时刻的第二车辆状态参数,第二预测时刻为当前时刻的下一时刻;根据第二预测时刻的导航定位信息以及第二车辆状态参数,得到第二预测时刻对应的行驶参数误差、导航定位误差以及速度变化零偏;将第二预测时刻更新为当前时刻,返回根据当前时刻的导航定位信息、速度变化信息、实际行驶参数以及与当前时刻对应的行驶参数误差、导航定位误差以及速度变化零偏,得到第二预测时刻的第二车辆状态参数的步骤;直到更新后的第二预测时刻为行驶时间段内的行驶终止时刻,根据行驶终止时刻的第二车辆状态参数得到估计姿态信息,根据行驶终止时刻的速度变化零偏得到估计零偏。
在一个实施例中,处理器执行计算机程序时还实现以下步骤:根据与当前时刻对应的速度变化零偏,修正当前时刻的速度变化信息,得到修正后速度变化信息;根据当前时刻的导航定位信息、实际行驶参数以及修正后速度变化信息,得到第二预测时刻的待调整车辆状态参数;根据与当前时刻对应的行驶参数误差以及导航定位误差对待调整车辆状态参数进行修正,得到第二预测时刻的第二车辆状态参数。
在一个实施例中,处理器执行计算机程序时还实现以下步骤:根据估计零偏中加速度计零偏,对速度变化信息中加速度进行修正,得到一次修正加速度,并确定与导航定位信息对应的重力加速度;根据估计姿态以及重力加速度,确定待修正加速度;根据待修正加速度对一次修正加速度进行修正,得到修正后加速度。
在一个实施例中,处理器执行计算机程序时还实现以下步骤:根据行驶终止时间的预测行驶速度以及行驶时间段,确定与信息采集时刻对应的补偿速度;将补偿速度叠加至与信息采集时刻对应的预测行驶速度,得到与信息采集时刻对应的修正后行驶速度。
在一个实施例中,提供了一种计算机可读存储介质,其上存储有计算机程序,该计算机程序被处理器执行时实现上述各方法实施例中的步骤。
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程,是可以通过计算机程序来指令相关的硬件来完成,所述的计算机程序可存储于一非易失性计算机可读取存储介质中,该计算机程序在执行时,可包括如上述各方法的实施例的流程。其中,本申请所提供的各实施例中所使用的对存储器、存储、数据库或其它介质的任何引用,均可包括非易失性和易失性存储器中的至少一种。非易失性存储器可包括只读存储器(Read-Only Memory,ROM)、磁带、软盘、闪存或光存储器等。易失性存储器可包括随机存取存储器(Random Access Memory,RAM)或外部高速缓冲存储器。作为说明而非局限,RAM可以是多种形式,比如静态随机存取存储器(Static Random Access Memory,SRAM)或动态随机存取存储器(Dynamic Random Access Memory,DRAM)等。
以上实施例的各技术特征可以进行任意的组合,为使描述简洁,未对上述实施例中的各个技术特征所有可能的组合都进行描述,然而,只要这些技术特征的组合不存在矛盾,都应当认为是本说明书记载的范围。
以上所述实施例仅表达了本申请的几种实施方式,其描述较为具体和详细,但并不能因此而理解为对发明专利范围的限制。应当指出的是,对于本领域的普通技术人员来说,在不脱离本申请构思的前提下,还可以做出若干变形和改进,这些都属于本申请的保护范围。因此,本申请专利的保护范围应以所附权利要求为准。
Claims (10)
1.一种惯性测量单元的安装角度标定方法,其特征在于,所述方法包括:
获取行驶时间段内每个信息采集时刻的导航定位信息、速度变化信息以及实际行驶参数;所述实际行驶参数包括实际行驶姿态以及实际行驶速度;
采用组合定位算法,根据所述导航定位信息、所述速度变化信息、所述实际行驶姿态以及所述实际行驶速度,得到估计姿态信息以及估计零偏;
根据所述导航定位信息、所述估计姿态以及所述估计零偏,对所述速度变化信息中加速度进行修正,得到每个信息采集时刻的修正后加速度;
根据所述修正后加速度,得到与每个信息采集时刻对应的预测行驶速度;
根据行驶终止时间的预测行驶速度,对与信息采集时刻对应的预测行驶速度进行修正,得到与信息采集时刻对应的修正后行驶速度;
根据所述修正后行驶速度,得到与所述行驶终止时间对应的行驶位移,根据所述行驶位移,得到惯性测量单元的安装角度。
2.根据权利要求1所述的方法,其特征在于,所述采用组合定位算法,根据所述导航定位信息、所述速度变化信息、所述实际行驶姿态以及所述实际行驶速度,得到估计姿态信息以及估计零偏包括:
根据行驶初始时刻的导航定位信息、速度变化信息、实际行驶姿态以及实际行驶速度,得到第一预测时刻的第一车辆状态参数,所述第一预测时刻为与所述行驶初始时刻对应的下一时刻;
根据所述第一预测时刻的导航定位信息以及第一车辆状态参数,得到与所述第一预测时刻对应的行驶参数误差、导航定位误差以及速度变化零偏;
根据所述与所述第一预测时刻对应的行驶参数误差、导航定位误差以及速度变化零偏,迭代计算每个信息采集时刻对应的车辆状态参数以及速度变化零偏,得到估计姿态信息以及估计零偏。
3.根据权利要求2所述的方法,其特征在于,
所述根据行驶初始时刻的导航定位信息、速度变化信息、实际行驶姿态以及实际行驶速度,得到第一预测时刻的第一车辆状态参数包括:
根据行驶初始时刻的实际行驶姿态以及速度变化信息中陀螺角速度,得到第一预测姿态信息;
根据所述第一预测姿态信息、所述行驶初始时刻的实际行驶速度以及速度变化信息中加速度计测量的比力,得到第一预测速度信息;
根据所述第一预测速度信息、所述行驶初始时刻的导航定位信息以及实际行驶速度,得到第一预测位置信息;
归集所述第一预测姿态信息、所述第一预测速度信息以及所述第一预测位置信息,得到第一预测时刻的第一车辆状态参数。
4.根据权利要求2所述的方法,其特征在于,所述根据所述与所述第一预测时刻对应的行驶参数误差、导航定位误差以及速度变化零偏,迭代计算每个信息采集时刻对应的车辆状态参数以及速度变化零偏,得到估计姿态信息以及估计零偏包括:
将所述第一预测时刻作为当前时刻;
根据所述当前时刻的导航定位信息、速度变化信息、实际行驶姿态以及实际行驶速度和与所述当前时刻对应的行驶参数误差、导航定位误差以及速度变化零偏,得到第二预测时刻的第二车辆状态参数,所述第二预测时刻为所述当前时刻的下一时刻;
根据所述第二预测时刻的导航定位信息以及第二车辆状态参数,得到所述第二预测时刻对应的行驶参数误差、导航定位误差以及速度变化零偏;
将所述第二预测时刻更新为当前时刻,返回所述根据所述当前时刻的导航定位信息、速度变化信息、实际行驶姿态以及实际行驶速度和与所述当前时刻对应的行驶参数误差、导航定位误差以及速度变化零偏,得到第二预测时刻的第二车辆状态参数的步骤;
直到更新后的第二预测时刻为所述行驶时间段内的行驶终止时刻,根据所述行驶终止时刻的第二车辆状态参数得到估计姿态信息,根据所述行驶终止时刻的速度变化零偏得到估计零偏。
5.根据权利要求4所述的方法,其特征在于,所述根据所述当前时刻的导航定位信息、速度变化信息、实际行驶姿态以及实际行驶速度和与所述当前时刻对应的行驶参数误差、导航定位误差以及速度变化零偏,得到第二预测时刻的第二车辆状态参数包括:
根据与所述当前时刻对应的速度变化零偏,修正所述当前时刻的速度变化信息,得到修正后速度变化信息;
根据所述当前时刻的导航定位信息、实际行驶姿态、实际行驶速度以及修正后速度变化信息,得到第二预测时刻的待调整车辆状态参数;
根据与所述当前时刻对应的行驶参数误差以及导航定位误差对所述待调整车辆状态参数进行修正,得到第二预测时刻的第二车辆状态参数。
6.根据权利要求1所述的方法,其特征在于,所述根据所述导航定位信息、所述根据所述导航定位信息、所述估计姿态以及所述估计零偏,对所述速度变化信息中加速度进行修正,得到每个信息采集时刻的修正后加速度包括:
根据所述估计零偏中加速度计零偏,对所述速度变化信息中加速度进行修正,得到一次修正加速度,并确定与所述导航定位信息对应的重力加速度;
根据所述估计姿态以及所述重力加速度,确定待修正加速度;
根据所述待修正加速度对所述一次修正加速度进行修正,得到修正后加速度。
7.根据权利要求1所述的方法,其特征在于,所述根据行驶终止时间的预测行驶速度,对与信息采集时刻对应的预测行驶速度进行修正,得到与信息采集时刻对应的修正后行驶速度包括:
根据行驶终止时间的预测行驶速度以及所述行驶时间段,确定与信息采集时刻对应的补偿速度;
将所述补偿速度叠加至与信息采集时刻对应的预测行驶速度,得到与信息采集时刻对应的修正后行驶速度。
8.一种惯性测量单元的安装角度标定装置,其特征在于,所述装置包括:
获取模块,用于获取行驶时间段内每个信息采集时刻的导航定位信息、速度变化信息以及实际行驶参数;所述实际行驶参数包括实际行驶姿态以及实际行驶速度;
估计模块,用于采用组合定位算法,根据所述导航定位信息、所述速度变化信息、所述实际行驶姿态以及所述实际行驶速度,得到估计姿态信息以及估计零偏;
第一修正模块,用于根据所述导航定位信息、所述估计姿态以及所述估计零偏,对所述速度变化信息中加速度进行修正,得到每个信息采集时刻的修正后加速度;
预测模块,用于根据所述修正后加速度,得到与每个信息采集时刻对应的预测行驶速度;
第二修正模块,用于根据行驶终止时间的预测行驶速度,对与信息采集时刻对应的预测行驶速度进行修正,得到与信息采集时刻对应的修正后行驶速度;
处理模块,用于根据所述修正后行驶速度,得到与所述行驶终止时间对应的行驶位移,根据所述行驶位移,得到惯性测量单元的安装角度。
9.一种计算机设备,包括存储器和处理器,所述存储器存储有计算机程序,其特征在于,所述处理器执行所述计算机程序时实现权利要求1至7中任一项所述的方法的步骤。
10.一种计算机可读存储介质,其上存储有计算机程序,其特征在于,所述计算机程序被处理器执行时实现权利要求1至7中任一项所述的方法的步骤。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110864781.9A CN113566849B (zh) | 2021-07-29 | 2021-07-29 | 惯性测量单元的安装角度标定方法、装置和计算机设备 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110864781.9A CN113566849B (zh) | 2021-07-29 | 2021-07-29 | 惯性测量单元的安装角度标定方法、装置和计算机设备 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN113566849A CN113566849A (zh) | 2021-10-29 |
CN113566849B true CN113566849B (zh) | 2024-03-05 |
Family
ID=78169105
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202110864781.9A Active CN113566849B (zh) | 2021-07-29 | 2021-07-29 | 惯性测量单元的安装角度标定方法、装置和计算机设备 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113566849B (zh) |
Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102865881A (zh) * | 2012-03-06 | 2013-01-09 | 武汉大学 | 一种惯性测量单元的快速标定方法 |
CN111238532A (zh) * | 2019-12-23 | 2020-06-05 | 湖北航天技术研究院总体设计所 | 一种适用于晃动基座环境的惯性测量单元标定方法 |
WO2020220729A1 (zh) * | 2019-04-29 | 2020-11-05 | 南京航空航天大学 | 基于角加速度计/陀螺/加速度计的惯性导航解算方法 |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US10527424B2 (en) * | 2013-05-14 | 2020-01-07 | National Institute Of Advanced Industrial Science And Technology | Estimated-azimuth-angle assessment device, mobile terminal device, computer-readable storage medium, control method for estimated-azimuth-angle assessment device, and positioning device |
-
2021
- 2021-07-29 CN CN202110864781.9A patent/CN113566849B/zh active Active
Patent Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102865881A (zh) * | 2012-03-06 | 2013-01-09 | 武汉大学 | 一种惯性测量单元的快速标定方法 |
WO2020220729A1 (zh) * | 2019-04-29 | 2020-11-05 | 南京航空航天大学 | 基于角加速度计/陀螺/加速度计的惯性导航解算方法 |
CN111238532A (zh) * | 2019-12-23 | 2020-06-05 | 湖北航天技术研究院总体设计所 | 一种适用于晃动基座环境的惯性测量单元标定方法 |
Non-Patent Citations (1)
Title |
---|
微型惯性测量组合标定技术;任大海, 顾启泰, 毛刚, 尤政, 刘学斌;清华大学学报(自然科学版)(第08期);全文 * |
Also Published As
Publication number | Publication date |
---|---|
CN113566849A (zh) | 2021-10-29 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN113465628B (zh) | 惯性测量单元数据补偿方法及系统 | |
US7979231B2 (en) | Method and system for estimation of inertial sensor errors in remote inertial measurement unit | |
JP6094026B2 (ja) | 姿勢判定方法、位置算出方法及び姿勢判定装置 | |
CN111156994B (zh) | 一种基于mems惯性组件的ins/dr&gnss松组合导航方法 | |
CN101949703B (zh) | 一种捷联惯性/卫星组合导航滤波方法 | |
US20070050138A1 (en) | Enhanced inertial system performance | |
CA3003298A1 (en) | Gnss and inertial navigation system utilizing relative yaw as an observable for an ins filter | |
CN110715659A (zh) | 零速检测方法、行人惯性导航方法、装置及存储介质 | |
US20170074678A1 (en) | Positioning and orientation data analysis system and method thereof | |
CN113566850B (zh) | 惯性测量单元的安装角度标定方法、装置和计算机设备 | |
CN113252048B (zh) | 一种导航定位方法、导航定位系统及计算机可读存储介质 | |
CN114545472B (zh) | 一种gnss/ins组合系统的导航方法和装置 | |
CN114526731A (zh) | 一种基于助力车的惯性组合导航方向定位方法 | |
CN116007620A (zh) | 一种组合导航滤波方法、系统、电子设备及存储介质 | |
JP2014240266A (ja) | センサドリフト量推定装置及びプログラム | |
CN116222551A (zh) | 一种融合多种数据的水下导航方法及装置 | |
CN114061570A (zh) | 车辆定位方法、装置、计算机设备和存储介质 | |
US20220057517A1 (en) | Method for constructing point cloud map, computer device, and storage medium | |
US8566055B1 (en) | Gyro indexing compensation method and system | |
CN114323007A (zh) | 一种载体运动状态估计方法及装置 | |
CN111912427A (zh) | 一种多普勒雷达辅助捷联惯导运动基座对准方法及系统 | |
CN114019954B (zh) | 航向安装角标定方法、装置、计算机设备和存储介质 | |
CN113566849B (zh) | 惯性测量单元的安装角度标定方法、装置和计算机设备 | |
CN114001730B (zh) | 融合定位方法、装置、计算机设备和存储介质 | |
CN115112119A (zh) | 一种基于lstm神经网络辅助的车载导航方法 |
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 |