CN112731483B - 一种自动驾驶组合导航***中rtk异常值的判断方法 - Google Patents
一种自动驾驶组合导航***中rtk异常值的判断方法 Download PDFInfo
- Publication number
- CN112731483B CN112731483B CN202011471789.0A CN202011471789A CN112731483B CN 112731483 B CN112731483 B CN 112731483B CN 202011471789 A CN202011471789 A CN 202011471789A CN 112731483 B CN112731483 B CN 112731483B
- Authority
- CN
- China
- Prior art keywords
- rtk
- automatic driving
- odometer
- automobile
- navigation
- 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 52
- 230000002159 abnormal effect Effects 0.000 title claims abstract description 15
- 238000004364 calculation method Methods 0.000 claims abstract description 28
- 230000001133 acceleration Effects 0.000 claims description 30
- 239000011159 matrix material Substances 0.000 claims description 9
- 238000005070 sampling Methods 0.000 claims description 9
- 230000009466 transformation Effects 0.000 claims description 7
- IAZDPXIOMUYVGZ-UHFFFAOYSA-N Dimethylsulphoxide Chemical compound CS(C)=O IAZDPXIOMUYVGZ-UHFFFAOYSA-N 0.000 claims description 3
- 230000005856 abnormality Effects 0.000 abstract description 3
- 238000005516 engineering process Methods 0.000 description 7
- 238000009825 accumulation Methods 0.000 description 2
- 238000004458 analytical method Methods 0.000 description 2
- 238000006243 chemical reaction Methods 0.000 description 2
- 238000011161 development Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 238000005259 measurement Methods 0.000 description 2
- 206010063385 Intellectualisation Diseases 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000005336 cracking Methods 0.000 description 1
- 230000001186 cumulative effect Effects 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
Classifications
-
- 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/01—Satellite radio beacon positioning systems transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/13—Receivers
- G01S19/35—Constructional details or hardware or software details of the signal processing chain
- G01S19/37—Hardware or software details of the signal processing chain
-
- 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/43—Determining position using carrier phase measurements, e.g. kinematic positioning; using long or short baseline interferometry
- G01S19/44—Carrier phase ambiguity resolution; Floating ambiguity; LAMBDA [Least-squares AMBiguity Decorrelation Adjustment] method
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Signal Processing (AREA)
- Computer Networks & Wireless Communication (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
Abstract
本发明属于自动驾驶车载组合导航定位领域,特别涉及一种自动驾驶组合导航***中RTK异常值的判断方法,包括步骤:利用惯性和里程计组合导航参数建立判断基准,判断RTK定位精度的有效性;当判断RTK定位精度无效时,航位推算预估自动驾驶汽车的当前位置信息;在自动驾驶汽车RTK解算位置长时间失效后,判断RTK解算位置信息的有效性;当判断RTK解算位置信息有效时,对惯性和里程计组合导航参数进行矫正。本发明可以在RTK信号受阻的情况下,及时判断出RTK导航的异常,进而及时切换导航参数获取的途径,避免自动驾驶汽车脱离正确行驶的方向。
Description
技术领域
本发明属于自动驾驶车载组合导航定位领域,特别涉及一种自动驾驶惯性/里程计/RTK自动驾驶组合导航***中RTK异常值的判断方法。
背景技术
科学技术的快速发展促进了机械化时代的步伐,为自动化、智能化、机械化的自动驾驶领域提供了前所未有的机遇。自动驾驶领域的崛起需要各种高精尖的核心技术支持,其中最关键的核心之一就是车载组合导航的融合定位技术,这一技术将代替人眼、手、脚的操作,来自动操控汽车,实现汽车的路径规划、行为决策等一系列的操作。
随着车载导航技术的不断成熟,虽给自动驾驶领域提供了机遇和平台,但核心技术中仍存在一些难题有待解决和方案优化。以RTK定位技术为例,目前虽然利用GPS载波差分原理实现了厘米级的高精度定位,但由于RTK导航受信号、环境等影响将造成局部区域导航定位精度裂化的现象,而无法满足自动驾驶导航定位的精度要求。在这部分区域内将需要利用惯性/里程计短期精度高的特点提供自动驾驶汽车导航参数。然而,由于没有准确的RTK信号受阻的反馈,导致无法及时判断出RTK导航的异常,进而就无法及时切换导航参数获取的途径,这将可能造成自动驾驶汽车脱离正确行驶的方向。
因此,为避免上述现象的发生,提供一种自动驾驶惯性/里程计/RTK组合导航***中RTK异常值判断方法是必要的,也是不可或缺的手段,其研究将在一定程度上推动自动驾驶汽车技术的发展,也将为同类型的工程问题的解决提供一种新的解决思路。
发明内容
为了解决上述问题,本发明提出一种自动驾驶组合导航***中RTK异常值的判断方法。其中,为判断自动驾驶汽车行驶过程中RTK定位的有效性,首先利用惯性/里程计组合导航参数建立判断基准,当RTK解算的自动驾驶汽车的速度与加速度不满足惯性/里程计组合导航解算结果所确定的阈值时,认定RTK信息异常,之后利用惯性/里程计组合导航短期定位精度高的特点,对当前时刻的载体车的位置进行估计,并将解算结果作为当前时刻自动驾驶汽车的位姿。然而,由于惯性/里程计组合导航具有误差随时间累积发散的不足,当RTK长时间无效时,惯性/里程计所建立的基准可信度将降低。为了准确判断RTK信息,当RTK长时间异常时,利用惯性/里程计在相邻两点距离测量精度高、且姿态精度高的特性,与RTK解算的相邻两点的距离及航向做对比,当满足一定阈值时,则认为RTK信号恢复正常,并利用RTK信息对惯性/里程计组合导航结果进行矫正,并将矫正后的结果作为判断RTK信号是否异常的条件,直到整个自动驾驶过程结束。
本发明提供了一种自动驾驶组合导航***中RTK异常值的判断方法,其特征在于,包括步骤:
S1:利用惯性和里程计组合导航参数建立判断基准,对自动驾驶车载组合导航中RTK定位精度的有效性进行判断,具体过程如下:
S11:建立惯性和里程计组合导航的速度阈值;
S12:建立惯性和里程计组合导航的加速度阈值;
S13:判断RTK解算位置信息的有效性;
S2:当判断自动驾驶车载组合导航中RTK定位精度无效时,航位推算预估自动驾驶汽车的当前位置信息,具体过程如下:
S21:基于惯性和里程计组合导航进行自动驾驶汽车的姿态解算;
S22:基于里程计位置信息差分原理估计当前时刻自动驾驶汽车的位置;
S3:在自动驾驶汽车RTK解算位置长时间失效后,判断RTK定位精度的有效性;
S4:当判断RTK定位精度有效时,对惯性和里程计组合导航参数进行矫正,然后将矫正后的惯性和里程计组合导航参数作为评价RTK定位精度是否有效的新条件。
进一步,步骤S11具体过程为:
定义RTK解算的自动驾驶汽车的速度为vRTK(t),加速度为aRTK(t);利用惯性和里程计差分原理解算获得的自动驾驶汽车的速度为vDR(t),加速度为aDR(t),且里程计的分辨率为m,里程计的分辨率造成的速度误差δvDR为:
δvDR≤m (1)
定义自动驾驶汽车的真实速度为v(t),RTK解算速度误差为δv(t),惯性和里程计的数据采样率为f,RTK数据采样率为q,当信号不稳定造成RTK定位误差最大值为a时,RTK解算速度误差δv(t)在理想状态下满足下式:
为保证自动驾驶汽车定位精度要求,考虑RTK器件误差条件下,自动驾驶汽车允许的RTK最大速度误差满足:
其中,Δs为最大允许定位误差,
利用惯性和里程计差分原理解算获得的自动驾驶汽车的速度vDR(t)与RTK解算的自动驾驶汽车的速度vRTK(t)满足下式(4)时,认为RTK解算位置信息准确:
vDR(t)-vRTK(t)≤|δv(t)|+|δvDR|=w (4)
其中,w为为可接受范围内速度解算误差阈值。
进一步,步骤S12具体过程为:
定义在自动驾驶汽车定位精度内允许的RTK解算加速度误差为δa(t),在自动驾驶汽车速度不超过最大速度时,RTK解算加速度误差满足:
其中,s表示在时间段[ti,ti-1]内的自动驾驶汽车的真实行驶距离;
在采样时间间隔内默认加速度为定值,则对上式(5)整理得到:
当RTK解算加速度误差δa(t)满足式(6)时,认为RTK解算位置信息准确。
进一步,步骤S13中,自动驾驶车载组合导航中RTK解算位置信息的有效性判断条件为:
其中,b为允许自动驾驶汽车行驶过程中的最大加速度。
进一步,步骤S21具体过程为:
定义自动驾驶汽车的坐标系为载体系,记为b系,设自动驾驶汽车的航向角为ψ,俯仰角为θ,横滚角为γ;定义地理坐标系为导航系,记为n系,根据坐标系转换原理计算出导航系与载体系的坐标转换矩阵为:
根据四元素解算出载体系与导航系的坐标转换矩阵为:
其中,q0、q1、q2、q3为四元素的系数,
令T12=2(q1q2-q0q3),T13=2(q1q3+q0q2),T21=2(q1q2+q0q3),T23=2(q2q3-q0q1),T31=2(q1q3-q0q2),T32=2(q2q3+q0q1),则记
由于n系到b系的旋转过程始终保持直角坐标系,则为正交矩阵:
则计算出自动驾驶汽车的姿态信息:
进一步,步骤S22具体过程为:
定义自动驾驶汽车的左轮的坐标为A(xl,yl),右轮的坐标为B(xr,yr),左轮角速度为wl,右轮的角速度为wr,左右轮的线速度分别为vl、vr,轴心线中点坐标为M(x,y),
通过在汽车两侧轮子安装两个光电编码器,根据编码器输出的脉冲数反解出汽车两轮的行驶距离,设左轮光电编码器在单位时间Δt内的行驶路程为Δsl,右轮光电编码器在单位时间Δt内的行驶路程为Δsr,则左、右轮的线速度为:
其中,ΔNl、ΔNr分别是单位时间Δt内左、右轮光电编码器输出的脉冲数;P是车轮每转一圈输出的脉冲数;D是车轮直径,
则汽车两轮中轴线中心点速度vM为:
设根据惯性解算的自动驾驶汽车当前时刻ti的航向角为θ(ti),则从上一时刻ti-1到当前时刻ti的偏航角Δθ(ti,ti-1)为:
Δθ(ti,ti-1)=Δθ(ti)-Δθ(ti-1) (15)
若上一时刻两轮中轴线中心点的地理位置为(xM(ti-1),yM(ti-1)),则当前时刻自动驾驶汽车所在位置为:
为保证实时性,自动驾驶汽车的两个数据采用点的时间间隔很小,时间间隔接近为0,即:
其中,c为定常值,
则式(16)化简为:
其中,xM(ti),yM(ti)为当前时刻自动驾驶汽车的地理位置坐标。
进一步,步骤S3具体过程为:
设,长时间失效后,RTK当前时刻定位点地理坐标为(x1(ti),y1(ti)),上一时刻定位点地理坐标为(x1(ti-1),y1(ti-1)),且里程计获取的当前时刻到上一时刻两点的路程增量为:
则RTK获取的相邻两点的路程增量为:
其中,RN表示地球经度圈半径,RM表示地球纬度圈半径,
定义惯性和里程计解算的姿态角为θDR(ti,ti-1),RTK解算的姿态角为θRTK(ti,ti-1),当两者解算参数满足下式(21)和(22)时,则认为RTK信号恢复正常,
δ(Δs(ti,ti-1))=|ΔsRTK(ti,ti-1)-ΔsDR(ti,ti-1)|∈(δsmin,δsmax) (21)
δθ(ti,ti-1)=|θRTK(ti,ti-1)-θDR(ti,ti-1)|∈(δθmin,δθmax) (22)
其中,δ(Δs(ti,ti-1))为RTK与惯性和里程计组合导航定位差,δsmin为置信区间最小值,δsmax为置信区间最大值,δθ(ti,ti-1)为RTK与惯性和里程计组合导航姿态误差,δθmin为姿态可信度置信区间最小值,δθmax为姿态可信度置信区间最大值。
本发明的有益效果:
当自动驾驶汽车在复杂环境条件下驾驶时,本发明所述方法可以在信号条件较弱时,对RTK定位是否有效进行判断,如果有效,按RTK技术提供自动驾驶汽车车载导航信息;若无效,采用惯性/里程计航位推算提供自动驾驶汽车行驶过程所需的导航信息。因此,本发明可以在RTK信号受阻的情况下,及时判断出RTK导航的异常,进而及时切换导航参数获取的途径,避免自动驾驶汽车脱离正确行驶的方向。
附图说明
图1为本发明实施例的自动驾驶惯性/里程计/RTK组合导航***中RTK异常值的判断方法总体流程图;
图2为本发明实施例的RTK信号是否异常的判断示意图;
图3为本发明实施例的基于里程计差分原理的自动驾驶汽车航迹推算示意图。
具体实施方式
下面结合附图和实施例进一步描述本发明,应该理解,以下所述实施例旨在便于对本发明的理解,而对其不起任何限定作用。
图1示出了本发明实施例提供的自动驾驶惯性/里程计/RTK组合导航***中RTK异常值的判断方法总体流程图,具体包括如下步骤:
S1:利用惯性/里程计组合导航参数建立判断基准,对自动驾驶车载组合导航中RTK定位精度的有效性进行判断。具体过程如下:
S11:建立惯性/里程计组合导航的速度阈值;
为保证自动驾驶汽车的正常行驶,本实施例要求车载导航定位精度在20cm之内,即可设当前时刻自动驾驶汽车的定位结果的置信区域为半径R=20cm的圆,允许最大定位误差为Δs。可以认为RTK定位误差要求最大不超过20cm,即,Δs≤20cm,当超过该范围时就认定RTK解算位置信息异常。
设RTK解算的自动驾驶汽车的速度为vRTK(t),加速度为aRTK(t)。利用惯性/里程计差分原理解算获得的自动驾驶汽车的速度为vDR(t),加速度为aDR(t),且里程计的分辨率为m=0.01km/h。此时里程计的分辨率造成的速度误差δvDR为:
δvDR≤0.01km/h≈0.003m/s (1)
定义惯性/里程计的数据采样率f=100Hz,RTK数据采样率q=10Hz,且自动驾驶汽车的速度为0~60km/h。以最大速度为例,当自动驾驶汽车的真实速度v(t)为60km/h时,设RTK解算速度误差为δv(t),当信号不稳定造成RTK定位误差最大值为20cm时,RTK解算速度误差δv(t)在理想状态下满足下式:
由于RTK器件存在***误差,现阶段在***误差影响下RTK产品可达到2-3cm的定位精度,极差为5cm。因此,为保证自动驾驶汽车定位精度要求,考虑RTK器件误差条件下,自动驾驶汽车允许的RTK最大速度误差应满足:
因此,惯性/里程计组合导航所解算的速度vDR(t)与RTK解算的速度vRTK(t)满足下式(4)时,可认为RTK解算位置信息准确:
vDR(t)-vRTK(t)≤|δv(t)|+|δvDR|=1.5003m/s (4)
S12:建立惯性/里程计组合导航的加速度阈值;
根据自动驾驶汽车的限制,设定汽车加速度不超过3m/s2。设在自动驾驶汽车定位精度内允许的RTK解算加速度误差为δa(t),则在车速不超过60km/h时,RTK解算加速度误差满足的要求为:
其中,s表示在时间段[ti,ti-1]内的自动驾驶汽车的真实行驶距离。
由于自动驾驶汽车的加速度属于离散数据,因此在采样时间间隔内默认加速度为定值,则对上式(5)整理可得:
|δa(t)|≤15m/s2 (6)
由式(6)可知,当RTK解算的自动驾驶汽车的加速度与惯性/里程计解算的加速度的误差δa(t)在15m/s2之内,则可认为RTK解算位置信息准确。
S13:判断RTK解算位置信息的有效性。
根据步骤S11和步骤S12的分析可知,自动驾驶汽车在行驶过程中,当RTK信号不稳定时,在20cm的定位精度要求下,允许的RTK解算的自动驾驶汽车的速度与惯性/里程计解算的速度的最大误差为1.5003m/s;而允许的RTK解算的自动驾驶汽车的加速度与惯性/里程计解算的加速度的最大误差为15m/s2。而由于无论以加速度还是速度作为RTK解算位置信息是否有效的判断条件,其都是关于时间的函数,且都是以定位精度要求反推的结果,因此,为尽可能保证自动驾驶汽车的定位精度,应同时满足步骤S11和步骤S12,则自动驾驶车载组合导航中RTK定位精度的有效性判断条件为:
|δv(t)|+|δvDR|≤1.5003m/s and|δa(t)|≤15m/s2 and aDR(t)<3m/s2 (7)
S2:当判断自动驾驶车载组合导航中RTK定位精度无效时,航位推算预估自动驾驶汽车的当前位置信息。具体过程如下:
S21:基于惯性/里程计组合导航进行自动驾驶汽车的姿态解算。
定义自动驾驶汽车的坐标系为载体系,记为b系,设自动驾驶汽车的航向角为ψ,俯仰角为θ,横滚角为γ;定义地理坐标系为导航系,即东北天坐标系,记为n系,根据坐标系转换原理计算出导航系与载体系的坐标转换矩阵为:
根据四元素可以解算出载体系(b系)与导航系(n系)的坐标转换矩阵为:
其中,q0、q1、q2、q3为四元素的系数。
令T12=2(q1q2-q0q3),T13=2(q1q3+q0q2),T21=2(q1q2+q0q3),T23=2(q2q3-q0q1),T31=2(q1q3-q0q2),T32=2(q2q3+q0q1),则记
由于n系到b系的旋转过程始终保持直角坐标系,则为正交矩阵:
则可以计算出自动驾驶汽车的姿态信息:
通过上述的解算实现对自动驾驶汽车的姿态解算。
S22:基于里程计位置信息差分原理估计当前时刻自动驾驶汽车的位置。
如图3所示,定义自动驾驶汽车的左轮的坐标为A(xl,yl),右轮的坐标为B(xr,yr),左轮角速度为wl,右轮的角速度为wr,左右轮的线速度分别为vl、vr,轴心线中点坐标为M(x,y),θ1表示单位时间内自动驾驶汽车车轮航向角变化量;利用三角形的性质可知θ2=θ1;θ3=θ1。
里程计的工作原理是通过在汽车两侧轮子安装两个光电编码器,根据编码器输出的脉冲数反解出汽车两轮的行驶距离。设左轮光电编码器在单位时间Δt内的行驶路程为Δsl,右轮光电编码器在单位时间Δt内的行驶路程为Δsr。则左、右轮的线速度为:
其中,ΔNl、ΔNr分别是Δt时间内左、右轮光电编码器输出的脉冲数;P是车轮每转一圈输出的脉冲数;D是车轮直径。
则汽车两轮中轴线中心点速度vM为:
设根据惯性解算的自动驾驶汽车当前时刻ti的航向角为θ(ti),则从上一时刻ti-1到当前时刻ti的偏航角Δθ(ti,ti-1)为:
Δθ(ti,ti-1)=Δθ(ti)-Δθ(ti-1) (15)
若上一时刻两轮中轴线中心点的地理位置为(xM(ti-1),yM(ti-1)),则当前时刻自动驾驶汽车所在位置为:
为保证实时性,自动驾驶汽车的数据更新频率一般很大(在1000kHz),即两个数据采用点的时间间隔很小,时间间隔接近为0。即:
其中,c为定常值。
则式(16)可化简为:
综上所述,根据惯性/里程计航位推算解的当前时刻自动驾驶汽车的地理位置为(xM(ti),yM(ti))。
S3:在自动驾驶汽车RTK解算位置长时间失效后,判断RTK定位精度的有效性。
当RTK解算位置信息长时间无效时,由于惯性/里程计组合导航的误差已经积累很长时间,此时精度已经无法衡量RTK解算位置信息的有效性,为准确分析RTK解算位置信息的有效性,采用步长、航向约束判断方法。
由于RTK在信号充足时,具有定位精度、航向精度高的特点,而里程计长时间发散的原因是由于里程计标度误差造成的,标度误差会随时间累积,且惯性航向误差的累积也间接造成了里程计的误差累积,但其都有短期精度高的特点。本步骤的整体思路为:假设在上一点惯性/里程计定位准确的基础上,到下一点引入的误差只有一倍的里程计标度误差,和微弱的航向误差角,即相邻两点的距离量测信息精度仍非常高,且航向精度高。同理,RTK在信号恢复后,相邻两点的定位误差很小,且航向精度高。
基于上述分析,当RTK解算位置长时间失效后,若判断出RTK定位的相邻两点的距离与惯性/里程计相邻两点的定位满足一定阈值,且航向姿态满足一定阈值时,则认为RTK信号恢复正常。
设,长时间失效后,RTK当前时刻定位点地理坐标为(x1(ti),y1(ti)),上一时刻定位点地理坐标为(x1(ti-1),y1(ti-1)),且里程计获取的当前时刻到上一时刻两点的路程增量为:
则RTK获取的相邻两点的路程增量为:
其中,RN表示地球经度圈半径,RM表示地球纬度圈半径。
定义惯性/里程计解算的姿态角为θDR(ti,ti-1),RTK解算的姿态角为θRTK(ti,ti-1),当两者解算参数满足下式(21)和(22)时,则认为RTK信号恢复正常。
δ(Δs(ti,ti-1))=|ΔsRTK(ti,ti-1)-ΔsDR(ti,ti-1)|∈(δsmin,δsmax) (21)
δθ(ti,ti-1)=|θRTK(ti,ti-1)-θDR(ti,ti-1)|∈(δθmin,δθmax) (22)
其中,δ(Δs(ti,ti-1))为RTK与惯性/里程计组合导航定位差,δsmin为置信区间最小值,δsmax为置信区间最大值,δθ(ti,ti-1)为RTK与惯性/里程计组合导航姿态误差,δθmin为姿态可信度置信区间最小值,δθmax为姿态可信度置信区间最大值。
S4:当判断RTK定位精度有效时,对惯性/里程计组合导航参数进行姿态、位置的矫正,然后利用矫正后的惯性/里程计参数作为新的评价RTK定位精度是否有效的条件。
对于本领域的普通技术人员来说,在不脱离本发明创造构思的前提下,还可以对本发明的实施例作出若干变型和改进,这些都属于本发明的保护范围。
Claims (4)
1.一种自动驾驶组合导航***中RTK异常值的判断方法,其特征在于,包括步骤:
S1:利用惯性和里程计组合导航参数建立判断基准,对自动驾驶车载组合导航中RTK定位精度的有效性进行判断,具体过程如下:
S11:建立惯性和里程计组合导航的速度阈值;
S12:建立惯性和里程计组合导航的加速度阈值;
S13:判断RTK解算位置信息的有效性;
S2:当判断自动驾驶车载组合导航中RTK定位精度无效时,航位推算预估自动驾驶汽车的当前位置信息,具体过程如下:
S21:基于惯性和里程计组合导航进行自动驾驶汽车的姿态解算;
S22:基于里程计位置信息差分原理估计当前时刻自动驾驶汽车的位置;
S3:在自动驾驶汽车RTK解算位置长时间失效后,判断RTK定位精度的有效性;
S4:当判断RTK定位精度有效时,对惯性和里程计组合导航参数进行矫正,然后将矫正后的惯性和里程计组合导航参数作为评价RTK定位精度是否有效的新条件;
步骤S11具体过程为:
定义RTK解算的自动驾驶汽车的速度为vRTK(t),加速度为aRTK(t);利用惯性和里程计差分原理解算获得的自动驾驶汽车的速度为vDR(t),加速度为aDR(t),且里程计的分辨率为m,里程计的分辨率造成的速度误差δvDR为:
δvDR≤m (1)
定义自动驾驶汽车的真实速度为v(t),RTK解算速度误差为δv(t),惯性和里程计的数据采样率为f,RTK数据采样率为q,当信号不稳定造成RTK定位误差最大值为a时,RTK解算速度误差δv(t)在理想状态下满足下式:
为保证自动驾驶汽车定位精度要求,考虑RTK器件误差条件下,自动驾驶汽车允许的RTK最大速度误差满足:
其中,Δs为最大允许定位误差,
利用惯性和里程计差分原理解算获得的自动驾驶汽车的速度vDR(t)与RTK解算的自动驾驶汽车的速度vRTK(t)满足下式(4)时,认为RTK解算位置信息准确:
vDR(t)-vRTK(t)≤|δv(t)|+|δvDRv=w (4)
其中,w为可接受范围内速度解算误差阈值;
步骤S12具体过程为:
定义在自动驾驶汽车定位精度内允许的RTK解算加速度误差为δa(t),在自动驾驶汽车速度不超过最大速度时,RTK解算加速度误差满足:
其中,s表示在时间段[ti,ti-1]内的自动驾驶汽车的真实行驶距离;
在采样时间间隔内默认加速度为定值,则对上式(5)整理得到:
当RTK解算加速度误差δa(t)满足式(6)时,认为RTK解算位置信息准确;
步骤S13中,自动驾驶车载组合导航中RTK解算位置信息的有效性判断条件为:
其中,b为允许自动驾驶汽车行驶过程中的最大加速度。
2.根据权利要求1所述的方法,其特征在于,步骤S21具体过程为:
定义自动驾驶汽车的坐标系为载体系,记为b系,设自动驾驶汽车的航向角为ψ,俯仰角为θ,横滚角为γ;定义地理坐标系为导航系,记为n系,根据坐标系转换原理计算出导航系与载体系的坐标转换矩阵为:
根据四元素解算出载体系与导航系的坐标转换矩阵为:
其中,q0、q1、q2、q3为四元素的系数,
令T12=2(q1q2-q0q3),T13=2(q1q3+q0q2),T21=2(q1q2+q0q3),T23=2(q2q3-q0q1),T31=2(q1q3-q0q2),T32=2(q2q3+q0q1),则记
由于n系到b系的旋转过程始终保持直角坐标系,则为正交矩阵:
则计算出自动驾驶汽车的姿态信息:
3.根据权利要求1所述的方法,其特征在于,步骤S22具体过程为:
定义自动驾驶汽车的左轮的坐标为A(xl,yl),右轮的坐标为B(xr,yr),左轮角速度为wl,右轮的角速度为wr,左右轮的线速度分别为vl、vr,轴心线中点坐标为M(x,y),
通过在汽车两侧轮子安装两个光电编码器,根据编码器输出的脉冲数反解出汽车两轮的行驶距离,设左轮光电编码器在单位时间Δt内的行驶路程为Δsl,右轮光电编码器在单位时间Δt内的行驶路程为Δsr,则左、右轮的线速度为:
其中,ΔNl、ΔNr分别是单位时间Δt内左、右轮光电编码器输出的脉冲数;P是车轮每转一圈输出的脉冲数;D是车轮直径,
则汽车两轮中轴线中心点速度vM为:
设根据惯性解算的自动驾驶汽车当前时刻ti的航向角为θ(ti),则从上一时刻ti-1到当前时刻ti的偏航角Δθ(ti,ti-1)为:
Δθ(ti,ti-1)=Δθ(ti)-Δθ(ti-1) (15)
若上一时刻两轮中轴线中心点的地理位置为(xM(ti-1),yM(ti-1)),则当前时刻自动驾驶汽车所在位置为:
为保证实时性,自动驾驶汽车的两个数据采用点的时间间隔很小,时间间隔接近为0,即:
其中,c为定常值,
则式(16)化简为:
其中,xM(ti),yM(ti)为当前时刻自动驾驶汽车的地理位置坐标。
4.根据权利要求3所述的方法,其特征在于,步骤S3具体过程为:
设,长时间失效后,RTK当前时刻定位点地理坐标为(x1(ti),y1(ti)),上一时刻定位点地理坐标为(x1(ti-1),y1(ti-1)),且里程计获取的当前时刻到上一时刻两点的路程增量为:
则RTK获取的相邻两点的路程增量为:
其中,RN表示地球经度圈半径,RM表示地球纬度圈半径,
定义惯性和里程计解算的姿态角为θDR(ti,ti-1),RTK解算的姿态角为θRTK(ti,ti-1),当两者解算参数满足下式(21)和(22)时,则认为RTK信号恢复正常,
δ(Δs(ti,ti-1))=|ΔsRTK(ti,ti-1)-ΔsDR(ti,ti-1)|∈(δsmin,δsmax) (21)
δθ(ti,ti-1)=|θRTK(ti,ti-1)-θDR(ti,ti-1)|∈(δθmin,δθmax) (22)
其中,δ(Δs(ti,ti-1))为RTK与惯性和里程计组合导航定位差,δsmin为置信区间最小值,δsmax为置信区间最大值,δθ(ti,ti-1)为RTK与惯性和里程计组合导航姿态误差,δθmin为姿态可信度置信区间最小值,δθmax为姿态可信度置信区间最大值。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202011471789.0A CN112731483B (zh) | 2020-12-14 | 2020-12-14 | 一种自动驾驶组合导航***中rtk异常值的判断方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202011471789.0A CN112731483B (zh) | 2020-12-14 | 2020-12-14 | 一种自动驾驶组合导航***中rtk异常值的判断方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN112731483A CN112731483A (zh) | 2021-04-30 |
CN112731483B true CN112731483B (zh) | 2024-04-09 |
Family
ID=75601995
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202011471789.0A Active CN112731483B (zh) | 2020-12-14 | 2020-12-14 | 一种自动驾驶组合导航***中rtk异常值的判断方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN112731483B (zh) |
Families Citing this family (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114485725B (zh) * | 2021-12-22 | 2024-05-24 | 深圳元戎启行科技有限公司 | 数据异常检测方法、自动驾驶平台以及计算机可读存储介质 |
CN114475655B (zh) * | 2022-01-29 | 2024-05-03 | 智道网联科技(北京)有限公司 | 自动驾驶的预警方法、装置和计算机可读存储介质 |
CN115079619B (zh) * | 2022-07-27 | 2022-11-15 | 智道网联科技(北京)有限公司 | 用于v2x设备的电路、控制方法、v2x设备及车辆 |
CN117722997B (zh) * | 2024-02-07 | 2024-04-23 | 哈尔滨工业大学 | 一种结构厚度连续手动超声成像测量方法及扫查装置 |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
KR20060102016A (ko) * | 2005-03-22 | 2006-09-27 | 현대자동차주식회사 | 차량 항법장치의 관성센서 교정장치 및 방법 |
CN110221333A (zh) * | 2019-04-11 | 2019-09-10 | 同济大学 | 一种车载ins/od组合导航***的量测误差补偿方法 |
CN110631574A (zh) * | 2018-06-22 | 2019-12-31 | 北京自动化控制设备研究所 | 一种惯性/里程计/rtk多信息融合方法 |
CN110779521A (zh) * | 2019-11-12 | 2020-02-11 | 成都中科微信息技术研究院有限公司 | 一种多源融合的高精度定位方法与装置 |
Family Cites Families (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
FR2878954B1 (fr) * | 2004-12-07 | 2007-03-30 | Sagem | Systeme de navigation inertielle hybride base sur un modele cinematique |
US20080071476A1 (en) * | 2006-09-19 | 2008-03-20 | Takayuki Hoshizaki | Vehicle dynamics conditioning method on MEMS based integrated INS/GPS vehicle navigation system |
-
2020
- 2020-12-14 CN CN202011471789.0A patent/CN112731483B/zh active Active
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
KR20060102016A (ko) * | 2005-03-22 | 2006-09-27 | 현대자동차주식회사 | 차량 항법장치의 관성센서 교정장치 및 방법 |
CN110631574A (zh) * | 2018-06-22 | 2019-12-31 | 北京自动化控制设备研究所 | 一种惯性/里程计/rtk多信息融合方法 |
CN110221333A (zh) * | 2019-04-11 | 2019-09-10 | 同济大学 | 一种车载ins/od组合导航***的量测误差补偿方法 |
CN110779521A (zh) * | 2019-11-12 | 2020-02-11 | 成都中科微信息技术研究院有限公司 | 一种多源融合的高精度定位方法与装置 |
Non-Patent Citations (3)
Title |
---|
"Integrity monitoring for Positioning of intelligent transport systems using integrated RTK-GNSS, IMU and vehicle odometer";Ahmed El-Mowafy;《IET Intelligent Transport Systems》;1-8 * |
"弹载大视场星惯组合深度融合导航技术";宋凝芳等;《航空学报》;第41卷(第8期);1-12 * |
"捷联惯导/里程计组合导航方法";张小跃等;《北京航空航天大学学报》;第39卷(第7期);922-926 * |
Also Published As
Publication number | Publication date |
---|---|
CN112731483A (zh) | 2021-04-30 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN112731483B (zh) | 一种自动驾驶组合导航***中rtk异常值的判断方法 | |
EP2519803B1 (en) | Technique for calibrating dead reckoning positioning data | |
CN111156994B (zh) | 一种基于mems惯性组件的ins/dr&gnss松组合导航方法 | |
CN100578153C (zh) | 车载速度测量仪的校准方法 | |
CN103033184B (zh) | 用于惯性导航***的纠错方法、装置和*** | |
CN101201255A (zh) | 基于智能导航算法的车辆组合导航*** | |
CN101556160B (zh) | 可车速信号自学习的车载导航***及方法 | |
CN104197958B (zh) | 一种基于激光测速仪航位推算***的里程计标定方法 | |
CN104215262A (zh) | 一种惯性导航***惯性传感器误差在线动态辨识方法 | |
CN109470276B (zh) | 基于零速修正的里程计标定方法与装置 | |
CN115060257B (zh) | 一种基于民用级惯性测量单元的车辆变道检测方法 | |
CN114136310B (zh) | 一种惯性导航***误差自主抑制***及方法 | |
CN114935345A (zh) | 一种基于模式识别的车载惯导安装角误差补偿方法 | |
CN113063441B (zh) | 里程计累计推算误差的数据源纠正方法及装置 | |
CN106646569A (zh) | 一种导航定位方法及设备 | |
CN101545781A (zh) | 车载组合导航中里程计脉冲当量确定方法 | |
CN110987023B (zh) | 一种惯性导航动态对准方法 | |
CN114111792B (zh) | 一种车载gnss/ins/里程计组合导航方法 | |
CN114543793B (zh) | 车辆导航***的多传感器融合定位方法 | |
CN115164935A (zh) | 一种惯性导航标定设备的控制装置及其控制方法 | |
CN114370885A (zh) | 一种惯性导航***误差补偿方法及*** | |
CN114379577A (zh) | 行驶轨迹生成方法及设备 | |
CN111665530A (zh) | 一种基于车辆状态的gps诊断方法 | |
CN109737954A (zh) | 一种基于车速和角速度的惯性导航方法 | |
CN116625360B (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 |