CN114413934B - 一种车辆定位系统校正方法和装置 - Google Patents
一种车辆定位系统校正方法和装置 Download PDFInfo
- Publication number
- CN114413934B CN114413934B CN202210066361.0A CN202210066361A CN114413934B CN 114413934 B CN114413934 B CN 114413934B CN 202210066361 A CN202210066361 A CN 202210066361A CN 114413934 B CN114413934 B CN 114413934B
- Authority
- CN
- China
- Prior art keywords
- error
- matrix
- information
- inertial navigation
- navigation system
- 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 69
- 238000012937 correction Methods 0.000 title claims abstract description 39
- 239000011159 matrix material Substances 0.000 claims abstract description 197
- 239000013598 vector Substances 0.000 claims abstract description 178
- 238000005259 measurement Methods 0.000 claims abstract description 67
- 238000001914 filtration Methods 0.000 claims abstract description 64
- 230000007704 transition Effects 0.000 claims abstract description 47
- 238000009434 installation Methods 0.000 claims abstract description 21
- 230000001133 acceleration Effects 0.000 claims description 31
- 230000009471 action Effects 0.000 claims description 18
- 238000004364 calculation method Methods 0.000 claims description 15
- 230000008569 process Effects 0.000 claims description 15
- 238000010276 construction Methods 0.000 claims description 8
- 230000000875 corresponding effect Effects 0.000 claims description 7
- 238000006073 displacement reaction Methods 0.000 claims description 6
- 230000002596 correlated effect Effects 0.000 claims description 4
- 230000001131 transforming effect Effects 0.000 claims description 3
- 238000004519 manufacturing process Methods 0.000 abstract description 5
- 238000003860 storage Methods 0.000 description 4
- 238000004422 calculation algorithm Methods 0.000 description 3
- 230000008878 coupling Effects 0.000 description 3
- 238000010168 coupling process Methods 0.000 description 3
- 238000005859 coupling reaction Methods 0.000 description 3
- 230000004927 fusion Effects 0.000 description 3
- 230000009286 beneficial effect Effects 0.000 description 2
- 230000008901 benefit Effects 0.000 description 2
- 238000004891 communication Methods 0.000 description 2
- 238000013461 design Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 238000004590 computer program Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000005284 excitation Effects 0.000 description 1
- 239000012467 final product Substances 0.000 description 1
- 230000006870 function Effects 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 230000007774 longterm Effects 0.000 description 1
- 238000000691 measurement method Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 230000000750 progressive effect Effects 0.000 description 1
- 230000009897 systematic effect Effects 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
- 238000012546 transfer Methods 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
-
- 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
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
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
技术领域
本发明涉及智能驾驶技术领域，具体涉及一种车辆定位系统校正方法和装置。
背景技术
受制于成本的限制，一般民用车辆上所使用的惯性测量器件(加速度计、陀螺仪等)、轮速计等传感器的精度较低，其误差会随着时间、环境温度的变化发生漂移。在安装到车辆上后，受装配误差、车辆载重变化、轮胎气压变化等因素的影响，传感器的相关参数也常常会发生变化进而影响测量的精度。
目前为对车辆上传感器所产生的误差进行校正，通常采用专门的标定操作，如对惯性器件进行六面翻转或转台激励、让车辆在专门设定的标定场地内按照规定路线行驶等。这种专门的标定操作难以保证长期有效，需要定期对车辆进行操作，不便于依赖于传感器的车载定位系统进行大规模生产装配。
发明内容
为了解决现有技术存在的标定操作复杂、不便于大规模的生产装配的问题，本发明提供了一种车辆定位系统校正方法、装置、设备和存储介质，其具有定位系统校正更加便捷、适于进行大规模生产装配等特点
根据本发明具体实施方式提供的一种车辆定位系统校正方法，包括：
基于传感器标定参数需求构建卡尔曼滤波的状态向量，所述状态向量包括：传感器自身的误差参数、惯导系统得到的车辆状态信息的误差和航位推算系统得到的车辆状态信息的误差，所述惯导系统得到的车辆状态信息的误差和航位推算系统得到的车辆状态信息的误差均由所述传感器输出的测量数据参与计算得到；
基于所述状态向量构建卡尔曼滤波的状态转移矩阵，所述状态转移矩阵由惯导系统相关联的矩阵和航位推算系统相关联的矩阵组合构成，所述惯导系统相关联的矩阵为：使所述惯导系统得到的车辆状态信息的误差和所述传感器自身的误差参数相关联的矩阵，所述航位推算系统相关联的矩阵为：使所述航位推算系统得到的车辆状态信息的误差和所述传感器自身的误差参数相关联的矩阵；
基于所述状态转移矩阵和上一时刻的状态向量进行卡尔曼滤波预测，以得到当前时刻的第一状态向量和第一协方差矩阵；
基于当前时刻惯导系统得到的车辆状态信息、航位推算系统得到的车辆状态信息以及车载实时差分定位系统的车辆状态信息构建卡尔曼滤波测量向量和测量方程；
基于所述第一状态向量、所述第一协方差矩阵、所述测量向量和所述测量方程依次进行卡尔曼滤波的滤波增益和状态估计，以得到当前时刻的第二状态向量；
基于所述第二状态向量对所述第一状态向量中的当前时刻的传感器自身的误差参数、惯导系统得到的车辆状态信息的误差和航位推算系统得到的车辆状态信息的误差进行校正。
进一步地，所述车辆定位系统校正方法还包括：在校正完成后将所述上一时刻的状态向量和所述当前时刻的第二状态向量置零，以进行下一时刻的校正。
进一步地，所述基于传感器标定参数需求构建卡尔曼滤波的状态向量，包括：
分别获取车辆上传感器、惯导系统和航位推算系统得到的车辆状态信息；
所述传感器得到的车辆状态信息包括：角速度信息和加速度信息：
所述惯导系统得到的车辆状态信息包括：第一速度信息、第一位置信息和姿态信息；
所述航位推算系统得到的车辆状态信息包括：第二速度信息和第二位置信息。
进一步地，所述基于传感器标定参数需求构建卡尔曼滤波的状态向量，还包括：
基于所述传感器得到的车辆状态信息、所述惯导系统得到的车辆状态信息和所述航位推算系统得到的车辆状态信息，对应得到所述传感器自身的误差参数、所述惯导系统得到的车辆状态信息的误差和所述航位推算系统得到的车辆状态信息的误差；
所述传感器自身的误差参数包括：陀螺仪和加速度计的零偏值、安装误差角度和比例系数误差，以及轮速计的比例系数误差；
所述惯导系统得到的车辆状态信息的误差包括：所述姿态信息的误差、所述第一速度信息的误差和所述第一位置信息的误差；
所述航位推算系统得到的车辆状态信息的误差包括：所述第二速度信息的误差和所述第二位置信息的误差。
进一步地，所述基于传感器标定参数需求构建卡尔曼滤波的状态向量，还包括：
基于所述车辆状态信息构建卡尔曼滤波的27维状态向量：
Xk＝[ΦδvinsδpinsδpDRbgbaλDRδKDRμDRδKgδKa]T
其中，Φ为惯导系统计算的姿态信息θ、φ、ψ的误差；δvins为惯导系统计算得到的第一速度信息vins的误差；δpins为惯导系统计算得到的第一位置信息pins的误差；δpDR为航位推算系统由轮速计推算得到的第二位置信息pDR的误差；bg、ba分别为陀螺仪、加速度计的零偏值；λDR、μDR分别为陀螺仪和加速度计与车体之间俯仰和航向方向的安装误差角；6KDR为轮速计的比例系数误差；εKg和εKa分别为陀螺仪和加速度计的比例系数误差。
进一步地，所述基于所述状态向量构建卡尔曼滤波的状态转移矩阵，包括：
基于所述陀螺仪的零偏值和比例系数误差对所述姿态信息的误差的作用关系，得到姿态矩阵和角速度矩阵，关系式如下：
式中，Φk为当前时刻的姿态信息的误差，Φk-1为上一时刻的姿态信息的误差，为车辆姿态矩阵，是由惯导系统输出的姿态信息θ、φ、ψ计算得到；Ω为角速度矢量ω构建的3×3维对角矩阵；
基于所述加速度计零偏值和比例系数误差对所述第一速度信息的误差的作用关系，得到加速度向量在所述惯导系统下的反对称矩阵和加速度矢量矩阵，关系式如下：
式中，为当前时刻第一速度信息的误差，/>为上一时刻第一速度信息的误差，(fn×)为将惯导系统变换至导航系下的加速度向量所构建的反对称矩阵；G为加速度矢量f构建的3×3维对角矩阵，其中惯导系统第一位置信息的误差受惯导系统的第一速度信息的误差的影响，关系式如下：
为当前时刻第一位置信息的误差，/>为上一时刻第一位置信息的误差，εvins为第一速度信息的误差；
基于所述第二速度信息的误差、所述安装误差角都误差和所述轮速计的比例系数误差对所述第二位置信息的误差的作用关系，得到所述第二速度信息的反对称矩阵以及所述姿态信息和所述第二速度信息的关联矩阵，关系式如下：
其中为当前时刻第二速度信息的误差，/>为上一时刻第二速度信息的误差，(vDR×)为轮速计计算得到车辆的第二速度信息vDR构建的反对称矩阵；Mcv为由姿态信息及第二速度信息构建的矩阵：
其中vD为轮速计得到的第二车速标量信息，C11～C33为矩阵中的各个元素。
进一步地，所述基于所述状态向量构建卡尔曼滤波的状态转移矩阵，还包括：
对所述姿态矩阵、所述角速度矩阵、所述加速度向量在所述惯导系统下的反对称矩阵、所述加速度矢量矩阵、所述第二速度信息的反对称矩阵以及所述姿态信息和所述第二速度信息的关联矩阵进行组合得到所述状态转移矩阵，如下：
A＝I+FΔT
其中
式中，0为3×3维的零矩阵，I为3维的单位矩阵，A为状态转移矩阵，ΔT为当前时刻和上一时刻的间隔时间。
进一步地，所述基于当前时刻惯导系统得到的车辆状态信息、航位推算系统得到的车辆状态信息以及车载实时差分定位系统的车辆状态信息构建卡尔曼滤波测量向量和测量方程，包括：
基于所述第一位置信息、所述姿态信息中的航向、所述第二位置信息和所述车载实时差分定位系统中的位移信息及航向构建所述测量向量和所述测量方程，如下：
式中，H1＝[0 0 1]，H2＝[C31 0 C33]，pins为惯导系统得到的第一位置信息，ψ为相应的航向信息；pDR为航位推算系统推算得到的第二位置信息；pgps为车载实时差分定位系统得到的位移，ψgps为车载实时差分定位系统的航向；
进行卡尔曼滤波的滤波增益计算、状态估计和协方差估计，得到：
K＝Pk，k-1HT(HPk，k-1HT+R)-1
Xk＝Xk-1+K(Z-HXk，k-1)
Pk＝(I-KH)Pk，k-1
式中，R为车载实时差分定位系统测量噪声方差构成的对角矩阵；Xk为卡尔曼滤波器估计得到的当前时刻的状态向量；Pk为卡尔曼滤波器当前时刻的协方差矩阵。
进一步地，所述基于所述第二状态向量对所述第一状态向量中的当前时刻的传感器自身的误差参数、惯导系统得到的车辆状态信息的误差和航位推算系统得到的车辆状态信息的误差进行校正，包括：
基于得到的当前时刻的状态向量Xk，得到各个误差为：
Φ＝Xk (1～3)
δvins＝Xk (4～6)
δpins＝Xk (7～9)
δpDR＝Xk (10～12)
bg＝Xk (13～15)
ba＝Xk (16～18)
λDR＝Xk (19)
δKDR＝Xk (20)
μDR＝Xk (21)
δKg＝Xk (22～24)
δKa＝Xk (25～27)
其中，括号中的数字代表相应误差值在状态向量中所占的列数位置，Φ为惯导系统计算的姿态信息θ、φ、ψ的误差；δvins为惯导系统计算的第一速度信息vins的误差；δpins为惯导系统计算的第一位置信息pins的误差；δpDR为航位推算系统的轮速计推算得到的第二位置信息pDR的误差；bg、ba分别为陀螺仪、加速度计的零偏值；λDR、μDR分别为陀螺仪及加速度计与车体之间俯仰和航向方向的安装误差角；δKDR为轮速计的比例系数误差；δKg和δKa分别为陀螺仪和加速度计的比例系数误差；
基于得到的各个误差值，对惯导系统和航位推算系统的误差参数进行校正，如下：
式中，为校正后的惯导系统的姿态矩阵，用于下一步的惯导系统的更新；/>为校正后的车体姿态矩阵，用于下一步的航位推算系统的航位推算；(Φ×)为惯导系统的姿态误差角Φ构成的反对称矩阵；(α×)为陀螺仪和加速度计与车体之间安装误差构成的反对称矩阵，α＝[λDR 0 μDR]T；/>为校正后惯导系统的第一速度信息，/>为校正后惯导系统的第一位置信息、/>为航位推算系统测量的速度标量、/>分别为校正后的陀螺仪和加速度计测量的角速度和加速度信息。
根据本发明具体实施方式提供的，一种车辆定位系统校正装置，包括：
状态向量模块，用于基于传感器标定参数需求构建卡尔曼滤波的状态向量，所述状态向量包括：传感器自身的误差参数、惯导系统得到的车辆状态信息的误差和航位推算系统得到的车辆状态信息的误差，所述惯导系统得到的车辆状态信息的误差和航位推算系统得到的车辆状态信息的误差均由所述传感器输出的测量数据参与计算得到；
状态转移矩阵模块，用于基于所述状态向量构建卡尔曼滤波的状态转移矩阵，所述状态转移矩阵由惯导系统相关联的矩阵和航位推算系统相关联的矩阵组合构成，所述惯导系统相关联的矩阵为：使所述惯导系统得到的车辆状态信息的误差和所述传感器自身的误差参数相关联的矩阵，所述航位推算系统相关联的矩阵为：使所述航位推算系统得到的车辆状态信息的误差和所述传感器自身的误差参数相关联的矩阵；
预测模块，用于基于所述状态转移矩阵和上一时刻的状态向量进行卡尔曼滤波预测，以得到当前时刻的第一状态向量和第一协方差矩阵；
测量构建模块，用于基于当前时刻惯导系统得到的车辆状态信息、航位推算系统得到的车辆状态信息以及车载实时差分定位系统的车辆状态信息构建卡尔曼滤波测量向量和测量方程；
误差确定模块，用于基于所述第一状态向量、所述第一协方差矩阵、所述测量向量和所述测量方程依次进行卡尔曼滤波的滤波增益和状态估计，以得到当前时刻的第二状态向量；以及
误差校正模块，用于基于所述第二状态向量对所述第一状态向量中的当前时刻的传感器自身的误差参数、惯导系统得到的车辆状态信息的误差和航位推算系统得到的车辆状态信息的误差进行校正。
本发明实施例提供的车辆定位系统校正方法，首先基于传感器标定参数需求构建卡尔曼滤波的状态向量，得到定位系统中存在各个误差参数。再基于状态向量构建卡尔曼滤波的状态转移矩阵，进而确定了上一时刻与当前时刻各个误差参数所存在的关联关系。然后基于状态转移矩阵和上一时刻的状态向量进行卡尔曼滤波预测，以得到当前时刻的第一状态向量和第一协方差矩阵，实现对当前误差的预测。在基于当前时刻惯导系统得到的车辆状态信息、航位推算系统得到的车辆状态信息以及车载实时差分定位系统的车辆状态信息构建卡尔曼滤波测量向量和测量方程，基于第一状态向量、第一协方差矩阵、测量向量和测量方程依次进行卡尔曼滤波的滤波增益和状态估计，以得到当前时刻的第二状态向量，得到了更加精准可靠的误差参数。最后基于第二状态向量对第一状态向量中的当前时刻的传感器自身的误差参数、惯导系统得到的车辆状态信息的误差和航位推算系统得到的车辆状态信息的误差进行校正。通过以上步骤实现了基于卡尔曼滤波对测量定位系统的误差校正，能够得到精确的车辆位置及姿态信息，相比于传统的专门进行的标定操作，可简化定位系统系统以及和定位系统相关系统的安装、使用步骤和条件，更加有利于低成本定位系统的大规模量产和装配。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据提供的附图获得其他的附图。
图1是根据一示例性实施例提供的车辆定位系统校正方法的流程图；
图2是根据一示例性实施例提供的车辆定位系统校正装置的结构图；
图3是根据一示例性实施例提供的设备的结构图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
参照图1所示，本发明的实施例提供了一种车辆定位系统校正方法，该方法可以包括以下步骤：
101、基于传感器标定参数需求构建卡尔曼滤波的状态向量，状态向量包括：传感器自身的误差参数、惯导系统得到的车辆状态信息的误差和航位推算系统得到的车辆状态信息的误差，惯导系统得到的车辆状态信息的误差和航位推算系统得到的车辆状态信息的误差均由传感器输出的测量数据参与计算得到，其中传感器输出的测量数据为受传感器自身的误差参数影响的测量数据。
卡尔曼滤波本质上是一个数据融合算法，其能够将具有同样测量目的、来自不同传感器、具有不同单位的数据融合在一起，得到一个更精确的目的测量值。其适用于拟合线性高斯系统，其最大的优点在于计算量小，能够利用前一时刻的状态(和可能的测量值)来得到当前时刻下的状态的最优估计。经常被应用于导航系统和定位系统的精确定位。在本发明的实施例中将其应用于传感器以及传感器相关系统误差的估计。
一般车辆上的定位系统大都采用由惯性器件构成的惯导系统和主要由轮速计构成的航位推算系统，惯导系统和航位推算系统所输出的各种车辆的状态信息如速度、加速度、位置、航向等信息都是经对相关传感器输出的数据计算得到的，因为传感器自身会因为外界环境的影响的产生误差，其输出的数据自然也会存在误差。正是基于这种影响关系，惯导系统和航位推算系统输出所产生的误差其根本在于传感器误差的存在，首先确定需要标定的传感器的自身误差参数以及该传感器输出的数据参与到惯导系统以及航位推算系统中得到的相关的误差，构成车辆的状态向量，通过卡尔曼滤可基于对上一时刻的状态向量的估计得到当前时刻的状态向量。
102、基于状态向量构建卡尔曼滤波的状态转移矩阵，状态转移矩阵由惯导系统相关联的矩阵和航位推算系统相关联的矩阵组合构成，惯导系统相关联的矩阵为：使惯导系统得到的车辆状态信息的误差和传感器自身的误差参数相关联的矩阵，航位推算系统相关联的矩阵为：使航位推算系统得到的车辆状态信息的误差和传感器自身的误差参数相关联的矩阵。
因为参与惯导系统和航位推算系统的传感器较多且受影响的车辆状态信息的误差参数也较多，那么需要确定和传感器的误差参数存在固定关系的矩阵，来构成状态转移矩阵，状态转移矩阵能够表征传感器误差参数和惯导系统误差以及航位推算系统之间的关系，在进行卡尔曼滤波状态预测试则能够将所有的状态向量转换为相应的传感器的误差进行表征，进而转换为了对传感器误差参数的预测。
103、基于状态转移矩阵和上一时刻的状态向量进行卡尔曼滤波预测，以得到当前时刻的第一状态向量和第一协方差矩阵。
根据卡尔曼滤波的特性，在得到状态转移矩阵后可对上一时刻得到的状态向量进行预测，在进行卡尔曼滤波状态预测计算和协方差更新计算后，可得到当前时刻的状态向量和协方差矩阵，其中协方差矩阵可表征预测所产生误差的程度。
104、基于当前时刻惯导系统得到的车辆状态信息、航位推算系统得到的车辆状态信息以及车载实时差分定位系统的车辆状态信息构建卡尔曼滤波测量向量和测量方程。
基于卡尔曼滤波的特性，对于车辆的状态信息的预测需要一个更为精准可靠的预测方式对惯导系统和航位推算系统进行融合调优。车载实时差分定位系统时基于RTK(载波相位差分技术)实时处理两个测量站载波相位观测量的差分方法，将基准站采集的载波相位发给用户接收机，进行求差解算坐标。是一种新的常用的卫星定位测量方法，适用于高精度的GPS测量。从而可基于车载实时差分定位系统获得高精度航向信息和定位结果，然后结合惯导系统得到的车辆状态信息、航位推算系统得到的车辆状态信息即可构建出卡尔曼滤波的测量向量和测量方程。
105、基于第一状态向量、第一协方差矩阵、测量向量和测量方程依次进行卡尔曼滤波的滤波增益和状态估计，以得到当前时刻的第二状态向量。
在得到测量向量和测量方程后就可结合卡尔曼滤波预测得到的第一状态向量和第一协方差矩阵，进行融合调优的计算，从而得到比基于状态转移矩阵预测得到的误差的状态向量更加精准的状态向量。
106、基于第二状态向量对第一状态向量中的当前时刻的传感器自身的误差参数、惯导系统得到的车辆状态信息的误差和航位推算系统得到的车辆状态信息的误差进行校正。
在得到经卡尔曼滤波融合调优后的状态向量后，可根据状态向量中所包含的误差信息对应对惯导系统和航位推算系统中误差和传感器自身的误差参数进行调整。根据卡尔曼滤波的特点只需要作为参考的车载实时差分定位系统能够正常的工作，输出高精度的车辆信息，那么就能够在车辆行驶过程中对各个惯导系统和航位推算系统的进行误差校正，并且还能够得到更加精确的车辆位置和姿态信息，相比于传统的专用标定操作，本发明能够简化惯性导航、航位推算系统的安装、使用步骤和条件，更加有利于低成本车载导航定位系统的大规模量产和装配。
在本发明的另一具体实施例中，在误差校正完成后将上一时刻的状态向量和当前时刻的第二状态向量置零，以进行下一时刻的校正。因为是采用卡尔曼滤波闭环校正的方式进行的校正，需将各个状态向量和相关的标定修正清零后进行下一轮参数校正。
作为上述实施例可行的实现方式，基于传感器标定参数需求构建卡尔曼滤波的状态向量的具体过程可包括：
分别获取车辆上传感器、惯导系统和航位推算系统得到的车辆状态信息；传感器得到的车辆状态信息包括：角速度信息和加速度信息：
惯导系统得到的车辆状态信息包括：第一速度信息、第一位置信息和姿态信息；
航位推算系统得到的车辆状态信息包括：第二速度信息和第二位置信息。
然后，基于传感器得到的车辆状态信息、惯导系统得到的车辆状态信息和航位推算系统得到的车辆状态信息，对应得到传感器自身的误差参数、惯导系统得到的车辆状态信息的误差和航位推算系统得到的车辆状态信息的误差；
其中，传感器自身的误差参数包括：陀螺仪和加速度计的零偏值、安装误差角度和比例系数误差，以及轮速计的比例系数误差；
惯导系统得到的车辆状态信息的误差包括：姿态信息的误差、第一速度信息的误差和第一位置信息的误差；
航位推算系统得到的车辆状态信息的误差包括：第二速度信息的误差和第二位置信息的误差。
具体的，在具体实施时利用车载实时差分定位系统获得车辆高精度航向信息ψgps和定位信息pgps；惯性器件陀螺仪和加速度计分别输出车辆角速度信息、加速度信息ω、f；惯导系统由惯性器件输出信息计算得到车辆在导航系下的第一速度信息vins、第一位置信息pins和姿态信息θ、φ、ψ，其中θ、φ、ψ分别表示陀螺仪和加速度计计算得到的车辆俯仰、滚转和航向角；航位推算系统通过轮速计计算得到车辆的第二速度信息vDR和第二位置信息pDR。
然后可构建卡尔曼滤波的27维状态向量：
Xk＝[ΦδvinsδpinsδpDRbgbaλDRδKDRμDRδKgδKa]T
其中，Φ为惯导系统计算的姿态信息θ、φ、ψ的误差；δvins为惯导系统计算得到的第一速度信息vins的误差；δpins为惯导系统计算得到的第一位置信息pins的误差；δpDR为航位推算系统由里程计推算得到的第二位置信息pDR的误差；bg、ba分别为陀螺仪、加速度计的零偏值；λDR、μDR分别为陀螺仪和加速度计与车体之间俯仰和航向方向的安装误差角；δKDR为轮速计的比例系数误差；δKg和δKa分别为陀螺仪和加速度计的比例系数误差。
需要说明的是，上述状态向量的维数可根据定位系统中相关误差的数量进行调整，在此不做限制。
在完成状态向量的构建后则可基于状态向量构建卡尔曼滤波的状态转移矩阵，具体过程包括：
基于陀螺仪的零偏值和比例系数误差对姿态信息的误差的作用关系，得到姿态矩阵和角速度矩阵；
基于加速度计零偏值和比例系数误差对第一速度信息的误差的作用关系，得到加速度向量在惯导系统下的反对称矩阵和加速度矢量矩阵；
基于第二速度信息的误差、安装误差角都误差和轮速计的比例系数误差对第二位置信息的误差的作用关系，得到第二速度信息的反对称矩阵以及姿态信息和第二速度信息的关联矩阵。
然后对姿态矩阵、角速度矩阵、加速度向量在惯导系统下的反对称矩阵、加速度矢量矩阵、第二速度信息的反对称矩阵以及姿态信息和第二速度信息的关联矩阵进行组合得到状态转移矩阵。
具体的，在卡尔曼滤波中姿态误差主要受陀螺仪的零偏和比例系数影响，其关系式如下：
式中，为车辆姿态矩阵，是由惯导系统输出的姿态信息θ、φ、ψ计算得到；Ω为角速度矢量ω构建的3×3维对角矩阵；
惯导系统得到的第一速度信息的误差，主要受加速度计零偏和比例系数影响，关系式如下：
式中，(fn×)为将惯导系统变换至导航系下的加速度向量所构建的反对称矩阵；G为加速度矢量f构建的3×3维对角矩阵；
惯导系统得到的位置误差，主要受惯导系统的第一速度信息的误差的影响，关系式如下：
航位推算系统推算得到的第二位置信息的误差，主要受轮速计计算的第二速度信息的误差、安装角误差以及轮速计比例系数的影响，关系式如下：
其中(vDR×)为轮速计计算得到车辆的第二速度信息vDR构建的反对称矩阵；Mcv为由车辆姿态信息及第二速度信息构建的矩阵：
其中vD为车辆轮速计得到的第二车速标量信息，C11～C33为矩阵中的各个元素。
进行整合后得到离散化的Kalman滤波的状态转移矩阵为：
A＝I+FΔT
其中
式中，0为3×3维的零矩阵；I为3维的单位矩阵。
基于上述得到的状态转移矩阵进行卡尔曼滤波状态预测计算和协方差更新计算得到：
Xk，k-1＝AXk-1
Pk，k-1＝APk-1AT+Q
式中，Xk-1为上一时刻的Kalman滤波状态向量，Pk-1为上一时刻的Kalman滤波协方差矩阵，Q为传感器噪声方差构成的对角矩阵。
在状态转移矩阵构建完成后，进行卡尔曼滤波测量向量和测量方程的构建可具体包括：
基于第一位置信息、姿态信息中的航向、第二位置信息和车载实时差分定位系统中的位移信息及航向构建测量向量和测量方程。得到的测量向量和测量方程如下所示：
式中，H1＝[0 0 1]，H2＝[C31 0 C33]，pins为惯导系统得到的第一位置信息，ψ为相应的航向信息；pDR为航位推算系统推算得到的第二位置信息；pgps为车载实时差分定位系统得到的位移，ψgps为车载实时差分定位系统的航向。
然后进行卡尔曼滤波的滤波增益计算、状态估计和协方差估计：
K＝Pk，k-1HT(HPk，k-1HT+R)-1
Xk＝Xk-1+K(Z-HXk，k-1)
Pk＝(I-KH)Pk，k-1
式中，R为车载实时差分定位系统测量噪声方差构成的对角矩阵；Xk为卡尔曼滤波器估计得到的当前时刻的状态向量，即各传感器和系统的的误差；Pk为卡尔曼滤波器当前时刻的协方差矩阵。
根据得到的当前时刻的状态向量Xk，得到各个误差为：
Φ＝Xk (1～3)
δvins＝Xk (4～6)
δpins＝Xk (7～9)
δpDR＝Xk (10～12)
bg＝Xk (13～15)
ba＝Xk (16～18)
λDR＝Xk (19)
δKDR＝Xk (20)
μDR＝Xk (21)
δKg＝Xk (22～24)
δKa＝Xk (25～27)
其中括号中的数字代表相应误差值在状态向量中所占的列数位置，Φ为惯导系统计算的姿态信息θ、φ、ψ的误差；δvins为惯导系统计算的第一速度信息vins的误差；δpins为惯导系统计算的第一位置信息pins的误差；δpDR为航位推算系统的轮速计推算得到的第二位置信息pDR的误差；bg、ba分别为陀螺仪、加速度计的零偏值；λDR、μDR分别为陀螺仪及加速度计与车体之间俯仰和航向方向的安装误差角；δKDR为轮速计的比例系数误差；δKg和δKa分别为陀螺仪和加速度计的比例系数误差。
基于上述得到的各个误差值，即可对惯导系统和航位推算系统的误差参数进行校正，具体校正过程如下：
式中，为校正后的惯导系统的姿态矩阵，可用于下一步的惯导系统的更新；/>为校正后的车体姿态矩阵，可用于下一步的航位推算系统的航位推算；(Φ×)为惯导系统的姿态误差角Φ构成的反对称矩阵；(α×)为陀螺仪和加速度计与车体之间安装误差构成的反对称矩阵，α＝[λDR 0 μDR]T；/>为校正后惯导系统的第一速度信息，/>为校正后惯导系统的第一位置信息、/>为航位推算系统测量的速度标量、/>分别为校正后的陀螺仪和加速度计测量的角速度和加速度信息，可用于下一步的惯导更新。
在将惯导系统和航位推算系统中的传感器参数进行修正后，需要将上一时刻卡尔曼滤波得到的状态向量和当前时刻的状态向量清零，完成卡尔曼滤波的闭环校正，然后再进行下一步的滤波计算。
基于同样的设计思路，参照图2所示，本发明的实施例还提供了一种车辆定位系统校正装置，用于执行上述实施例所述的车辆定位系统校正方法的各个步骤，该装置可以包括：
状态向量模块201，用于基于传感器标定参数需求构建卡尔曼滤波的状态向量，状态向量包括：传感器自身的误差参数、惯导系统得到的车辆状态信息的误差和航位推算系统得到的车辆状态信息的误差，惯导系统得到的车辆状态信息的误差和航位推算系统得到的车辆状态信息的误差均由传感器输出的测量数据参与计算得到。
状态转移矩阵模块202，用于基于状态向量构建卡尔曼滤波的状态转移矩阵，状态转移矩阵由惯导系统相关联的矩阵和航位推算系统相关联的矩阵组合构成，惯导系统相关联的矩阵为：使惯导系统得到的车辆状态信息的误差和传感器自身的误差参数相关联的矩阵，航位推算系统相关联的矩阵为：使航位推算系统得到的车辆状态信息的误差和传感器自身的误差参数相关联的矩阵。
预测模块203，用于基于状态转移矩阵和上一时刻的状态向量进行卡尔曼滤波预测，以得到当前时刻的第一状态向量和第一协方差矩阵。
测量构建模块204，用于基于当前时刻惯导系统得到的车辆状态信息、航位推算系统得到的车辆状态信息以及车载实时差分定位系统的车辆状态信息构建卡尔曼滤波测量向量和测量方程。
误差确定模块205，用于基于第一状态向量、第一协方差矩阵、测量向量和测量方程依次进行卡尔曼滤波的滤波增益和状态估计，以得到当前时刻的第二状态向量。以及
误差校正模块206，用于基于第二状态向量对第一状态向量中的当前时刻的传感器自身的误差参数、惯导系统得到的车辆状态信息的误差和航位推算系统得到的车辆状态信息的误差进行校正。
该实施例所提供的车辆定位系统校正装置具有和上述实施例所提供的车辆定位系统校正方法相同的有益效果，其具体实现过程可参照上述车辆定位系统校正方法的具体实现方式，本发明在此不再赘述。
参照图3所示，本发明的实施例还提供了一种设备，该设备可以包括：存储器301和处理器302；
存储器301，用于存储程序；
处理器302，用于执行程序，实现如上实施例所述的车辆定位系统校正方法的各个步骤。
本发明的实施例还提供了一种存储介质，其上存储有计算机程序，该计算机程序被处理器执行时，实现如上实施例所述的车辆定位系统校正方法的各个步骤。
本发明上述实施例所提供的车辆定位系统校正方法、装置、设备和存储介质提供了构建状态向量、状态转移矩阵基于车载实时差分定位系统输出的高精度的车辆的定位信息进行卡尔曼滤波，基于卡尔曼滤波的结果对传感器误差，惯导系统、航位推算系统中和传感器相关误差参数的校正。使得在车辆行驶的过程中即可实时完成对各传感器的误差校正，并且能够得到精确的车辆位置及姿态信息，相比于传统的专门进行的标定操作，能够简化惯性导航、航位推算系统的安装、使用步骤和条件，有利于低成本车载导航系统的大规模量产和装配。
对于前述的各方法实施例，为了简单描述，故将其都表述为一系列的动作组合，但是本领域技术人员应该知悉，本发明并不受所描述的动作顺序的限制，因为依据本发明，某些步骤可以采用其他顺序或者同时进行。其次，本领域技术人员也应该知悉，说明书中所描述的实施例均属于优选实施例，所涉及的动作和模块并不一定是本发明所必须的。
需要说明的是，本说明书中的各个实施例均采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似的部分互相参见即可。对于装置类实施例而言，由于其与方法实施例基本相似，所以描述的比较简单，相关之处参见方法实施例的部分说明即可。
本发明各实施例方法中的步骤可以根据实际需要进行顺序调整、合并和删减，各实施例中记载的技术特征可以进行替换或者组合。
本发明各实施例种装置及终端中的模块和子模块可以根据实际需要进行合并、划分和删减。
本发明所提供的几个实施例中，应该理解到，所揭露的终端，装置和方法，可以通过其它的方式实现。例如，以上所描述的终端实施例仅仅是示意性的，例如，模块或子模块的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个子模块或模块可以结合或者可以集成到另一个模块，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，装置或模块的间接耦合或通信连接，可以是电性，机械或其它的形式。
作为分离部件说明的模块或子模块可以是或者也可以不是物理上分开的，作为模块或子模块的部件可以是或者也可以不是物理模块或子模块，即可以位于一个地方，或者也可以分布到多个网络模块或子模块上。可以根据实际的需要选择其中的部分或者全部模块或子模块来实现本实施例方案的目的。
另外，在本发明各个实施例中的各功能模块或子模块可以集成在一个处理模块中，也可以是各个模块或子模块单独物理存在，也可以两个或两个以上模块或子模块集成在一个模块中。上述集成的模块或子模块既可以采用硬件的形式实现，也可以采用软件功能模块或子模块的形式实现。
专业人员还可以进一步意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、计算机软件或者二者的结合来实现，为了清楚地说明硬件和软件的可互换性，在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。
结合本文中所公开的实施例描述的方法或算法的步骤可以直接用硬件、处理器执行的软件单元，或者二者的结合来实施。软件单元可以置于随机存储器(RAM)、内存、只读存储器(ROM)、电可编程ROM、电可擦除可编程ROM、寄存器、硬盘、可移动磁盘、CD-ROM、或技术领域内所公知的任意其它形式的存储介质中。
最后，还需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。
对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。
Claims (8)
1.一种车辆定位系统校正方法，其特征在于，包括：
基于传感器标定参数需求构建卡尔曼滤波的状态向量，所述状态向量包括：传感器自身的误差参数、惯导系统得到的车辆状态信息的误差和航位推算系统得到的车辆状态信息的误差，所述惯导系统得到的车辆状态信息的误差和航位推算系统得到的车辆状态信息的误差均由所述传感器输出的测量数据参与计算得到；
基于所述状态向量构建卡尔曼滤波的状态转移矩阵，所述状态转移矩阵由惯导系统相关联的矩阵和航位推算系统相关联的矩阵组合构成，所述惯导系统相关联的矩阵为：使所述惯导系统得到的车辆状态信息的误差和所述传感器自身的误差参数相关联的矩阵，所述航位推算系统相关联的矩阵为：使所述航位推算系统得到的车辆状态信息的误差和所述传感器自身的误差参数相关联的矩阵；
基于所述状态转移矩阵和上一时刻的状态向量进行卡尔曼滤波预测，以得到当前时刻的第一状态向量和第一协方差矩阵；
基于当前时刻惯导系统得到的车辆状态信息、航位推算系统得到的车辆状态信息以及车载实时差分定位系统的车辆状态信息构建卡尔曼滤波测量向量和测量方程；
基于所述第一状态向量、所述第一协方差矩阵、所述测量向量和所述测量方程依次进行卡尔曼滤波的滤波增益和状态估计，以得到当前时刻的第二状态向量；
基于所述第二状态向量对所述第一状态向量中的当前时刻的传感器自身的误差参数、惯导系统得到的车辆状态信息的误差和航位推算系统得到的车辆状态信息的误差进行校正；
所述基于传感器标定参数需求构建卡尔曼滤波的状态向量，包括：
分别获取车辆上传感器、惯导系统和航位推算系统得到的车辆状态信息；
所述传感器得到的车辆状态信息包括：角速度信息和加速度信息：
所述惯导系统得到的车辆状态信息包括：第一速度信息、第一位置信息和姿态信息；
所述航位推算系统得到的车辆状态信息包括：第二速度信息和第二位置信息；
基于所述传感器得到的车辆状态信息、所述惯导系统得到的车辆状态信息和所述航位推算系统得到的车辆状态信息，对应得到所述传感器自身的误差参数、所述惯导系统得到的车辆状态信息的误差和所述航位推算系统得到的车辆状态信息的误差；
所述传感器自身的误差参数包括：陀螺仪和加速度计的零偏值、安装误差角度和比例系数误差，以及轮速计的比例系数误差；
所述惯导系统得到的车辆状态信息的误差包括：所述姿态信息的误差、所述第一速度信息的误差和所述第一位置信息的误差；
所述航位推算系统得到的车辆状态信息的误差包括：所述第二速度信息的误差和所述第二位置信息的误差；
所述基于所述状态向量构建卡尔曼滤波的状态转移矩阵，包括：
基于所述陀螺仪的零偏值和比例系数误差对所述姿态信息的误差的作用关系，得到姿态矩阵和角速度矩阵；
基于所述加速度计零偏值和比例系数误差对所述第一速度信息的误差的作用关系，得到加速度向量在所述惯导系统下的反对称矩阵和加速度矢量矩阵；
基于所述第二速度信息的误差、所述安装误差角都误差和所述轮速计的比例系数误差对所述第二位置信息的误差的作用关系，得到所述第二速度信息的反对称矩阵以及所述姿态信息和所述第二速度信息的关联矩阵。
2.根据权利要求1所述的方法，其特征在于，还包括：在校正完成后将所述上一时刻的状态向量和所述当前时刻的第二状态向量置零，以进行下一时刻的校正。
3.根据权利要求1所述的方法，其特征在于，所述基于传感器标定参数需求构建卡尔曼滤波的状态向量，还包括：
基于所述车辆状态信息构建卡尔曼滤波的27维状态向量：
Xk＝[ΦδvinsδpinsδpDRbgbaλDRδKDRμDRδKgδKa]T
其中，Φ为惯导系统计算的姿态信息θ、ψ、ψ的误差；δvins为惯导系统计算得到的第一速度信息vins的误差；δpins为惯导系统计算得到的第一位置信息pins的误差；δpDR为航位推算系统由轮速计推算得到的第二位置信息pDR的误差；bg、ba分别为陀螺仪、加速度计的零偏值；λDR、μDR分别为陀螺仪和加速度计与车体之间俯仰和航向方向的安装误差角；δKDR为轮速计的比例系数误差；δKg和δKa分别为陀螺仪和加速度计的比例系数误差。
4.根据权利要求3所述的方法，其特征在于，所述基于所述状态向量构建卡尔曼滤波的状态转移矩阵，包括：
基于所述陀螺仪的零偏值和比例系数误差对所述姿态信息的误差的作用关系，得到姿态矩阵和角速度矩阵，关系式如下：
式中，Φk为当前时刻的姿态信息的误差，Φk-1为上一时刻的姿态信息的误差，为车辆姿态矩阵，是由惯导系统输出的姿态信息θ、φ、ψ计算得到；Ω为角速度矢量ω构建的3×3维对角矩阵；
基于所述加速度计零偏值和比例系数误差对所述第一速度信息的误差的作用关系，得到加速度向量在所述惯导系统下的反对称矩阵和加速度矢量矩阵，关系式如下：
式中，为当前时刻第一速度信息的误差，/>为上一时刻第一速度信息的误差，(fn×)为将惯导系统变换至导航系下的加速度向量所构建的反对称矩阵；G为加速度矢量f构建的3×3维对角矩阵，其中惯导系统第一位置信息的误差受惯导系统的第一速度信息的误差的影响，关系式如下：
为当前时刻第一位置信息的误差，/>为上一时刻第一位置信息的误差，δvins为第一速度信息的误差；
基于所述第二速度信息的误差、所述安装误差角都误差和所述轮速计的比例系数误差对所述第二位置信息的误差的作用关系，得到所述第二速度信息的反对称矩阵以及所述姿态信息和所述第二速度信息的关联矩阵，关系式如下：
其中为当前时刻第二速度信息的误差，/>为上一时刻第二速度信息的误差，(vDR×)为轮速计计算得到车辆的第二速度信息vDR构建的反对称矩阵；Mcv为由姿态信息及第二速度信息构建的矩阵：
其中vD为轮速计得到的第二车速标量信息，C11～C33为矩阵中的各个元素。
5.根据权利要求4所述的方法，其特征在于，所述基于所述状态向量构建卡尔曼滤波的状态转移矩阵，还包括：
对所述姿态矩阵、所述角速度矩阵、所述加速度向量在所述惯导系统下的反对称矩阵、所述加速度矢量矩阵、所述第二速度信息的反对称矩阵以及所述姿态信息和所述第二速度信息的关联矩阵进行组合得到所述状态转移矩阵，如下：
A＝I+FΔT
其中
式中，0为3×3维的零矩阵，I为3维的单位矩阵，A为状态转移矩阵，ΔT为当前时刻和上一时刻的间隔时间。
6.根据权利要求5所述的方法，其特征在于，所述基于当前时刻惯导系统得到的车辆状态信息、航位推算系统得到的车辆状态信息以及车载实时差分定位系统的车辆状态信息构建卡尔曼滤波测量向量和测量方程，包括：
基于所述第一位置信息、所述姿态信息中的航向、所述第二位置信息和所述车载实时差分定位系统中的位移信息及航向构建所述测量向量和所述测量方程，如下：
式中，H1＝[0 0 1]，H2＝[C31 0 C33]，pins为惯导系统得到的第一位置信息，ψ为相应的航向信息；pDR为航位推算系统推算得到的第二位置信息；pgps为车载实时差分定位系统得到的位移，ψgps为车载实时差分定位系统的航向；
进行卡尔曼滤波的滤波增益计算、状态估计和协方差估计，得到：
K＝Pk，k-1HT(HPk，k-1HT+R)-1
Xk＝Xk-1+K(Z-HXk，k-1)
Pk＝(I-KH)Pk，k-1
式中，R为车载实时差分定位系统测量噪声方差构成的对角矩阵；Xk为卡尔曼滤波器估计得到的当前时刻的状态向量；Pk为卡尔曼滤波器当前时刻的协方差矩阵。
7.根据权利要求6所述的方法，其特征在于，所述基于所述第二状态向量对所述第一状态向量中的当前时刻的传感器自身的误差参数、惯导系统得到的车辆状态信息的误差和航位推算系统得到的车辆状态信息的误差进行校正，包括：
基于得到的当前时刻的状态向量Xk，得到各个误差为：
Φ＝Xk(1～3)
δvins＝Xk(4～6)
δpins＝Xk(7～9)
δpDR＝Xk(10～12)
bg＝Xk(13～15)
ba＝Xk(16～18)
λDR＝Xk(19)
δKDR＝Xk(20)
μDR＝Xk(21)
δKg＝Xk(22～24)
δKa＝Xk(25～27)
其中，括号中的数字代表相应误差值在状态向量中所占的列数位置，Φ为惯导系统计算的姿态信息θ、φ、ψ的误差；δvins为惯导系统计算的第一速度信息vins的误差；δpins为惯导系统计算的第一位置信息pins的误差；δpDR为航位推算系统的轮速计推算得到的第二位置信息pDR的误差；bg、ba分别为陀螺仪、加速度计的零偏值；λDR、μDR分别为陀螺仪及加速度计与车体之间俯仰和航向方向的安装误差角；δKDR为轮速计的比例系数误差；δKg和δKa分别为陀螺仪和加速度计的比例系数误差；
基于得到的各个误差值，对惯导系统和航位推算系统的误差参数进行校正，如下：
式中，为校正后的惯导系统的姿态矩阵，用于下一步的惯导系统的更新；/>为校正后的车体姿态矩阵，用于下一步的航位推算系统的航位推算；(Φ×)为惯导系统的姿态误差角Φ构成的反对称矩阵；(α×)为陀螺仪和加速度计与车体之间安装误差构成的反对称矩阵，α＝[λDR 0 μDR]T；/>为校正后惯导系统的第一速度信息，/>为校正后惯导系统的第一位置信息、/>为航位推算系统测量的速度标量、/>分别为校正后的陀螺仪和加速度计测量的角速度和加速度信息。
8.一种车辆定位系统校正装置，其特征在于，包括：
状态向量模块，用于基于传感器标定参数需求构建卡尔曼滤波的状态向量，所述状态向量包括：传感器自身的误差参数、惯导系统得到的车辆状态信息的误差和航位推算系统得到的车辆状态信息的误差，所述惯导系统得到的车辆状态信息的误差和航位推算系统得到的车辆状态信息的误差均由所述传感器输出的测量数据参与计算得到；
状态转移矩阵模块，用于基于所述状态向量构建卡尔曼滤波的状态转移矩阵，所述状态转移矩阵由惯导系统相关联的矩阵和航位推算系统相关联的矩阵组合构成，所述惯导系统相关联的矩阵为：使所述惯导系统得到的车辆状态信息的误差和所述传感器自身的误差参数相关联的矩阵，所述航位推算系统相关联的矩阵为：使所述航位推算系统得到的车辆状态信息的误差和所述传感器自身的误差参数相关联的矩阵；
预测模块，用于基于所述状态转移矩阵和上一时刻的状态向量进行卡尔曼滤波预测，以得到当前时刻的第一状态向量和第一协方差矩阵；
测量构建模块，用于基于当前时刻惯导系统得到的车辆状态信息、航位推算系统得到的车辆状态信息以及车载实时差分定位系统的车辆状态信息构建卡尔曼滤波测量向量和测量方程；
误差确定模块，用于基于所述第一状态向量、所述第一协方差矩阵、所述测量向量和所述测量方程依次进行卡尔曼滤波的滤波增益和状态估计，以得到当前时刻的第二状态向量；以及
误差校正模块，用于基于所述第二状态向量对所述第一状态向量中的当前时刻的传感器自身的误差参数、惯导系统得到的车辆状态信息的误差和航位推算系统得到的车辆状态信息的误差进行校正；
所述状态向量模块基于传感器标定参数需求构建卡尔曼滤波的状态向量，包括：
分别获取车辆上传感器、惯导系统和航位推算系统得到的车辆状态信息；
所述传感器得到的车辆状态信息包括：角速度信息和加速度信息：
所述惯导系统得到的车辆状态信息包括：第一速度信息、第一位置信息和姿态信息；
所述航位推算系统得到的车辆状态信息包括：第二速度信息和第二位置信息；
基于所述传感器得到的车辆状态信息、所述惯导系统得到的车辆状态信息和所述航位推算系统得到的车辆状态信息，对应得到所述传感器自身的误差参数、所述惯导系统得到的车辆状态信息的误差和所述航位推算系统得到的车辆状态信息的误差；
所述传感器自身的误差参数包括：陀螺仪和加速度计的零偏值、安装误差角度和比例系数误差，以及轮速计的比例系数误差；
所述惯导系统得到的车辆状态信息的误差包括：所述姿态信息的误差、所述第一速度信息的误差和所述第一位置信息的误差；
所述航位推算系统得到的车辆状态信息的误差包括：所述第二速度信息的误差和所述第二位置信息的误差；
所述状态转移矩阵模块基于所述状态向量构建卡尔曼滤波的状态转移矩阵，包括：
基于所述陀螺仪的零偏值和比例系数误差对所述姿态信息的误差的作用关系，得到姿态矩阵和角速度矩阵；
基于所述加速度计零偏值和比例系数误差对所述第一速度信息的误差的作用关系，得到加速度向量在所述惯导系统下的反对称矩阵和加速度矢量矩阵；
基于所述第二速度信息的误差、所述安装误差角都误差和所述轮速计的比例系数误差对所述第二位置信息的误差的作用关系，得到所述第二速度信息的反对称矩阵以及所述姿态信息和所述第二速度信息的关联矩阵。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210066361.0A CN114413934B (zh) | 2022-01-20 | 2022-01-20 | 一种车辆定位系统校正方法和装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210066361.0A CN114413934B (zh) | 2022-01-20 | 2022-01-20 | 一种车辆定位系统校正方法和装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114413934A CN114413934A (zh) | 2022-04-29 |
| CN114413934B true CN114413934B (zh) | 2024-01-26 |
Family
ID=81275948
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210066361.0A Active CN114413934B (zh) | 2022-01-20 | 2022-01-20 | 一种车辆定位系统校正方法和装置 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114413934B (zh) |
Families Citing this family (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116242410B (zh) * | 2022-09-05 | 2023-12-19 | 浙江智马达智能科技有限公司 | 一种标定方法、终端及计算机存储介质 |
| CN115900695A (zh) * | 2022-09-30 | 2023-04-04 | 中国第一汽车股份有限公司 | 一种应用于车辆的智能泊车车辆定位的方法 |
| CN115790615B (zh) * | 2023-01-30 | 2023-05-05 | 安徽蔚来智驾科技有限公司 | 基于自动驾驶的多传感器融合方法、装置、介质及车辆 |
Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2012049492A1 (en) * | 2010-10-13 | 2012-04-19 | University Of Nottingham | Positioning system |
| CN105300379A (zh) * | 2015-10-13 | 2016-02-03 | 上海新纪元机器人有限公司 | 一种基于加速度的卡尔曼滤波姿态估计方法及系统 |
| CN106950586A (zh) * | 2017-01-22 | 2017-07-14 | 无锡卡尔曼导航技术有限公司 | 用于农机作业的gnss/ins/车辆组合导航方法 |
| CN108180925A (zh) * | 2017-12-15 | 2018-06-19 | 中国船舶重工集团公司第七0七研究所 | 一种里程计辅助车载动态对准方法 |
| CN109163721A (zh) * | 2018-09-18 | 2019-01-08 | 河北美泰电子科技有限公司 | 姿态测量方法及终端设备 |
| CN111351481A (zh) * | 2020-03-13 | 2020-06-30 | 南京理工大学 | 一种基于发射惯性坐标系的传递对准方法 |
| WO2020220729A1 (zh) * | 2019-04-29 | 2020-11-05 | 南京航空航天大学 | 基于角加速度计/陀螺/加速度计的惯性导航解算方法 |
| CN111947681A (zh) * | 2020-06-24 | 2020-11-17 | 中铁第四勘察设计院集团有限公司 | 一种gnss与惯导组合导航位置输出的滤波校正方法 |
| CN112762961A (zh) * | 2020-12-28 | 2021-05-07 | 厦门华源嘉航科技有限公司 | 一种车载惯性里程计组合导航在线标定方法 |
| WO2022007437A1 (zh) * | 2020-07-04 | 2022-01-13 | 华为技术有限公司 | 传感器安装偏差角的标定方法、组合定位系统和车辆 |
Family Cites Families (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US10533856B2 (en) * | 2017-04-05 | 2020-01-14 | Novatel Inc. | Navigation system utilizing yaw rate constraint during inertial dead reckoning |
| CN109059904A (zh) * | 2018-06-01 | 2018-12-21 | 浙江亚特电器有限公司 | 用于移动载具的组合导航方法 |
-
2022
- 2022-01-20 CN CN202210066361.0A patent/CN114413934B/zh active Active
Patent Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2012049492A1 (en) * | 2010-10-13 | 2012-04-19 | University Of Nottingham | Positioning system |
| CN105300379A (zh) * | 2015-10-13 | 2016-02-03 | 上海新纪元机器人有限公司 | 一种基于加速度的卡尔曼滤波姿态估计方法及系统 |
| CN106950586A (zh) * | 2017-01-22 | 2017-07-14 | 无锡卡尔曼导航技术有限公司 | 用于农机作业的gnss/ins/车辆组合导航方法 |
| CN108180925A (zh) * | 2017-12-15 | 2018-06-19 | 中国船舶重工集团公司第七0七研究所 | 一种里程计辅助车载动态对准方法 |
| CN109163721A (zh) * | 2018-09-18 | 2019-01-08 | 河北美泰电子科技有限公司 | 姿态测量方法及终端设备 |
| WO2020220729A1 (zh) * | 2019-04-29 | 2020-11-05 | 南京航空航天大学 | 基于角加速度计/陀螺/加速度计的惯性导航解算方法 |
| CN111351481A (zh) * | 2020-03-13 | 2020-06-30 | 南京理工大学 | 一种基于发射惯性坐标系的传递对准方法 |
| CN111947681A (zh) * | 2020-06-24 | 2020-11-17 | 中铁第四勘察设计院集团有限公司 | 一种gnss与惯导组合导航位置输出的滤波校正方法 |
| WO2022007437A1 (zh) * | 2020-07-04 | 2022-01-13 | 华为技术有限公司 | 传感器安装偏差角的标定方法、组合定位系统和车辆 |
| CN112762961A (zh) * | 2020-12-28 | 2021-05-07 | 厦门华源嘉航科技有限公司 | 一种车载惯性里程计组合导航在线标定方法 |
Non-Patent Citations (3)
| Title |
|---|
| SINS/DR组合导航系统可观测性研究;严涛;王跃钢;杨波;贾磊;;现代防御技术(第03期);全文 * |
| Unscented kalman filter with process noise covariance estimation for vehicular ins/gps integration system;Gaoge Hu Et al.;Information Fusion;全文 * |
| 基于多项式卡尔曼滤波器的车辆定位试验;赵汝棋等;农业工程学报;第28卷(第18期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114413934A (zh) | 2022-04-29 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN114413934B (zh) | 一种车辆定位系统校正方法和装置 | |
| EP2519803B1 (en) | Technique for calibrating dead reckoning positioning data | |
| JP5237723B2 (ja) | 動的に較正されるセンサデータと、ナビゲーションシステム内の繰り返し拡張カルマンフィルタとを使用する、ジャイロコンパスの整合用のシステム及び方法 | |
| CN102313822B (zh) | 偏置估算方法、姿势估算方法、偏置估算装置及姿势估算装置 | |
| JP5586994B2 (ja) | 位置標定装置、位置標定装置の位置標定方法および位置標定プログラム | |
| JP5074950B2 (ja) | 航法装置 | |
| CN109186597B (zh) | 一种基于双mems-imu的室内轮式机器人的定位方法 | |
| CN102645222A (zh) | 一种卫星惯性导航方法和设备 | |
| CN111982106A (zh) | 导航方法、装置、存储介质及电子装置 | |
| CN112432642B (zh) | 一种重力灯塔与惯性导航融合定位方法及系统 | |
| EP3411661B1 (en) | System and method for calibrating magnetic sensors in real and finite time | |
| JP4941199B2 (ja) | ナビゲーション装置 | |
| CN113252048B (zh) | 一种导航定位方法、导航定位系统及计算机可读存储介质 | |
| CN110207691A (zh) | 一种基于数据链测距的多无人车协同导航方法 | |
| CN109405837A (zh) | 物体定位方法、应用和车辆 | |
| JP5164645B2 (ja) | カルマンフィルタ処理における繰り返し演算制御方法及び装置 | |
| CN115615430B (zh) | 基于捷联惯导的定位数据修正方法及系统 | |
| CN113074757B (zh) | 车载惯导安装误差角的标定方法 | |
| JP2016170124A (ja) | 移動体測位装置、及び移動体測位方法 | |
| CN106646569A (zh) | 一种导航定位方法及设备 | |
| CN111220151B (zh) | 载体系下考虑温度模型的惯性和里程计组合导航方法 | |
| JP2021085880A (ja) | 移動体の定位誤差の分析 | |
| CN114897942B (zh) | 点云地图的生成方法、设备及相关存储介质 | |
| CN114019954B (zh) | 航向安装角标定方法、装置、计算机设备和存储介质 | |
| CN114964262A (zh) | 一种基于多源融合的机器人定位方法 |
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 |