CN113048987A - 一种车载导航系统定位方法 - Google Patents
一种车载导航系统定位方法 Download PDFInfo
- Publication number
- CN113048987A CN113048987A CN202110269452.XA CN202110269452A CN113048987A CN 113048987 A CN113048987 A CN 113048987A CN 202110269452 A CN202110269452 A CN 202110269452A CN 113048987 A CN113048987 A CN 113048987A
- Authority
- CN
- China
- Prior art keywords
- vehicle
- speed
- error
- sins
- real
- 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
- 238000000034 method Methods 0.000 title claims abstract description 20
- 238000005259 measurement Methods 0.000 claims abstract description 48
- 230000003287 optical effect Effects 0.000 claims abstract description 29
- 239000013598 vector Substances 0.000 claims abstract description 17
- 238000001914 filtration Methods 0.000 claims abstract description 9
- 238000004364 calculation method Methods 0.000 claims abstract description 6
- 239000011159 matrix material Substances 0.000 claims description 17
- 238000009434 installation Methods 0.000 claims description 7
- 230000007704 transition Effects 0.000 description 3
- 238000009825 accumulation Methods 0.000 description 2
- 238000003384 imaging method Methods 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 240000007594 Oryza sativa Species 0.000 description 1
- 235000007164 Oryza sativa Nutrition 0.000 description 1
- 230000001133 acceleration Effects 0.000 description 1
- 230000003190 augmentative effect Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000007812 deficiency Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 235000009566 rice Nutrition 0.000 description 1
- 238000012546 transfer Methods 0.000 description 1
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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
- G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
- G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
- G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- Automation & Control Theory (AREA)
- Navigation (AREA)
Abstract
Description
技术领域
本发明涉及导航定位技术领域,具体涉及车载导航系统定位方法。
背景技术
随着微机电系统(MEMS)技术的发展,捷联惯导系统(SINS)的体积和成本有效降低,但精度也降低了,会随着时间的累积而产生较大的测量误差。在车辆经过隧道、高架桥、山区以及高楼密集的环境时,BDS北斗卫星导航系统(BeiDou Navigation SatelliteSystem,BDS)信号受到干扰和遮挡,常常出现短期失锁的现象,而SINS在单独工作的情况下又有误差迅速累积的问题,因此很难在BDS信号被中断时,为车辆提供准确的速度信息和位置信息。
现阶段,大部分学者的研究仅集中于提高车辆定位估计的准确性,忽略了由于BDS系统自身缺陷而造成的环境限制和SINS的误差累积问题,因此需要一种辅助方法,在BDS信号被中断时,为车辆提供准确的速度信息和位置信息。
发明内容
针对现有技术的不足,本发明提供了一种车载导航系统定位方法,能为车辆提供准确的速度信息和位置信息。
本发明所提供的技术方案为:
一方面,本发明提供一种车载导航系统定位方法,包括以下步骤:基于车体安装的SINS和摄像头采集的数据,分别通过SINS推算和光流估计得到车辆实时速度和将和之差作为量测矢量,建立量测方程;通过卡尔曼滤波算法对导航系统的状态方程和量测方程进行解算,得到速度误差的最优估计值,使用速度误差的最优估计值对通过SINS推算得到的车辆实时速度进行修正,得到修正后的车辆实时速度;最后,基于修正后的车辆实时速度进行积分计算求出车辆实时位置。
另一方面,本发明提供一种车载BDS/SINS组合导航系统定位方法,包括以下步骤:
步骤1、建立BDS/SINS组合导航系统的状态方程和以下两个量测方程;
量测方程一:
Z1(t)=H1(t)·X(t)+V1(t)
量测方程二:
Z2(t)=H2(t)·X(t)+V2(t)
式中:Z1(t)和Z2(t)为系统的两个量测矢量; 其中和分别是SINS推算的和BDS接收机测得的导航坐标系下的车辆实时速度;和分别是SINS推算的和BDS接收机测得的导航坐标系下的车辆实时位置;是基于光流估计测得的导航坐标系中的车辆实时速度;
X(t)为系统的状态矩阵,由系统的误差数据构成,包括位置误差和速度误差;
步骤2、在BDS可用时,利用卡尔曼滤波算法对BDS/SINS组合导航系统的状态方程和量测方程一进行解算,得到位置误差和速度误差的最优估计值,使用位置误差和速度误差的最优估计值对BDS接收机测得的车辆实时位置和速度进行修正;
在BDS不可用时,利用通过卡尔曼滤波算法对BDS/SINS组合导航系统的状态方程和量测方程二进行解算,得到速度误差的最优估计值,使用速度误差的最优估计值对对通过SINS推算得到的车辆实时速度进行修正,得到修正后的车辆实时速度(即车辆实时速度的最优估计值);最后,基于修正后的车辆实时速度进行积分计算求出车辆实时位置。
进一步地,
Vy=z*vy/ay
式中:θ为SINS与车体的俯仰安装误差角,为SINS与车体的航向安装误差角;Vy表示摄像头坐标系中的车辆实时速度;ay是车辆前进方向的像素焦距;vy表示图像在车辆前进方向的像素速度;z表示摄像头与地面的距离。
进一步地,所述状态矩阵为:
其中,δrc、δvc、ψ、bg、ba、sg、sa和δk分别表示位置误差、速度误差、姿态误差、陀螺仪的零偏、加速度计的零偏、陀螺仪的误差、加速度计的误差和光流刻度系数误差。
有益效果:
本发明提出的车载导航系统定位方法及车载BDS/SINS组合导航系统定位方法,基于光流信息辅助导航系统进行定位,可以在BDS中断期间准确定位。对光流测速的原理进行深入研究,依据车辆建模的侧面速度约束(即假定车辆在地面行驶过程中不发生侧滑或跳跃,即车辆在载体坐标系左右和上下方向速度为0)确定两个方向上的速度虚拟观测量,再加上基于光流计算的行进方向上的速度观测量,以此形成完整的三维速度观测量,建立光流测速输出模型并通过扩展卡尔曼滤波修正SINS误差,能有效抑制组合导航系统的误差发散,位置、速度精度得到明显改善。本发明可以在BDS信号中断时提供可靠的速度信息,能有效抑制组合导航系统的误差发散,位置、速度估计精度得到明显改善。
附图说明
图1为本发明实施例中的光流测速中坐标系示意图;
图2为本发明实施例中的光流辅助车载BDS/SINS组合导航系统;
具体实施方式
以下结合附图和具体实施方式对本发明进行进一步具体说明。
实施例1:
本实施例提供一种车载导航系统定位方法(基于光流辅助的定位方法),包括以下步骤:基于车体安装的SINS和摄像头采集的数据,分别通过SINS推算和光流估计得到车辆实时速度和将和之差作为量测矢量,建立量测方程;通过卡尔曼滤波算法对导航系统的状态方程和量测方程进行解算,得到速度误差的最优估计值,使用速度误差的最优估计值对通过SINS推算得到的车辆实时速度进行修正,得到修正后的车辆实时速度;最后,基于修正后的车辆实时速度进行积分计算求出车辆实时位置。
实施例2:
本实施例提供一种车载BDS/SINS组合导航系统定位方法,将上述实施例1中提供的定位方法用于车载BDS/SINS组合导航系统中,在BDS信号中断时进行辅助定位,具体包括以下步骤:
步骤1.基于光流估计获得摄像头坐标系中的车辆实时速度Vy;
将摄像头(光流传感器)安装在车辆底部,光轴垂直于地面,以实现地面纹理的垂直拍摄;以摄像头为坐标原点O,以车辆前进方向为Y轴,左右方向为X轴,与Y轴垂直,Z轴方向与地面垂直,建立摄像头坐标系OXYZ。由于运动约束(即短时间内车辆保持向前直线行驶,不会左右转弯和上下颠簸),汽车X轴、Z轴的运动可以忽略。设oxy为二维成像平面,那么点O与oxyz的距离f即为摄像头焦距。设地面上有一参考点P,在OXYZ下坐标为(x,y,z),则在oxy成像平面内有一点P′为P的投影,假设点P′在OXYZ下坐标为(x′,y′,f),那么存在如下关系式:
x′=f*x/z
y′=f*y/z
由上述关系式可知,假设车辆相对于P存在相对运动,并且运动期间地形表面是平坦的,那么有:
Vy=z*vy/ay
式中:Vy表示摄像头坐标系中的车辆实时速度,其单位是米/秒;ay是Y方向的像素焦距,其单位是像素;vy表示图像在Y方向的像素速度,其中像素速度通过观察像素在摄像头捕捉到的相邻两帧图像中(在时间域上)的位置变化,从而推算出像素运动的瞬时速度,其单位是像素/秒;z表示摄像头与地面的距离,其单位是米。
在不考虑SINS与车体的安装误差角度时,光流在载体坐标系(本发明中载体是指车体,载体坐标系与载体固连,坐标原点是载体中心)下输出速度可以表示为:
实际应用中,很难确保SINS轴向与光流轴向一致,存在一定的角度偏差。通常SINS固定在车辆上,安装误差角为小角度,并且几乎不会变化。把SINS坐标系记为b系,导航坐标系记为n系,表示b和n之间的姿态转移矩阵,则光流估计的速度在导航坐标系(导航坐标系选用“东-北-天”地理坐标系,坐标原点是载体中心初始位置)中的实际形式为:
步骤3.利用扩展卡尔曼滤波进行数据融合,建立BDS/SINS组合导航系统的状态方程和两个量测方程;
采用随机常数描述光流刻度系数误差δk的变化,并将δk增广到状态矩阵中:
X(t)是一个22×1的矩阵,其中变量δrc、δvc、ψ、bg、ba、sg、sa和δk分别表示位置误差、速度误差、姿态误差、陀螺仪的零偏、加速度计的零偏、陀螺仪的误差、加速度计的误差和光流刻度系数误差,前面7个变量各包含3个维度,最后一个变量维度是1;
系统状态转移矩阵为:
式中,Fw为对应于SINS的9个误差的状态转移矩阵,Fs表示姿态转移矩阵。
系统的噪声分配矩阵为:
因此,BDS/SINS组合导航系统的状态方程为:
当BDS可用时,导航系统采用BDS/SINS进行组合,组合系统采用SINS解算的位置和速度与BDS测量得到的位置与速度之差作为量测信息,量测方程可表示为:
式中:Z1(t)为系统的量测矢量;H1(t)为系统的量测矩阵,X(t)为系统的状态矩阵,V1(t)为系统的量测噪声矢量;和分别是SINS推算的和BDS接收机测量的导航坐标系下的车辆实时速度;和分别是SINS推算的和BDS接收机测量的导航坐标系下的车辆实时位置;
当BDS不可用时,系统采用SINS和光流的速度之差建立量测方程,具体形式为:
其中,Z2(t)为系统的量测矢量;H2(t)为系统的量测矩阵,V2(t)为系统的量测噪声矢量;H2(t)=Hv,V2(t)=Vv;
步骤4.对车辆速度误差进行最优估计,得到修正后的速度和位置。
在BDS可用时,BDS接收机直接可以输出车辆位置和速度分量,通过卡尔曼滤波算法解算得到速度误差最优估计值,使用速度误差最优估计值对通过SINS推算得到的/BDS接收机测得的位置和速度进行修正;
在BDS不可用时,BDS接收机无法输出车辆位置和速度,此时使用光流解算出的速度信息,通过卡尔曼滤波算法解算得到速度误差最优估计值,使用速度误差最优估计值对通过SINS推算得到的车辆实时速度进行修正,得到修正后的车辆实时速度(最优估计);最后,通过对修正后的车辆实时速度进行积分求出车辆实时位置。
本发明上述实施例依据车辆建模的侧面速度约束(即假定车辆在地面行驶过程中不发生侧滑或跳跃,即车辆在载体坐标系左右和上下方向速度为0)确定两个方向上的速度虚拟观测量,再加上基于光流计算的行进方向上的速度观测量,以此形成完整的三维速度观测量,建立光流测速输出模型并通过扩展卡尔曼滤波修正SINS误差,能有效抑制组合导航系统的误差发散,位置、速度精度得到明显改善。本发明可以在BDS信号中断时提供可靠的速度信息,能有效抑制组合导航系统的误差发散,位置、速度估计精度得到明显改善。
Claims (4)
2.一种车载BDS/SINS组合导航系统定位方法,其特征在于,包括以下步骤:
步骤1、建立BDS/SINS组合导航系统的状态方程和以下两个量测方程;
量测方程一:
Z1(t)=H1(t)·X(t)+V1(t)
量测方程二:
Z2(t)=H2(t)·X(t)+V2(t)
式中:Z1(t)和Z2(t)为系统的两个量测矢量; 其中和分别是SINS推算的和BDS接收机测得的导航坐标系下的车辆实时速度;和分别是SINS推算的和BDS接收机测得的导航坐标系下的车辆实时位置;是基于光流估计测得的导航坐标系中的车辆实时速度;
X(t)为系统的状态矩阵,由系统的误差数据构成,包括位置误差和速度误差;
步骤2、在BDS可用时,利用卡尔曼滤波算法对BDS/SINS组合导航系统的状态方程和量测方程一进行解算,得到位置误差和速度误差的最优估计值,使用位置误差和速度误差的最优估计值对SINS推算得到的/BDS接收机测得的车辆实时位置和速度进行修正;
在BDS不可用时,利用通过卡尔曼滤波算法对BDS/SINS组合导航系统的状态方程和量测方程二进行解算,得到速度误差的最优估计值,使用速度误差的最优估计值对对通过SINS推算得到的车辆实时速度进行修正,得到修正后的车辆实时速度;最后,基于修正后的车辆实时速度进行积分计算求出车辆实时位置。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110269452.XA CN113048987A (zh) | 2021-03-12 | 2021-03-12 | 一种车载导航系统定位方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110269452.XA CN113048987A (zh) | 2021-03-12 | 2021-03-12 | 一种车载导航系统定位方法 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN113048987A true CN113048987A (zh) | 2021-06-29 |
Family
ID=76512043
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202110269452.XA Pending CN113048987A (zh) | 2021-03-12 | 2021-03-12 | 一种车载导航系统定位方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113048987A (zh) |
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114869604A (zh) * | 2022-05-07 | 2022-08-09 | 江苏理工学院 | 物联网轮椅智能导航系统 |
CN118397733A (zh) * | 2024-06-28 | 2024-07-26 | 贾维斯智能(深圳)有限公司 | 一种具有自动校正功能的行车记录仪及其使用方法 |
Citations (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108534783A (zh) * | 2018-05-11 | 2018-09-14 | 安徽尼古拉电子科技有限公司 | 一种基于北斗导航技术的飞行器导航方法 |
-
2021
- 2021-03-12 CN CN202110269452.XA patent/CN113048987A/zh active Pending
Patent Citations (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108534783A (zh) * | 2018-05-11 | 2018-09-14 | 安徽尼古拉电子科技有限公司 | 一种基于北斗导航技术的飞行器导航方法 |
Non-Patent Citations (3)
Title |
---|
CHEN ZHONGYUAN,ET.AL: "Fault-Tolerant Optical Flow Sensor/SINS Integrated Navigation Scheme for MAV in a GPS-Denied Environment", 《JOURNAL OF SENSORS》 * |
MENG FEI,ET.AL: "Research of UAV Location Control System Based on SINS, GPS and Optical Flow", 《2020 IEEE INTERNATIONAL CONFERENCE ON INFORMATION TECHNOLOGY, BIG DATA AND ARTIFICIAL INTELLIGENCE (ICIBA)》 * |
李伟: "基于EKF视觉组合导航方法的研究", 《中国优秀硕士学位论文全文数据库信息科技辑》 * |
Cited By (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114869604A (zh) * | 2022-05-07 | 2022-08-09 | 江苏理工学院 | 物联网轮椅智能导航系统 |
CN118397733A (zh) * | 2024-06-28 | 2024-07-26 | 贾维斯智能(深圳)有限公司 | 一种具有自动校正功能的行车记录仪及其使用方法 |
CN118397733B (zh) * | 2024-06-28 | 2024-09-24 | 贾维斯智能(深圳)有限公司 | 一种具有自动校正功能的行车记录仪及其使用方法 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109946732B (zh) | 一种基于多传感器数据融合的无人车定位方法 | |
CN113819914A (zh) | 一种地图构建方法及装置 | |
CN109540126A (zh) | 一种基于光流法的惯性视觉组合导航方法 | |
CN108731670A (zh) | 基于量测模型优化的惯性/视觉里程计组合导航定位方法 | |
CN109186597B (zh) | 一种基于双mems-imu的室内轮式机器人的定位方法 | |
CN110530361B (zh) | 基于农业机械双天线gnss自动导航系统的转向角度估计器 | |
CN111678514B (zh) | 一种基于载体运动条件约束和单轴旋转调制的车载自主导航方法 | |
CN108345021B (zh) | 一种多普勒雷达辅助gps/ins车辆测速方法 | |
US20170074678A1 (en) | Positioning and orientation data analysis system and method thereof | |
CN113819905A (zh) | 一种基于多传感器融合的里程计方法及装置 | |
CN115060257B (zh) | 一种基于民用级惯性测量单元的车辆变道检测方法 | |
CN110823224B (zh) | 一种车辆定位方法及车辆 | |
CN113008229B (zh) | 一种基于低成本车载传感器的分布式自主组合导航方法 | |
CN110426011A (zh) | 车辆转向角测量系统及方法 | |
CN113048987A (zh) | 一种车载导航系统定位方法 | |
JP2014240266A (ja) | センサドリフト量推定装置及びプログラム | |
Niu et al. | Camera-based lane-aided multi-information integration for land vehicle navigation | |
CN115790613B (zh) | 一种视觉信息辅助的惯性/里程计组合导航方法及装置 | |
CN117053802A (zh) | 一种基于旋转mems imu的车载导航系统定位误差减小的方法 | |
Liu et al. | IMU/vehicle calibration and integrated localization for autonomous driving | |
CN211012986U (zh) | 一种基于惯导技术的无人自主巡航车导航系统 | |
CN114659488B (zh) | 一种基于误差卡尔曼滤波的高动态车辆姿态估计方法 | |
CN112923934A (zh) | 一种适用于非结构化场景中结合惯导的激光slam技术 | |
CN109827569A (zh) | 无人车定位方法及系统 | |
CN116576849A (zh) | 一种基于gmm辅助的车辆融合定位方法及系统 |
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 | ||
WD01 | Invention patent application deemed withdrawn after publication | ||
WD01 | Invention patent application deemed withdrawn after publication |
Application publication date: 20210629 |