CN116448146A - 一种惯性导航***自标定方法、装置、设备及存储介质 - Google Patents
一种惯性导航***自标定方法、装置、设备及存储介质 Download PDFInfo
- Publication number
- CN116448146A CN116448146A CN202310289342.9A CN202310289342A CN116448146A CN 116448146 A CN116448146 A CN 116448146A CN 202310289342 A CN202310289342 A CN 202310289342A CN 116448146 A CN116448146 A CN 116448146A
- Authority
- CN
- China
- Prior art keywords
- vehicle
- forward direction
- coordinate system
- navigation system
- running
- 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 92
- 238000005259 measurement Methods 0.000 claims abstract description 34
- 230000006870 function Effects 0.000 claims description 12
- 238000004364 calculation method Methods 0.000 claims description 11
- 238000004590 computer program Methods 0.000 claims description 6
- 230000008569 process Effects 0.000 abstract description 16
- 238000012937 correction Methods 0.000 abstract description 5
- 238000009434 installation Methods 0.000 description 11
- 238000004422 calculation algorithm Methods 0.000 description 9
- 230000009466 transformation Effects 0.000 description 4
- 230000008859 change Effects 0.000 description 3
- 230000000694 effects Effects 0.000 description 3
- 238000010586 diagram Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 238000007726 management method Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000002093 peripheral effect Effects 0.000 description 2
- 238000004458 analytical method Methods 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000006073 displacement reaction Methods 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 239000007787 solid Substances 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
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation 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/40—Correcting position, velocity or attitude
-
- 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)
- Manufacturing & Machinery (AREA)
- Automation & Control Theory (AREA)
- Navigation (AREA)
Abstract
本申请实施例涉及自动驾驶技术领域,特别涉及一种惯性导航***自标定方法、装置、设备及存储介质,该方法包括:获得并判断车辆行驶情况;若车辆行驶情况为直线行驶,全球卫星导航***的轨迹切线方向为车辆坐标系向前的方向;若车辆行驶情况为任意行驶,则基于车辆在局部坐标系下的位置坐标,得到车辆坐标系向前的方向;基于惯性测量单元输出的前向方向角以及车辆坐标系向前的方向,得到待标定偏置角。本申请实施例提供的惯性导航***自标定方法,能够解决在使用过程中外参校正需求,且无需设置特定场景,在普通道路场景中,不限制车辆行驶路线和道路特征,正常驾驶过程中即可完成标定。
Description
技术领域
本申请实施例涉及自动驾驶技术领域,特别涉及一种惯性导航***自标定方法、装置、设备及存储介质。
背景技术
驾驶***是一个复杂的多模块协作***,其中传感器是车辆完成各项任务的基础,多个传感器的使用和性能直接决定自动驾驶车辆的安全性和可行性。大多数传感器在使用前需要标定内部参数与外部参数,内参描述了传感器内部的映射关系,外参描述了不同传感器和车辆、世界坐标系之间的转换关系。
由于内参在传感器制造完成后变化较小,基本上在出厂前完成标定,采用基于人工目标的实验室标定方法。而外参会在安装时受到工艺和操作的影响,在使用过程中由于车辆振动等影响会发生变化,因此对实验室标定和在线标定都有需求。
对于惯性导航***的标定方法可分为实验标定和在线标定两种。通常,实验室标定使用一些测距仪器来标定传感器与车体的位移,使用GNSS输出的方向和已知车辆方向来测量惯性导航***与车体的角度。而在线标定方法通常使用手眼标定,即通过求解多个时刻下惯性导航***坐标系的变换和目标坐标系的变换,构成两者之间的外参约束,用滤波器或非线性优化方法求解。这类方法多用于标定惯性导航***与其他传感器之间的外参,如激光雷达,然而,与车辆的外参标定的方法较少。其他在线标定方法对道路场景和车辆行驶方式有特殊要求。目前对于传感器外参的在线标定方法大多关注不同传感器之间的标定,缺少单个传感器与车体坐标系的外参在线标定方法,即缺少传感器自标定方法,而传感器与车体外参中的旋转部分会对后续多个自动驾驶任务产生影响。
发明内容
本申请实施例提供一种惯性导航***自标定方法、装置、设备及存储介质,能够解决在使用过程中外参校正需求,且无需设置特定场景,在普通道路场景中,不限制车辆行驶路线和道路特征,正常驾驶过程中即可完成标定。
为解决上述技术问题,第一方面,本申请实施例提供一种惯性导航***自标定方法,包括以下步骤:获得车辆行驶情况,车辆行驶情况包括直线行驶和任意行驶;判断车辆行驶情况,并基于车辆行驶情况,得到车辆坐标系向前的方向;若车辆行驶情况为直线行驶,则将车辆位置的坐标拟合为直线;直线的方向为车辆的向前的方向,全球卫星导航***的轨迹切线方向为车辆坐标系向前的方向;若车辆行驶情况为任意行驶,则基于车辆在局部坐标系下的位置坐标,得到车辆位置相对于时间的导数;并基于车辆位置相对于时间的导数,得到车辆坐标系向前的方向;基于惯性测量单元输出的前向方向角以及车辆坐标系向前的方向,得到待标定偏置角。
在一些示例性实施例中,惯性导航***包括全球卫星导航***和惯性测量单元;车辆位置的坐标由全球卫星导航***输出;前向方向角为惯性导航***向前的方向,前向方向角由惯性测量单元输出。
在一些示例性实施例中,若车辆行驶情况为直线行驶,待标定偏置角表示为:
其中,ψi为惯性测量单元测量到的前向方向角;为全球卫星导航***的轨迹切线方向;S为所有直线行驶的数据点集合。
在一些示例性实施例中,若车辆行驶情况为任意行驶,则基于车辆在局部坐标系下的位置坐标,得到车辆位置相对于时间的导数;并基于车辆位置相对于时间的导数,得到车辆坐标系向前的方向,包括:将全球卫星导航***的输出转换为车辆在局部坐标系下的位置坐标,所述位置坐标表示为{xi,yi,zi};采用插值法从离散的x,y位置拟合得到x,y相对于时间的连续函数,并得到x,y相对于时间的导数;基于x,y相对于时间的导数,得到车辆坐标系向前的方向。
在一些示例性实施例中,x,y相对于时间的连续函数分别表示为:
x(t)=Bspline({xi},{ti})
y(t)=Bspline({yi},{ti})
其中,x(t),y(t)分别为x,y相对于时间t的连续函数,Bspline为B样条插值法;
x,y相对于时间的导数表示为:
其中,x′i,y′i分别为x,y相对于时间t的导数,x′i,y′i分别表示车辆在x,y方向的分速度方向;
车辆坐标系向前的方向表示为:
其中,为车辆坐标系向前的方向,表示车辆的速度方向。
在一些示例性实施例中,若车辆行驶情况为任意行驶,所述待标定偏置角表示为:
其中,ψi为惯性测量单元测量到的前向方向角;为在各个时刻,车辆坐标系向前的方向的角度与惯性测量单元测量到的前向方向角之间的差值的平均值;S为过滤后的任意行驶的数据点集合。
在一些示例性实施例中,通过去除车辆运动速度小于预设速度的数据以及车辆行驶过程转弯角度大于预设角度的数据,得到过滤后的任意行驶的数据点集合。
第二方面,本申请实施例还提供了一种惯性导航***自标定装置,包括依次连接的行驶情况获取模块、判断模块以及计算模块;行驶情况获取模块用于获得车辆行驶情况,车辆行驶情况包括直线行驶和任意行驶;判断模块用于判断车辆行驶情况;计算模块用于根据车辆行驶情况,得到车辆坐标系向前的方向,并根据惯性测量单元输出的前向方向角以及所述车辆坐标系向前的方向,得到待标定偏置角;其中,在计算车辆坐标系向前的方向时,若车辆行驶情况为直线行驶,则将车辆位置的坐标拟合为直线;直线的方向为车辆的向前的方向,全球卫星导航***的轨迹切线方向为车辆坐标系向前的方向;若车辆行驶情况为任意行驶,则基于车辆在局部坐标系下的位置坐标,得到车辆位置相对于时间的导数;并基于车辆位置相对于时间的导数,得到车辆坐标系向前的方向。
另外,本申请还提供了一种电子设备,包括:至少一个处理器;以及,与至少一个处理器通信连接的存储器;其中,存储器存储有可被至少一个处理器执行的指令,指令被所述至少一个处理器执行,以使所述至少一个处理器能够执行上述惯性导航***自标定方法。
另外,本申请还提供了一种计算机可读存储介质,存储有计算机程序,所述计算机程序被处理器执行时实现上述惯性导航***自标定方法。
本申请实施例提供的技术方案至少具有以下优点:
本申请实施例提供一种惯性导航***自标定方法、装置、设备及存储介质,该方法包括以下步骤:获得车辆行驶情况,车辆行驶情况包括直线行驶和任意行驶;判断车辆行驶情况,并基于车辆行驶情况,得到车辆坐标系向前的方向;若车辆行驶情况为直线行驶,则将车辆位置的坐标拟合为直线;直线的方向为车辆的向前的方向,全球卫星导航***的轨迹切线方向为车辆坐标系向前的方向;若车辆行驶情况为任意行驶,则基于车辆在局部坐标系下的位置坐标,得到车辆位置相对于时间的导数;并基于车辆位置相对于时间的导数,得到车辆坐标系向前的方向;基于惯性测量单元输出的前向方向角以及车辆坐标系向前的方向,得到待标定偏置角。
本申请提出一种全新的惯性导航***自标定算法,能够在车辆运行过程中自动标定惯性导航***到车辆的偏置角(偏航角),本申请的自标定方法避免了精确的安装要求,能够修正安装不当导致的误差,不依赖特定的环境和标识物,只需要在道路上正常行驶就可以完成自标定。本申请提供的惯性导航***自标定方法针对两种行驶情况进行设计:对于直线行驶情况使用随机采样一致(Random Sample Consensus,RANSAC)算法拟合直线标定;任意行驶情况使用B样条(B-spline)插值法拟合轨迹进行标定,最后计算得出车辆坐标系向前的方向(轨迹切线方向)与惯性测量单元输出的前向方向角的差值,得到最终的标定结果。本申请提供的惯性导航***自标定方法是一种在线标定方法,能够解决在使用过程中外参校正需求,且无需设置特定场景,在普通道路场景中,不限制车辆行驶路线和道路特征,正常驾驶过程中即可完成标定。
附图说明
一个或多个实施例通过与之对应的附图中的图片进行示例性说明,这些示例性说明并不构成对实施例的限定,除非有特别申明,附图中的图不构成比例限制。
图1为本申请一实施例提供的一种惯性导航***自标定方法的流程示意图;
图2为本申请一实施例提供的B样条插值前后的惯性测量单元与车辆的在世界坐标系下的偏置角;
图3为本申请一实施例提供的一种惯性导航***自标定装置的模块示意图;
图4为本申请一实施例提供的一种电子设备的结构示意图。
具体实施方式
由背景技术可知,目前现有的在线标定方法存在着惯性导航***与车辆的外参标定的方法较少,且其他在线标定方法对道路场景和车辆行驶方式有特殊要求的问题。
当前,自动驾驶是工业界和学术界的关注热点之一。自动驾驶***依赖于各类传感器对环境的感知,以实现下游决策,规划,控制等任务。传感器在使用前首先要标定其内部参数和外部参数,为后续建图、定位、感知和控制打下坚实基础。标定是自动驾驶***稳定运行的核心部分与前置条件。标定的精度会影响传感器的使用上限精度,最终影响自动驾驶各个功能的实现效果。
现有标定技术一般分两种,实验室标定和在线标定。实验室标定利用特定环境进行标定,适用于传感器安装时的初始标定,无法解决在长时间使用过程中外参由于碰撞,振动等因素出现的误差。然而,对于惯性导航***的在线标定方法中,手眼标定一般用于标定与其他传感器的外参,缺少与车体坐标系的外参标定。其他在线标定方法对道路场景和车辆行驶方式有特殊要求。
传感器外参由于存在初始安装误差,使用中容易因碰撞,振动等原因发生变化,因此需要在线标定算法。由背景技术可知:目前对于传感器外参的在线标定方法大多关注不同传感器之间的标定,缺少单个传感器与车体坐标系的外参在线标定方法,即传感器自标定方法,而传感器与车体外参中的旋转部分会对后续多个自动驾驶任务产生影响。目前,惯性导航***与车辆的外参标定的方法较少,有相关方法通过多次刹车测量比力,根据比力方向计算车辆前进方向,以估计惯性导航***与车辆坐标系的前向安装角,该方法要求车辆在平坦无角度的地面上直线行驶,要求较为严苛。
为解决上述技术问题,本申请实施例提供一种惯性导航***自标定方法,包括以下步骤:获得车辆行驶情况,车辆行驶情况包括直线行驶和任意行驶;判断车辆行驶情况,并基于车辆行驶情况,得到车辆坐标系向前的方向;若车辆行驶情况为直线行驶,则将车辆位置的坐标拟合为直线;直线的方向为车辆的向前的方向,全球卫星导航***的轨迹切线方向为车辆坐标系向前的方向;若车辆行驶情况为任意行驶,则基于车辆在局部坐标系下的位置坐标,得到车辆位置相对于时间的导数;并基于车辆位置相对于时间的导数,得到车辆坐标系向前的方向;基于惯性测量单元输出的前向方向角以及车辆坐标系向前的方向,得到待标定偏置角。本申请提供的惯性导航***自标定方法,能够解决在使用过程中外参校正需求,且无需设置特定场景,在普通道路场景中,不限制车辆行驶路线和道路特征,正常驾驶过程中即可完成标定。
下面将结合附图对本申请的各实施例进行详细的阐述。然而,本领域的普通技术人员可以理解,在本申请各实施例中,为了使读者更好地理解本申请而提出了许多技术细节。但是,即使没有这些技术细节和基于以下各实施例的种种变化和修改,也可以实现本申请所要求保护的技术方案。
参看图1,本申请实施例提供了一种惯性导航***自标定方法,包括以下步骤:
步骤S1、获得车辆行驶情况,车辆行驶情况包括直线行驶和任意行驶。
步骤S2、判断车辆行驶情况,并基于车辆行驶情况,得到车辆坐标系向前的方向。
步骤S201、若车辆行驶情况为直线行驶,则将车辆位置的坐标拟合为直线;直线的方向为车辆的向前的方向,全球卫星导航***的轨迹切线方向为车辆坐标系向前的方向。
步骤S202、若车辆行驶情况为任意行驶,则基于车辆在局部坐标系下的位置坐标,得到车辆位置相对于时间的导数;并基于车辆位置相对于时间的导数,得到车辆坐标系向前的方向。
步骤S3、基于惯性测量单元输出的前向方向角以及车辆坐标系向前的方向,得到待标定偏置角。
本申请提出采用车辆坐标系向前的方向角度(车辆轨迹切线角度)与惯性测量单元IMU角度差值计算偏置角(yaw角)的思路。首先,步骤S1需要获得车辆行驶情况,然后执行步骤S2根据车辆行驶情况,得到车辆坐标系向前的方向。具体的,步骤S2包括两种情况,若车辆行驶情况为直线行驶,则执行步骤S201;若车辆行驶情况为任意行驶,则执行步骤S202。在得到车辆坐标系向前的方向之后,执行步骤S3,结合惯性测量单元测量得到的前向方向角,最终计算得到待标定偏置角。
本申请提出了一种全新的惯性导航***自标定算法,能够在车辆运行过程中自动标定惯性导航***到车辆的前向安装角yaw。该算法可以在不依赖人工环境,不需要特定车辆行驶要求的条件下完成对惯性导航***与车体坐标系的前向安装角yaw的标定。同时,该算法避免了精确的安装要求,能够修正安装不当导致的误差,不依赖特定的环境和标识物,只需要在道路上正常行驶就可以完成自标定。
在一些实施例中,惯性导航***包括全球卫星导航***和惯性测量单元;车辆位置的坐标由全球卫星导航***输出;前向方向角为惯性导航***向前的方向,前向方向角由惯性测量单元输出。
具体的,惯性导航***是目前自动驾驶***中普遍使用的一种传感器,惯性导航***包括全球卫星导航***(Global Navigation Satellite System,GNSS)和惯性测量单元(Inertial Measurement Unit,IMU)。本申请通过设计一种对惯性导航***的外参自标定方案,在线标定其前向安装角,即惯导坐标系与车体坐标系的偏置角(yaw角)。
本申请提供的惯性导航***自标定方法分为两种情况:对于直线行驶情况使用随机采样一致(Random Sample Consensus,RANSAC)算法拟合轨迹;对于曲线行驶情况使用B样条(B-spline)插值拟合轨迹,最后都计算轨迹切线方向与IMU得到的方向的差值得到最终的标定结果。
下面对直线行驶情况下的惯性导航***自标定方法进行详细说明。
在一些实施例中,若车辆行驶情况为直线行驶,待标定偏置角表示为:
其中,ψi为惯性测量单元测量到的前向方向角;为全球卫星导航***的轨迹切线方向;S为所有直线行驶的数据点集合。
当车辆已知直线行驶时,使用RANSAC算法将全球卫星导航***GNSS输出的车辆位置(x,y)拟合为一条直线,该直线的方向即为车辆的向前的方向。将惯性测量单元IMU测量到的前向方向角(绕z轴)用ψi表示,将GNSS轨迹切线方向用表示,这两个方向的差值即为待标定偏置角。
下面对任意行驶情况下的惯性导航***自标定方法进行详细说明。
在一些实施例中,若车辆行驶情况为任意行驶,则基于车辆在局部坐标系下的位置坐标,得到车辆位置相对于时间的导数;并基于车辆位置相对于时间的导数,得到车辆坐标系向前的方向,包括:将全球卫星导航***的输出转换为车辆在局部坐标系下的位置坐标,所述位置坐标表示为{xi,yi,zi};采用插值法从离散的x,y位置拟合得到x,y相对于时间的连续函数,并得到x,y相对于时间的导数;基于x,y相对于时间的导数,得到车辆坐标系向前的方向。
在一些实施例中,x,y相对于时间的连续函数分别表示为:
x(t)=Bspline({xi},{ti})
y(t)=Bspline({yi},{ti})
其中,x(t),y(t)分别为x,y相对于时间t的连续函数,Bspline为B样条插值法;
x,y相对于时间的导数表示为:
其中,x′i,y′i分别为x,y相对于时间t的导数,x′i,y′i分别表示车辆在x,y方向的分速度方向;
x′i,y′i相当于车辆在x,y方向的分速度方向,因此可以计算得到车辆前进的速度方向。
车辆坐标系向前的方向表示为:
其中,为车辆坐标系向前的方向,表示车辆的速度方向。
车辆的速度方向可以认为是车辆坐标系向前的方向,而惯性测量单元输出的角度是惯性导航***向前的方向,因此这两个方向的差值就是我们需要标定的车辆与惯性导航***之间的yaw角:
整体上,本方法取各个时刻与ψi的差值的平均值作为最终估计的yaw角。
具体的,在一些实施例中,若车辆行驶情况为任意行驶,所述待标定偏置角表示为:
其中,ψi为惯性测量单元测量到的前向方向角;为在各个时刻,车辆坐标系向前的方向的角度与惯性测量单元测量到的前向方向角之间的差值的平均值;S为过滤后的任意行驶的数据点集合。
在一些实施例中,通过去除车辆运动速度小于预设速度的数据以及车辆行驶过程转弯角度大于预设角度的数据,得到过滤后的任意行驶的数据点集合,然后通过车辆行驶情况为任意行驶下的待标定偏置角的公式计算得到待标定偏置角。
需要说明的是,为了获得更好的效果,需要过滤掉一些角度计算不准确的数据点,一是去除运动速度过小的点,即保证(x′i)2+(y′i)2>vth;二是删除车辆行驶过程转弯角度过大的点。本申请通过预设一个车辆运动速度数值以及预设一个车辆行驶过程转弯角度数值,将车辆运动速度小于预设速度的数据去除,并将车辆行驶过程转弯角度大于预设角度的数据去除,最终得到过滤后的任意行驶的数据点集合S。
算法的运行效果如图2所示:在B样条拟合前yaw角的波动幅度较大,在拟合后曲线更加平滑。且车辆的yaw角与IMU的yaw角有一个保持一定的偏置,符合本申请分析的情况。
参看图3,本申请实施例还提供了一种惯性导航***自标定装置,包括依次连接的行驶情况获取模块101、判断模块102以及计算模块103;行驶情况获取模块101用于获得车辆行驶情况,车辆行驶情况包括直线行驶和任意行驶;判断模块102用于判断车辆行驶情况;计算模块103用于根据车辆行驶情况,得到车辆坐标系向前的方向,并根据惯性测量单元输出的前向方向角以及所述车辆坐标系向前的方向,得到待标定偏置角;其中,计算模块103包括直线行驶计算模块1031和任意行驶计算模块1032,直线行驶计算模块1031用于在车辆行驶情况为直线行驶时,计算车辆坐标系向前的方向;具体的,将车辆位置的坐标拟合为直线;直线的方向为车辆的向前的方向,全球卫星导航***的轨迹切线方向为车辆坐标系向前的方向;任意行驶计算模块1032用于在车辆行驶情况为任意行驶时,计算车辆坐标系向前的方向;若车辆行驶情况为任意行驶,则基于车辆在局部坐标系下的位置坐标,得到车辆位置相对于时间的导数;并基于车辆位置相对于时间的导数,得到车辆坐标系向前的方向。
参看图4,本申请另一实施例提供了一种电子设备,包括:至少一个处理器110;以及,与至少一个处理器通信连接的存储器111;其中,存储器111存储有可被至少一个处理器110执行的指令,指令被至少一个处理器110执行,以使至少一个处理器110能够执行上述任一方法实施例。
其中,存储器111和处理器110采用总线方式连接,总线可以包括任意数量的互联的总线和桥,总线将一个或多个处理器110和存储器111的各种电路连接在一起。总线还可以将诸如***设备、稳压器和功率管理电路等之类的各种其他电路连接在一起,这些都是本领域所公知的,因此,本文不再对其进行进一步描述。总线接口在总线和收发机之间提供接口。收发机可以是一个元件,也可以是多个元件,比如多个接收器和发送器,提供用于在传输介质上与各种其他装置通信的单元。经处理器110处理的数据通过天线在无线介质上进行传输,进一步,天线还接收数据并将数据传送给处理器110。
处理器110负责管理总线和通常的处理,还可以提供各种功能,包括定时,***接口,电压调节、电源管理以及其他控制功能。而存储器111可以被用于存储处理器110在执行操作时所使用的数据。
本申请另一实施例涉及一种计算机可读存储介质,存储有计算机程序。计算机程序被处理器执行时实现上述方法实施例。
即,本领域技术人员可以理解,实现上述实施例方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成,该程序存储在一个存储介质中,包括若干指令用以使得一个设备(可以是单片机,芯片等)或处理器(processor)执行本申请各个实施例上述方法的全部或部分步骤。而前述的存储介质包括:U盘、移动硬盘、只读存储器(ROM,Read-OnlyMemory)、随机存取存储器(RAM,Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。
由以上技术方案,本申请实施例提供一种惯性导航***自标定方法、装置、设备及存储介质,该方法包括以下步骤:获得车辆行驶情况,车辆行驶情况包括直线行驶和任意行驶;判断车辆行驶情况,并基于车辆行驶情况,得到车辆坐标系向前的方向;若车辆行驶情况为直线行驶,则将车辆位置的坐标拟合为直线;直线的方向为车辆的向前的方向,全球卫星导航***的轨迹切线方向为车辆坐标系向前的方向;若车辆行驶情况为任意行驶,则基于车辆在局部坐标系下的位置坐标,得到车辆位置相对于时间的导数;并基于车辆位置相对于时间的导数,得到车辆坐标系向前的方向;基于惯性测量单元输出的前向方向角以及车辆坐标系向前的方向,得到待标定偏置角。
本申请提出一种全新的惯性导航***自标定算法,能够在车辆运行过程中自动标定惯性导航***到车辆的偏置角(偏航角),本申请的自标定方法避免了精确的安装要求,能够修正安装不当导致的误差,不依赖特定的环境和标识物,只需要在道路上正常行驶就可以完成自标定。本申请提供的惯性导航***自标定方法针对两种行驶情况进行设计:对于直线行驶情况使用RANSAC算法拟合直线标定;任意行驶情况使用B样条插值法拟合轨迹进行标定,最后计算得出车辆坐标系向前的方向(轨迹切线方向)与惯性测量单元IMU输出的前向方向角的差值,得到最终的标定结果。本申请提供的惯性导航***自标定方法是一种在线标定方法,能够解决在使用过程中外参校正需求,且无需设置特定场景,在普通道路场景中,不限制车辆行驶路线和道路特征,正常驾驶过程中即可完成标定。
本领域的普通技术人员可以理解,上述各实施方式是实现本申请的具体实施例,而在实际应用中,可以在形式上和细节上对其作各种改变,而不偏离本申请的精神和范围。任何本领域技术人员,在不脱离本申请的精神和范围内,均可作各自更动与修改,因此本申请的保护范围应当以权利要求限定的范围为准。
Claims (10)
1.一种惯性导航***自标定方法,其特征在于,包括:
获得车辆行驶情况,所述车辆行驶情况包括直线行驶和任意行驶;
判断所述车辆行驶情况,并基于所述车辆行驶情况,得到车辆坐标系向前的方向;
若所述车辆行驶情况为直线行驶,则将车辆位置的坐标拟合为直线;所述直线的方向为车辆的向前的方向,全球卫星导航***的轨迹切线方向为车辆坐标系向前的方向;
若所述车辆行驶情况为任意行驶,则基于车辆在局部坐标系下的位置坐标,得到车辆位置相对于时间的导数;并基于所述车辆位置相对于时间的导数,得到车辆坐标系向前的方向;
基于惯性测量单元输出的前向方向角以及所述车辆坐标系向前的方向,得到待标定偏置角。
2.根据权利要求1所述的惯性导航***自标定方法,其特征在于,惯性导航***包括全球卫星导航***和惯性测量单元;
所述车辆位置的坐标由所述全球卫星导航***输出;
所述前向方向角为惯性导航***向前的方向,所述前向方向角由所述惯性测量单元输出。
3.根据权利要求1所述的惯性导航***自标定方法,其特征在于,若所述车辆行驶情况为直线行驶,所述待标定偏置角表示为:
其中,ψi为惯性测量单元测量到的前向方向角;为全球卫星导航***的轨迹切线方向;S为所有直线行驶的数据点集合。
4.根据权利要求1所述的惯性导航***自标定方法,其特征在于,若所述车辆行驶情况为任意行驶,则基于车辆在局部坐标系下的位置坐标,得到车辆位置相对于时间的导数;
并基于所述车辆位置相对于时间的导数,得到车辆坐标系向前的方向,包括:
将全球卫星导航***的输出转换为车辆在局部坐标系下的位置坐标,所述位置坐标表示为{xi,yi,zi};
采用插值法从离散的x,y位置拟合得到x,y相对于时间的连续函数,并得到x,y相对于时间的导数;
基于x,y相对于时间的导数,得到车辆坐标系向前的方向。
5.根据权利要求4所述的惯性导航***自标定方法,其特征在于,x,y相对于时间的连续函数分别表示为:
x(t)=Bspline({xi},{ti})
y(t)=Bspline({yi},{ti})
其中,x(t),y(t)分别为x,y相对于时间t的连续函数,Bspline为B样条插值法;
x,y相对于时间的导数表示为:
其中,x′i,y′i分别为x,y相对于时间t的导数,x′i,y′i分别表示车辆在x,y方向的分速度方向;
所述车辆坐标系向前的方向表示为:
其中,为车辆坐标系向前的方向,表示车辆的速度方向。
6.根据权利要求1所述的惯性导航***自标定方法,其特征在于,若所述车辆行驶情况为任意行驶,所述待标定偏置角表示为:
其中,ψi为惯性测量单元测量到的前向方向角;为在各个时刻,车辆坐标系向前的方向的角度与惯性测量单元测量到的前向方向角之间的差值的平均值;S为过滤后的任意行驶的数据点集合。
7.根据权利要求6所述的惯性导航***自标定方法,其特征在于,通过去除车辆运动速度小于预设速度的数据以及车辆行驶过程转弯角度大于预设角度的数据,得到过滤后的任意行驶的数据点集合。
8.一种惯性导航***自标定装置,其特征在于,包括依次连接的行驶情况获取模块、判断模块以及计算模块;
所述行驶情况获取模块用于获得车辆行驶情况,所述车辆行驶情况包括直线行驶和任意行驶;
所述判断模块用于判断所述车辆行驶情况;
所述计算模块用于根据所述车辆行驶情况,得到车辆坐标系向前的方向,并根据惯性测量单元输出的前向方向角以及所述车辆坐标系向前的方向,得到待标定偏置角;
其中,在计算车辆坐标系向前的方向时,若所述车辆行驶情况为直线行驶,则将车辆位置的坐标拟合为直线;所述直线的方向为车辆的向前的方向,全球卫星导航***的轨迹切线方向为车辆坐标系向前的方向;若所述车辆行驶情况为任意行驶,则基于车辆在局部坐标系下的位置坐标,得到车辆位置相对于时间的导数;并基于所述车辆位置相对于时间的导数,得到车辆坐标系向前的方向。
9.一种电子设备,其特征在于,包括:
至少一个处理器;以及,
与所述至少一个处理器通信连接的存储器;其中,所述存储器存储有可被所述至少一个处理器执行的指令,所述指令被所述至少一个处理器执行,以使所述至少一个处理器能够执行如权利要求1至7中任一所述的惯性导航***自标定方法。
10.一种计算机可读存储介质,存储有计算机程序,其特征在于,所述计算机程序被处理器执行时实现权利要求1至7中任一所述的惯性导航***自标定方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202310289342.9A CN116448146A (zh) | 2023-03-22 | 2023-03-22 | 一种惯性导航***自标定方法、装置、设备及存储介质 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202310289342.9A CN116448146A (zh) | 2023-03-22 | 2023-03-22 | 一种惯性导航***自标定方法、装置、设备及存储介质 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN116448146A true CN116448146A (zh) | 2023-07-18 |
Family
ID=87129390
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202310289342.9A Pending CN116448146A (zh) | 2023-03-22 | 2023-03-22 | 一种惯性导航***自标定方法、装置、设备及存储介质 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN116448146A (zh) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN116663329A (zh) * | 2023-07-26 | 2023-08-29 | 西安深信科创信息技术有限公司 | 自动驾驶仿真测试场景生成方法、装置、设备及存储介质 |
-
2023
- 2023-03-22 CN CN202310289342.9A patent/CN116448146A/zh active Pending
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN116663329A (zh) * | 2023-07-26 | 2023-08-29 | 西安深信科创信息技术有限公司 | 自动驾驶仿真测试场景生成方法、装置、设备及存储介质 |
CN116663329B (zh) * | 2023-07-26 | 2024-03-29 | 安徽深信科创信息技术有限公司 | 自动驾驶仿真测试场景生成方法、装置、设备及存储介质 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110673115B (zh) | 雷达与组合导航***的联合标定方法、装置、设备及介质 | |
US20200206945A1 (en) | Robot pose estimation method and apparatus and robot using the same | |
CN108715166B (zh) | 基于深度学习的车辆稳定性指标估计方法 | |
CN113602350A (zh) | 车辆前轮偏差角动态标定方法、装置、设备及存储介质 | |
CN102466802B (zh) | 使用推算定位法追踪车辆位置和车辆方位角的方法以及实现该方法的追踪装置 | |
CN107697155A (zh) | 车轮转向角的控制方法、装置和车轮转向角控制器 | |
CN112793585B (zh) | 一种自动驾驶的轨迹跟踪控制方法 | |
CN112284416B (zh) | 一种自动驾驶定位信息校准装置、方法及存储介质 | |
JP7113134B2 (ja) | 車両制御装置 | |
CN116448146A (zh) | 一种惯性导航***自标定方法、装置、设备及存储介质 | |
CN110579754A (zh) | 用于确定车辆的激光雷达与车辆其他的传感器的外参数的方法 | |
CN110637209B (zh) | 估计机动车的姿势的方法、设备和具有指令的计算机可读存储介质 | |
CN103712598A (zh) | 一种小型无人机姿态确定***与确定方法 | |
EP4345421A2 (en) | Method for calibrating sensor parameters based on autonomous driving, apparatus, storage medium, and vehicle | |
CN114241062A (zh) | 自动驾驶的相机外参确定方法、装置和计算机可读存储介质 | |
JP2016120882A (ja) | 車両運動パラメータ推定装置 | |
CN115390086A (zh) | 用于自动驾驶的融合定位方法、装置及电子设备、存储介质 | |
CN112078570A (zh) | 一种基于阿克曼转向模型的汽车定位方法 | |
CN113788020A (zh) | 一种车辆的自适应巡航控制方法及装置 | |
CN116559845A (zh) | 一种激光雷达自标定方法、***、设备及存储介质 | |
CN114347994B (zh) | 车道线位置估计方法和装置、电子设备和存储介质 | |
CN112945266A (zh) | 激光导航机器人及其机器人的里程计校准方法 | |
CN116080754A (zh) | 一种车辆自主驾驶横向控制方法 | |
CN114019954B (zh) | 航向安装角标定方法、装置、计算机设备和存储介质 | |
CN113884089B (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 |