CN112525143B - 一种设备安装角的确定方法及车载终端 - Google Patents
一种设备安装角的确定方法及车载终端 Download PDFInfo
- Publication number
- CN112525143B CN112525143B CN201910887036.9A CN201910887036A CN112525143B CN 112525143 B CN112525143 B CN 112525143B CN 201910887036 A CN201910887036 A CN 201910887036A CN 112525143 B CN112525143 B CN 112525143B
- Authority
- CN
- China
- Prior art keywords
- estimated
- state quantity
- vehicle
- moment
- angular velocity
- 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
- 238000009434 installation Methods 0.000 title claims abstract description 118
- 238000000034 method Methods 0.000 title claims abstract description 32
- 238000005259 measurement Methods 0.000 claims abstract description 94
- 239000011159 matrix material Substances 0.000 claims description 52
- 238000001514 detection method Methods 0.000 claims description 22
- 238000006243 chemical reaction Methods 0.000 claims description 10
- 230000004048 modification Effects 0.000 claims description 7
- 238000012986 modification Methods 0.000 claims description 7
- 238000012935 Averaging Methods 0.000 claims description 3
- 238000010586 diagram Methods 0.000 description 6
- 230000001133 acceleration Effects 0.000 description 2
- 230000008859 change Effects 0.000 description 2
- 230000008569 process Effects 0.000 description 2
- 230000009466 transformation Effects 0.000 description 2
- 241000135164 Timea Species 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000006467 substitution reaction 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
- G01C1/00—Measuring angles
-
- G—PHYSICS
- G07—CHECKING-DEVICES
- G07C—TIME OR ATTENDANCE REGISTERS; REGISTERING OR INDICATING THE WORKING OF MACHINES; GENERATING RANDOM NUMBERS; VOTING OR LOTTERY APPARATUS; ARRANGEMENTS, SYSTEMS OR APPARATUS FOR CHECKING NOT PROVIDED FOR ELSEWHERE
- G07C5/00—Registering or indicating the working of vehicles
- G07C5/08—Registering or indicating performance data other than driving, working, idle, or waiting time, with or without registering driving, working, idle or waiting time
- G07C5/0808—Diagnosing performance data
Landscapes
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Navigation (AREA)
- Control Of Driving Devices And Active Controlling Of Vehicle (AREA)
Abstract
本发明实施例公开一种设备安装角的确定方法及车载终端。该方法包括:根据车辆转弯时第一时刻的设备角速度以及车辆转弯时的角速度特征,确定实际车辆角速度;获取第二时刻的包括估计设备角速度和估计设备安装角的估计状态量;根据该估计状态量以及车辆角速度、估计设备安装角、估计设备角速度之间的预设关系,确定估计状态量与车辆角速度之间的量测方程;根据状态方程和估计状态量确定预测状态量;根据量测方程对预测状态量进行转换得到预测车辆角速度;根据实际车辆角速度与预测车辆角速度之间的偏差对预测状态量进行修改,得到包含第一时刻的估计设备安装角的估计状态量。应用本发明实施例提供的方案,能够实时地确定更加准确的设备安装角。
Description
技术领域
本发明涉及智能驾驶技术领域,具体而言,涉及一种设备安装角的确定方法及车载终端。
背景技术
在车载定位***中,智能车辆通常安装有惯性测量单元(Inertial MeasurementUnit,IMU)等传感器。车载定位***可以根据IMU输出的数据对车辆的位置和状态进行测量。在该测量中,通常需要得知IMU坐标系与车体坐标系之间的转换矩阵,而该转换矩阵与IMU相对于车体坐标系的安装角有关。通过标定,可以粗略地得到该安装角。但是,随着智能车辆的不断行驶,该安装角可能会发生缓慢变化。由于智能驾驶技术领域对数据的精度要求非常高,如果采用重新标定的方式确定新的安装角,其无法满足实时性要求。因此,亟需一种能够实时地确定更加准确的设备安装角的方法。
发明内容
本发明提供了一种设备安装角的确定方法及车载终端,以实时地确定更加准确的设备安装角。具体的技术方案如下。
第一方面,本发明实施例提供了一种设备安装角的确定方法,包括:
获取当车辆转弯时运动检测设备在第一时刻采集的设备角速度;
根据所述设备角速度以及预设的车辆转弯时的角速度特征,确定第一时刻的实际车辆角速度;
获取第二时刻的估计状态量;其中,估计状态量包括:估计设备角速度和估计设备安装角;所述第二时刻为所述第一时刻的上一时刻;
根据所述第二时刻的估计状态量,以及车辆角速度、估计设备安装角、估计设备角速度之间的预设关系,确定估计状态量与车辆角速度之间的量测方程;根据预设的状态方程和所述第二时刻的估计状态量,确定第一时刻的预测状态量;
根据所述量测方程对所述预测状态量进行转换,得到第一时刻的预测车辆角速度;
根据所述实际车辆角速度与所述预测车辆角速度之间的偏差对所述预测状态量进行修改,得到包含第一时刻的估计设备角速度和第一时刻的估计设备安装角的估计状态量。
可选的,所述根据所述设备角速度以及预设的车辆转弯时的角速度特征,确定第一时刻的实际车辆角速度的步骤,包括:
将预设的车辆转弯时的角速度方向作为第一时刻的实际车辆角速度的方向,将所述设备角速度的大小确定为所述实际车辆角速度的大小。
可选的,每个估计状态量包括估计状态量均值和估计状态量置信度;
所述根据所述第二时刻的估计状态量,以及车辆角速度、估计设备安装角、估计设备角速度之间的预设关系,确定估计状态量与车辆角速度之间的量测方程的步骤,包括:
根据所述第二时刻的估计状态量均值,以及车辆角速度、估计设备安装角、估计设备角速度之间的预设关系,确定估计状态量均值与车辆角速度之间的量测方程;
所述根据预设的状态方程和所述第二时刻的估计状态量,确定第一时刻的预测状态量的步骤,包括:
根据预设的状态方程和所述第二时刻的估计状态量均值,确定第一时刻的预测状态量均值;根据预设的状态方程和所述第二时刻的估计状态量置信度,确定第一时刻的预测状态量置信度;
所述根据所述量测方程对所述预测状态量进行转换,得到第一时刻的预测车辆角速度的步骤,包括:
根据所述量测方程对所述预测状态量均值进行转换,得到第一时刻的预测车辆角速度;
所述根据所述实际角速度与所述预测角速度之间的偏差对所述预测状态量进行修改,得到包含第一时刻的估计设备角速度和第一时刻的估计设备安装角的估计状态量的步骤,包括:
根据所述实际车辆角速度与所述预测车辆角速度之间的偏差以及所述预测状态量置信度,对所述预测状态量均值进行修改,得到包含第一时刻的估计设备角速度和第一时刻的估计设备安装角的估计状态量均值。
可选的,所述根据所述第二时刻的估计状态量均值,以及车辆角速度、估计设备安装角、估计设备角速度之间的预设关系,确定估计状态量均值与车辆角速度之间的量测方程的步骤,包括:
根据所述第二时刻的估计状态量均值中的估计设备安装角,以及车辆角速度、估计设备安装角、估计设备角速度之间的预设关系,确定车辆角速度与估计设备角速度之间的旋转矩阵;
根据车辆角速度、估计设备角速度与所述旋转矩阵之间的关系,构建估计状态量均值与车辆角速度之间的量测方程。
可选的,所述根据所述第二时刻的估计状态量均值中的估计设备安装角,以及车辆角速度、估计设备安装角、估计设备角速度之间的预设关系,确定车辆角速度与估计设备角速度之间的旋转矩阵的步骤,包括:
其中,所述θ和r分别为所述第二时刻的估计状态量均值中的估计设备安装角的模和单位向量,r=[rx,ry,rz]T,所述I为单位矩阵;
所述根据车辆角速度、估计设备角速度与所述旋转矩阵之间的关系,构建估计状态量均值与车辆角速度之间的量测方程的步骤,包括:
ωv=Ct·μ
可选的,所述根据预设的状态方程和所述第二时刻的估计状态量均值,确定第一时刻的预测状态量均值的步骤,包括:
所述根据预设的状态方程和所述第二时刻的估计状态量置信度,确定第一时刻的预测状态量置信度的步骤,包括:
所述根据所述量测方程对所述预测状态量均值进行转换,得到第一时刻的预测车辆角速度的步骤,包括:
所述根据所述实际车辆角速度与所述预测车辆角速度之间的偏差以及所述预测状态量置信度,对所述预测状态量均值进行修改,得到包含第一时刻的估计设备角速度和第一时刻的估计设备安装角的估计状态量均值的步骤,包括:
根据以下公式,对所述预测状态量均值进行修改,得到包含第一时刻的估计设备角速度和第一时刻的估计设备安装角的估计状态量均值μt:
第二方面,本发明实施例提供了一种车载终端,包括:处理器和运动检测设备;所述处理器包括:
第一获取模块,用于获取当车辆转弯时所述运动检测设备在第一时刻采集的设备角速度;
第一确定模块,用于根据所述设备角速度以及预设的车辆转弯时的角速度特征,确定第一时刻的实际车辆角速度;
第二获取模块,用于获取第二时刻的估计状态量;其中,估计状态量包括:估计设备角速度和估计设备安装角;所述第二时刻为所述第一时刻的上一时刻;
第二确定模块,用于根据所述第二时刻的估计状态量,以及车辆角速度、估计设备安装角、估计设备角速度之间的预设关系,确定估计状态量与车辆角速度之间的量测方程;根据预设的状态方程和所述第二时刻的估计状态量,确定第一时刻的预测状态量;
转换模块,用于根据所述量测方程对所述预测状态量进行转换,得到第一时刻的预测车辆角速度;
修改模块,用于根据所述实际车辆角速度与所述预测车辆角速度之间的偏差对所述预测状态量进行修改,得到包含第一时刻的估计设备角速度和第一时刻的估计设备安装角的估计状态量。
可选的,所述第一确定模块,具体用于:
将预设的车辆转弯时的角速度方向作为第一时刻的实际车辆角速度的方向,将所述设备角速度的大小确定为所述实际车辆角速度的大小。
可选的,每个估计状态量包括估计状态量均值和估计状态量置信度;
所述第二确定模块,根据所述第二时刻的估计状态量,以及车辆角速度、估计设备安装角、估计设备角速度之间的预设关系,确定估计状态量与车辆角速度之间的量测方程的步骤,包括:
根据所述第二时刻的估计状态量均值,以及车辆角速度、估计设备安装角、估计设备角速度之间的预设关系,确定估计状态量均值与车辆角速度之间的量测方程;
所述第二确定模块,根据预设的状态方程和所述第二时刻的估计状态量,确定第一时刻的预测状态量的步骤,包括:
根据预设的状态方程和所述第二时刻的估计状态量均值,确定第一时刻的预测状态量均值;根据预设的状态方程和所述第二时刻的估计状态量置信度,确定第一时刻的预测状态量置信度;
所述转换模块,具体用于:
根据所述量测方程对所述预测状态量均值进行转换,得到第一时刻的预测车辆角速度;
所述修改模块,具体用于:
根据所述实际车辆角速度与所述预测车辆角速度之间的偏差以及所述预测状态量置信度,对所述预测状态量均值进行修改,得到包含第一时刻的估计设备角速度和第一时刻的估计设备安装角的估计状态量均值。
可选的,所述第二确定模块,根据所述第二时刻的估计状态量均值,以及车辆角速度、估计设备安装角、估计设备角速度之间的预设关系,确定估计状态量均值与车辆角速度之间的量测方程时,包括:
根据所述第二时刻的估计状态量均值中的估计设备安装角,以及车辆角速度、估计设备安装角、估计设备角速度之间的预设关系,确定车辆角速度与估计设备角速度之间的旋转矩阵;
根据车辆角速度、估计设备角速度与所述旋转矩阵之间的关系,构建估计状态量均值与车辆角速度之间的量测方程。
可选的,所述第二确定模块,根据所述第二时刻的估计状态量均值中的估计设备安装角,以及车辆角速度、估计设备安装角、估计设备角速度之间的预设关系,确定车辆角速度与估计设备角速度之间的旋转矩阵时,包括:
其中,所述θ和r分别为所述第二时刻的估计状态量均值中的估计设备安装角的模和单位向量,r=[rx,ry,rz]T,所述I为单位矩阵;
所述第二确定模块,根据车辆角速度、估计设备角速度与所述旋转矩阵之间的关系,构建估计状态量均值与车辆角速度之间的量测方程时,包括:
ωv=Ct·μ
可选的,所述第二确定模块,根据预设的状态方程和所述第二时刻的估计状态量均值,确定第一时刻的预测状态量均值时,包括:
所述第二确定模块,根据预设的状态方程和所述第二时刻的估计状态量置信度,确定第一时刻的预测状态量置信度时,包括:
所述转换模块,具体用于:
所述修改模块,具体用于:
根据以下公式,对所述预测状态量均值进行修改,得到包含第一时刻的估计设备角速度和第一时刻的估计设备安装角的估计状态量均值μt:
由上述内容可知,本发明实施例提供的设备安装角的确定方法及车载终端中,提出了一种虚拟的量测量车辆角速度,并且以估计设备角速度和估计设备安装角等作为估计状态量,利用车辆角速度、估计设备安装角、估计设备角速度之间的预设关系来确定估计状态量与车辆角速度之间的量测方程,进而利用量测方程和根据状态方程预测得到的预测状态量,确定当前时刻的预测车辆角速度,并根据实际车辆角速度与预测车辆角速度之间的偏差,对预测状态量进行修改,得到第一时刻的估计状态量,这样能够使得得到的估计状态量尽可能接近于真实值,因此能够实时地确定更加准确的设备安装角。当然,实施本发明的任一产品或方法并不一定需要同时达到以上所述的所有优点。
本发明实施例的创新点包括:
1、提出了一个虚拟的量测量,即车辆角速度,根据状态方程对上一时刻的状态量进行预测,根据实际量测量与预测量测量之间的偏差,对预测阶段得到的预测值进行修改,能够得到一个更接近真实值的新估计值,进而能够实时地确定更加准确的设备安装角。
2、以预设的角速度方向作为车辆角速度方向,以设备角速度大小作为车辆角速度的大小,来确定虚拟的量测量,提供了可实施的具体方式。
3、针对估计状态量引入了置信度,即每个估计状态量均值对应一个置信度,根据置信度计算估计状态量,能够使得确定的估计状态量更准确。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案,下面将对实施例或现有技术描述中所需要使用的附图作简单介绍。显而易见地,下面描述中的附图仅仅是本发明的一些实施例。对于本领域普通技术人员来讲,在不付出创造性劳动的前提下,还可以根据这些附图获得其他的附图。
图1为本发明实施例提供的设备安装角的确定方法的一种流程示意图;
图2为本发明实施例提供的设备坐标系与车体坐标系之间的一种关系示意图;
图3为本发明实施例提供的车辆转弯时车辆角速度方向的一种示意图;
图4为本发明实施例提供的设备安装角的确定方法的另一种流程示意图;
图5为本发明实施例提供的车载终端的一种结构示意图。
具体实施方式
下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整的描述。显然,所描述的实施例仅仅是本发明的一部分实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有付出创造性劳动前提下所获得的所有其他实施例,都属于本发明保护的范围。
需要说明的是,本发明实施例及附图中的术语“包括”和“具有”以及它们的任何变形,意图在于覆盖不排他的包含。例如包含的一系列步骤或单元的过程、方法、***、产品或设备没有限定于已列出的步骤或单元,而是可选地还包括没有列出的步骤或单元,或可选地还包括对于这些过程、方法、产品或设备固有的其他步骤或单元。
本发明实施例公开了一种设备安装角的确定方法及车载终端,能够实时地确定更加准确的设备安装角。下面对本发明实施例进行详细说明。
图1为本发明实施例提供的设备安装角的确定方法的一种流程示意图。该方法应用于电子设备。该电子设备可以为普通计算机、服务器或者智能终端设备等,也可以为车载电脑或车载工业控制计算机(Industrial personal Computer,IPC)等车载终端。该方法具体包括以下步骤。
S110:获取当车辆转弯时运动检测设备在第一时刻采集的设备角速度。
其中,运动检测设备可以为惯性量测单元(Inertial Measurement Unit,IMU) 或者其他能够实现与该IMU所实现的功能相似的设备。运动检测设备可以设置于车辆中,用于检测车辆的运动状态。例如,运动检测设备可以采集角速度和/或加速度等运动状态量。该车辆可以为智能驾驶车辆。本实施例中,为了估计设备安装角,可以将车辆和运动检测设备作为***进行考察。
运动检测设备可以按照预设的采集周期采集数据,并确定所采集数据的采集时刻。
运动检测设备所采集的设备角速度,可以理解为运动检测设备的角速度。该角速度可以采用设备坐标系中的三个角速度分量表示。设备坐标系为运动检测设备所在的坐标系。
在车辆定位时,经常需要使用到车体坐标系。车体坐标系的具***置与所选取的坐标轴方向和原点位置有关,例如,车体坐标系可以将两个后轮的中心点作为原点,坐标轴方向为车辆的右方、前方和上方。
设备安装角指的是车体坐标系与设备坐标系之间的相对旋转角度。参见图2所示,为设备坐标系与车体坐标系之间的一种关系示意图。其中,标注了设备坐标系和车体坐标系的示例。设备安装角会根据设备在车辆中的安装位置不同而不同。
理想情况下,运动检测设备与车辆为刚性连接,设备安装角是不变化的。这种情况下可以通过标定确定该设备安装角。但是在实际应用中,该设备安装角可能会随着车辆的行驶和振动而发生缓慢变化。在这种情况下,可以采用本实施例的方式,实时地估测出该设备安装角。
本实施例中,可以采用以下方式检测车辆是否处于转弯状态:检测运动检测设备输出的设备角速度是否大于阈值,如果大于,则确定车辆处于转弯状态。
S120:根据设备角速度以及预设的车辆转弯时的角速度特征,确定第一时刻的实际车辆角速度。
具体的,本步骤可以包括以下实施方式:将预设的车辆转弯时的角速度方向作为第一时刻的实际车辆角速度的方向,将设备角速度的大小确定为实际车辆角速度的大小。例如,预设的车辆转弯时的角速度方向可以但不限于为竖直向上。
考虑车辆在转弯时的运动特征,车辆在水平面上转弯时可以参见图3示意图所示,其中车辆正在沿着道路左转,可以认为车辆在转弯时,车辆角速度在车体坐标系下的方向是竖直向上的,车辆角速度可以采用[0,0,|ω|]T表示。其中,|ω|为设备角速度的大小,车体坐标系中z轴竖直向上。
S130:获取第二时刻的估计状态量。
其中,估计状态量包括:估计设备角速度和估计设备安装角。由于设备安装角在随着时间缓慢变化,第一时刻的真实设备安装角是未知的,而运动检测设备本身输出的设备角速度并不是当前时刻与真实设备安装角对应的真实的设备角速度。估计设备角速度是***给出的当前时刻的、***认为最优的真实设备角速度的估计值。估计设备安装角是***给出的当前时刻的、***认为最优的真实设备安装角的估计值。上述***可以理解为作为执行主体的电子设备。
第二时刻的估计状态量为根据第二时刻采集的设备角速度确定的估计状态量。第二时刻为第一时刻的上一时刻。确定设备安装角的操作可以为按照预设频率周期性进行的,上一时刻可以理解为上一次确定设备安装角所对应的时刻。
初始时刻的估计状态量中的估计设备角速度可以采用运动检测设备直接输出的设备角速度,初始时刻的估计状态量中的估计设备安装角可以采用预先标定的设备安装角。
S140:根据第二时刻的估计状态量,以及车辆角速度、估计设备安装角、估计设备角速度之间的预设关系,确定估计状态量与车辆角速度之间的量测方程;根据预设的状态方程和第二时刻的估计状态量,确定第一时刻的预测状态量。
由于估计设备角速度为当前时刻的、***认为最优的真实设备角速度的估计值,估计设备安装角是当前时刻的、***认为最优的真实设备安装角的估计值,并且车辆角速度与车体坐标系对应,估计设备角速度与设备坐标系对应,因此两坐标系之间的转换矩阵与设备安装角相关。
本实施例中,将车辆角速度作为虚拟的量测量,认为存在虚拟传感器,该虚拟传感器可以量测该车辆角速度。当得到量测量之后,可以根据量测量估测状态量。量测方程能够实现状态量与车辆角速度(量测量)之间的相互转换。
状态方程能够根据上一个时刻的状态量预测下一个时刻的状态量。第一时刻的预测状态量包括第一时刻的预测设备安装角和第一时刻的预测设备角速度。
S150:根据量测方程对预测状态量进行转换,得到第一时刻的预测车辆角速度。
将第一时刻的预测状态量代入量测方程,可以得到第一时刻的预测车辆角速度。
S160:根据实际车辆角速度与预测车辆角速度之间的偏差对预测状态量进行修改,得到包含第一时刻的估计设备角速度和第一时刻的估计设备安装角的估计状态量。
预测车辆角速度为将第二时刻的估计设备安装角作为第一时刻的估计设备安装角而得到的预测量,根据实际车辆角速度与预测车辆角速度之间的偏差,能够在第二时刻的估计设备安装角基础上,得到修正后的更加准确的设备安装角。
由上述内容可知,本实施例中提出了一种虚拟的量测量车辆角速度,并且以估计设备角速度和估计设备安装角等作为估计状态量,利用车辆角速度、估计设备安装角、估计设备角速度之间的预设关系来确定估计状态量与车辆角速度之间的量测方程,进而利用量测方程和根据状态方程预测得到的预测状态量,确定当前时刻的预测车辆角速度,并根据实际车辆角速度与预测车辆角速度之间的偏差,对预测状态量进行修改,得到第一时刻的估计状态量,这样能够使得得到的估计状态量尽可能接近于真实值,因此能够实时地确定更加准确的设备安装角。
在本发明的另一实施例中,基于图1所示实施例,当每个估计状态量包括估计状态量均值和估计状态量置信度时,可以得到图2所示流程示意图,该方法具体包括以下步骤:
S210:获取当车辆转弯时运动检测设备在第一时刻采集的设备角速度。
S220:根据设备角速度以及预设的车辆转弯时的角速度特征,确定第一时刻的实际车辆角速度。
S230:获取第二时刻的估计状态量。
其中,估计状态量包括:估计设备角速度和估计设备安装角;所述第二时刻为所述第一时刻的上一时刻。
上述步骤S210~S230分别与图1所示实施例中的步骤S110~S130相同,具体说明可参见图1中相应部分的说明,此处不再赘述。
S240:根据第二时刻的估计状态量均值,以及车辆角速度、估计设备安装角、估计设备角速度之间的预设关系,确定估计状态量均值与车辆角速度之间的量测方程;根据预设的状态方程和第二时刻的估计状态量均值,确定第一时刻的预测状态量均值,根据预设的状态方程和第二时刻的估计状态量置信度,确定第一时刻的预测状态量置信度。
其中,状态量置信度可以采用协方差表示。初始时刻的估计状态量置信度可以为预设值。
本实施例中,量测方程能够实现估计状态量均值与车辆角速度之间的转换。状态方程能够根据上一个时刻的估计状态量均值预测下一个时刻的估计状态量均值,也能够根据上一个时刻的估计状态量置信度预测下一个时刻的估计状态量置信度。
S250:根据量测方程对预测状态量均值进行转换,得到第一时刻的预测车辆角速度。
将第一时刻的预测状态量均值代入该量测方程,得到第一时刻的预测车辆角速度。
S260:根据实际车辆角速度与预测车辆角速度之间的偏差以及预测状态量置信度,对预测状态量均值进行修改,得到包含第一时刻的估计设备角速度和第一时刻的估计设备安装角的估计状态量均值。
根据量测方程以及第一时刻的预测状态量置信度,可以确定第一时刻的估计状态量置信度。
综上,本实施例中,针对估计状态量引入了置信度,即每个估计状态量均值对应一个置信度,根据实际车辆角速度与预测车辆角速度之间的偏差以及预测状态置信度计算估计状态量均值,能够使得确定的估计状态量更准确。
在本发明的另一实施例中,基于图2所示实施例,步骤S240中根据第二时刻的估计状态量均值,以及车辆角速度、估计设备安装角、估计设备角速度之间的预设关系,确定估计状态量均值与车辆角速度之间的量测方程的步骤,包括以下步骤1和步骤2。
步骤1:根据第二时刻的估计状态量均值中的估计设备安装角,以及车辆角速度、估计设备安装角、估计设备角速度之间的预设关系,确定车辆角速度与估计设备角速度之间的旋转矩阵。
其中,θ和r分别为第二时刻的估计状态量均值中的估计设备安装角的模和单位向量,r=[rx,ry,rz]T,I为单位矩阵,维数为3×3。上述公式可以根据罗德里克斯公式得到。
步骤2:根据车辆角速度、估计设备角速度与所述旋转矩阵之间的关系,构建估计状态量均值与车辆角速度之间的量测方程。
ωv=Ct·μ
其中,ωv为车辆角速度,ωi为估计设备角速度,为旋转矩阵,μ为估计状态量均值,μ=[ωi,α]T,α为估计设备安装角,B0为与估计设备安装角的向量维数对应的零矩阵。例如,Ct可以为3×6维的矩阵,为3×3 维的矩阵,B0为3×3维的零矩阵。
综上,本实施例提供了根据第二时刻的估计状态量均值,以及车辆角速度、估计设备安装角、估计设备角速度之间的预设关系,确定估计状态量均值与车辆角速度之间的量测方程的具体公式,为实现本实施例提供了具体的实施方式。
步骤S250中根据所述量测方程对预测状态量均值进行转换,得到第一时刻的预测车辆角速度的步骤,可以包括:
步骤S260中根据实际车辆角速度与预测车辆角速度之间的偏差以及预测状态量置信度,对预测状态量均值进行修改,得到包含第一时刻的估计设备角速度和第一时刻的估计设备安装角的估计状态量均值的步骤,包括:
根据以下公式,对预测状态量均值进行修改,得到包含第一时刻的估计设备角速度和第一时刻的估计设备安装角的估计状态量均值μt:
其中, 为预测状态量置信度,Ct为量测方程中的系数,Q为预设的量测方程置信度,Zt为实际车辆角速度,为预测车辆角速度,为实际车辆角速度与预测车辆角速度之间的偏差, T为转置符号。估计状态量均值μt中包括估计设备安装角和估计设备角速度。
其中,Σt为第一时刻的估计状态量置信度,I为单位矩阵。
综上,本实施例中提供了实施各个步骤的具体公式,为本实施例的实现提供了可实施的方式。
在本发明的另一实施例中,可以基于对现有的卡尔曼滤波器进行修改,采用修改后的卡尔曼滤波器实现对状态量的估计,无需重新编写所有程序,这样能够提高处理效率。
在本发明的另一实施例中,车辆中可以设置但不限于全球定位*** (GlobalPositioning System,GPS)和IMU等传感器。车载终端在根据GPS 数据和IMU数据对车辆进行定位时,可以周期性地估计设备安装角,以便更新设备安装角,使得对车辆的定位更准确。在该实施例中,估计状态量可以包括但不限于:IMU位姿、IMU安装角、IMU速度、IMU加速度和IMU角速度等。量测方程中的中的B0可以根据估计状态量中的具体参量进行设置。
图5为本发明实施例提供的车载终端的一种结构示意图。该实施例与图 1所示实施例相对应。该车载终端包括:处理器510和运动检测设备520;处理器510包括:
第一获取模块511,用于获取当车辆转弯时运动检测设备在第一时刻采集的设备角速度;
第一确定模块512,用于根据设备角速度以及预设的车辆转弯时的角速度特征,确定第一时刻的实际车辆角速度;
第二获取模块513,用于获取第二时刻的估计状态量;其中,估计状态量包括:估计设备角速度和估计设备安装角;第二时刻为第一时刻的上一时刻;
第二确定模块514,用于根据第二时刻的估计状态量,以及车辆角速度、估计设备安装角、估计设备角速度之间的预设关系,确定估计状态量与车辆角速度之间的量测方程;根据预设的状态方程和第二时刻的估计状态量,确定第一时刻的预测状态量;
转换模块515,用于根据量测方程对预测状态量进行转换,得到第一时刻的预测车辆角速度;
修改模块516,用于根据实际车辆角速度与预测车辆角速度之间的偏差对预测状态量进行修改,得到包含第一时刻的估计设备角速度和第一时刻的估计设备安装角的估计状态量。
在本发明的另一实施例中,基于图5所示实施例,第一确定模块512具体用于:
将预设的车辆转弯时的角速度方向作为第一时刻的实际车辆角速度的方向,将设备角速度的大小确定为实际车辆角速度的大小。
在本发明的另一实施例中,基于图5所示实施例,每个估计状态量包括估计状态量均值和估计状态量置信度;
第二确定模块514,根据第二时刻的估计状态量,以及车辆角速度、估计设备安装角、估计设备角速度之间的预设关系,确定估计状态量与车辆角速度之间的量测方程的步骤,包括:
根据第二时刻的估计状态量均值,以及车辆角速度、估计设备安装角、估计设备角速度之间的预设关系,确定估计状态量均值与车辆角速度之间的量测方程;
第二确定模块514,根据预设的状态方程和第二时刻的估计状态量,确定第一时刻的预测状态量的步骤,包括:
根据预设的状态方程和第二时刻的估计状态量均值,确定第一时刻的预测状态量均值;根据预设的状态方程和第二时刻的估计状态量置信度,确定第一时刻的预测状态量置信度;
转换模块515具体用于:
根据量测方程对预测状态量均值进行转换,得到第一时刻的预测车辆角速度;
修改模块516具体用于:
根据实际车辆角速度与预测车辆角速度之间的偏差以及预测状态量置信度,对预测状态量均值进行修改,得到包含第一时刻的估计设备角速度和第一时刻的估计设备安装角的估计状态量均值。
在本发明的另一实施例中,基于上述所示实施例,第二确定模块514,根据第二时刻的估计状态量均值,以及车辆角速度、估计设备安装角、估计设备角速度之间的预设关系,确定估计状态量均值与车辆角速度之间的量测方程时,包括:
根据第二时刻的估计状态量均值中的估计设备安装角,以及车辆角速度、估计设备安装角、估计设备角速度之间的预设关系,确定车辆角速度与估计设备角速度之间的旋转矩阵;
根据车辆角速度、估计设备角速度与旋转矩阵之间的关系,构建估计状态量均值与车辆角速度之间的量测方程。
在本发明的另一实施例中,基于上述所示实施例,第二确定模块514,根据第二时刻的估计状态量均值中的估计设备安装角,以及车辆角速度、估计设备安装角、估计设备角速度之间的预设关系,确定车辆角速度与估计设备角速度之间的旋转矩阵时,包括:
其中,θ和r分别为第二时刻的估计状态量均值中的估计设备安装角的模和单位向量,r=[rx,ry,rz]T,I为单位矩阵;
第二确定模块,根据车辆角速度、估计设备角速度与旋转矩阵之间的关系,构建估计状态量均值与车辆角速度之间的量测方程时,包括:
ωv=Ct·μ
在本发明的另一实施例中,基于图1所示实施例,第二确定模块514,根据预设的状态方程和第二时刻的估计状态量均值,确定第一时刻的预测状态量均值时,包括:
第二确定模块514,根据预设的状态方程和第二时刻的估计状态量置信度,确定第一时刻的预测状态量置信度时,包括:
转换模块515,具体用于:
修改模块516,具体用于:
根据以下公式,对预测状态量均值进行修改,得到包含第一时刻的估计设备角速度和第一时刻的估计设备安装角的估计状态量均值μt:
该终端实施例与图1所示方法实施例是基于同一发明构思得到的实施例,相关之处可以相互参照。上述终端实施例与方法实施例相对应,与该方法实施例具有同样的技术效果,具体说明参见方法实施例。
本领域普通技术人员可以理解:附图只是一个实施例的示意图,附图中的模块或流程并不一定是实施本发明所必须的。
本领域普通技术人员可以理解:实施例中的装置中的模块可以按照实施例描述分布于实施例的装置中,也可以进行相应变化位于不同于本实施例的一个或多个装置中。上述实施例的模块可以合并为一个模块,也可以进一步拆分成多个子模块。
最后应说明的是:以上实施例仅用以说明本发明的技术方案,而非对其限制;尽管参照前述实施例对本发明进行了详细的说明,本领域的普通技术人员应当理解:其依然可以对前述实施例所记载的技术方案进行修改,或者对其中部分技术特征进行等同替换;而这些修改或者替换,并不使相应技术方案的本质脱离本发明实施例技术方案的精神和范围。
Claims (10)
1.一种设备安装角的确定方法,其特征在于,包括:
获取当车辆转弯时运动检测设备在第一时刻采集的设备角速度;
根据所述设备角速度以及预设的车辆转弯时的角速度特征,确定第一时刻的实际车辆角速度;
获取第二时刻的估计状态量;其中,估计状态量包括:估计设备角速度和估计设备安装角;所述第二时刻为所述第一时刻的上一时刻;
根据所述第二时刻的估计状态量,以及车辆角速度、估计设备安装角、估计设备角速度之间的预设关系,确定估计状态量与车辆角速度之间的量测方程;根据预设的状态方程和所述第二时刻的估计状态量,确定第一时刻的预测状态量;
根据所述量测方程对所述预测状态量进行转换,得到第一时刻的预测车辆角速度;
根据所述实际车辆角速度与所述预测车辆角速度之间的偏差对所述预测状态量进行修改,得到包含第一时刻的估计设备角速度和第一时刻的估计设备安装角的第一时刻的估计状态量。
2.如权利要求1所述的方法,其特征在于,所述根据所述设备角速度以及预设的车辆转弯时的角速度特征,确定第一时刻的实际车辆角速度的步骤,包括:
将预设的车辆转弯时的角速度方向作为第一时刻的实际车辆角速度的方向,将所述设备角速度的大小确定为所述实际车辆角速度的大小。
3.如权利要求1所述的方法,其特征在于,每个估计状态量包括估计状态量均值和估计状态量置信度;
所述根据所述第二时刻的估计状态量,以及车辆角速度、估计设备安装角、估计设备角速度之间的预设关系,确定估计状态量与车辆角速度之间的量测方程的步骤,包括:
根据所述第二时刻的估计状态量均值,以及车辆角速度、估计设备安装角、估计设备角速度之间的预设关系,确定估计状态量均值与车辆角速度之间的量测方程;
所述根据预设的状态方程和所述第二时刻的估计状态量,确定第一时刻的预测状态量的步骤,包括:
根据预设的状态方程和所述第二时刻的估计状态量均值,确定第一时刻的预测状态量均值;根据预设的状态方程和所述第二时刻的估计状态量置信度,确定第一时刻的预测状态量置信度;
所述根据所述量测方程对所述预测状态量进行转换,得到第一时刻的预测车辆角速度的步骤,包括:
根据所述量测方程对所述预测状态量均值进行转换,得到第一时刻的预测车辆角速度;
所述根据所述实际车辆角速度与所述预测车辆角速度之间的偏差对所述预测状态量进行修改,得到包含第一时刻的估计设备角速度和第一时刻的估计设备安装角的第一时刻的估计状态量的步骤,包括:
根据所述实际车辆角速度与所述预测车辆角速度之间的偏差以及所述预测状态量置信度,对所述预测状态量均值进行修改,得到包含第一时刻的估计设备角速度和第一时刻的估计设备安装角的第一时刻的估计状态量均值。
4.如权利要求3所述的方法,其特征在于,所述根据所述第二时刻的估计状态量均值,以及车辆角速度、估计设备安装角、估计设备角速度之间的预设关系,确定估计状态量均值与车辆角速度之间的量测方程的步骤,包括:
根据所述第二时刻的估计状态量均值中的估计设备安装角,以及车辆角速度、估计设备安装角、估计设备角速度之间的预设关系,确定车辆角速度与估计设备角速度之间的旋转矩阵;
根据车辆角速度、估计设备角速度与所述旋转矩阵之间的关系,构建估计状态量均值与车辆角速度之间的量测方程。
5.如权利要求4所述的方法,其特征在于,所述根据所述第二时刻的估计状态量均值中的估计设备安装角,以及车辆角速度、估计设备安装角、估计设备角速度之间的预设关系,确定车辆角速度与估计设备角速度之间的旋转矩阵的步骤,包括:
其中,所述θ和r分别为所述第二时刻的估计状态量均值中的估计设备安装角的模和单位向量,r=[rx,ry,rz]T,所述I为单位矩阵;
所述根据车辆角速度、估计设备角速度与所述旋转矩阵之间的关系,构建估计状态量均值与车辆角速度之间的量测方程的步骤,包括:
ωv=Ct·μ
6.如权利要求3所述的方法,其特征在于,所述根据预设的状态方程和所述第二时刻的估计状态量均值,确定第一时刻的预测状态量均值的步骤,包括:
所述根据预设的状态方程和所述第二时刻的估计状态量置信度,确定第一时刻的预测状态量置信度的步骤,包括:
所述根据所述量测方程对所述预测状态量均值进行转换,得到第一时刻的预测车辆角速度的步骤,包括:
所述根据所述实际车辆角速度与所述预测车辆角速度之间的偏差以及所述预测状态量置信度,对所述预测状态量均值进行修改,得到包含第一时刻的估计设备角速度和第一时刻的估计设备安装角的第一时刻的估计状态量均值的步骤,包括:
根据以下公式,对所述预测状态量均值进行修改,得到包含第一时刻的估计设备角速度和第一时刻的估计设备安装角的第一时刻的估计状态量均值μt:
7.一种车载终端,其特征在于,包括:处理器和运动检测设备;所述处理器包括:
第一获取模块,用于获取当车辆转弯时所述运动检测设备在第一时刻采集的设备角速度;
第一确定模块,用于根据所述设备角速度以及预设的车辆转弯时的角速度特征,确定第一时刻的实际车辆角速度;
第二获取模块,用于获取第二时刻的估计状态量;其中,估计状态量包括:估计设备角速度和估计设备安装角;所述第二时刻为所述第一时刻的上一时刻;
第二确定模块,用于根据所述第二时刻的估计状态量,以及车辆角速度、估计设备安装角、估计设备角速度之间的预设关系,确定估计状态量与车辆角速度之间的量测方程;根据预设的状态方程和所述第二时刻的估计状态量,确定第一时刻的预测状态量;
转换模块,用于根据所述量测方程对所述预测状态量进行转换,得到第一时刻的预测车辆角速度;
修改模块,用于根据所述实际车辆角速度与所述预测车辆角速度之间的偏差对所述预测状态量进行修改,得到包含第一时刻的估计设备角速度和第一时刻的估计设备安装角的第一时刻的估计状态量。
8.如权利要求7所述的车载终端,其特征在于,所述第一确定模块,具体用于:
将预设的车辆转弯时的角速度方向作为第一时刻的实际车辆角速度的方向,将所述设备角速度的大小确定为所述实际车辆角速度的大小。
9.如权利要求7所述的车载终端,其特征在于,每个估计状态量包括估计状态量均值和估计状态量置信度;
所述第二确定模块,根据所述第二时刻的估计状态量,以及车辆角速度、估计设备安装角、估计设备角速度之间的预设关系,确定估计状态量与车辆角速度之间的量测方程的步骤,包括:
根据所述第二时刻的估计状态量均值,以及车辆角速度、估计设备安装角、估计设备角速度之间的预设关系,确定估计状态量均值与车辆角速度之间的量测方程;
所述第二确定模块,根据预设的状态方程和所述第二时刻的估计状态量,确定第一时刻的预测状态量的步骤,包括:
根据预设的状态方程和所述第二时刻的估计状态量均值,确定第一时刻的预测状态量均值;根据预设的状态方程和所述第二时刻的估计状态量置信度,确定第一时刻的预测状态量置信度;
所述转换模块,具体用于:
根据所述量测方程对所述预测状态量均值进行转换,得到第一时刻的预测车辆角速度;
所述修改模块,具体用于:
根据所述实际车辆角速度与所述预测车辆角速度之间的偏差以及所述预测状态量置信度,对所述预测状态量均值进行修改,得到包含第一时刻的估计设备角速度和第一时刻的估计设备安装角的第一时刻的估计状态量均值。
10.如权利要求9所述的车载终端,其特征在于,所述第二确定模块,根据所述第二时刻的估计状态量均值,以及车辆角速度、估计设备安装角、估计设备角速度之间的预设关系,确定估计状态量均值与车辆角速度之间的量测方程时,包括:
根据所述第二时刻的估计状态量均值中的估计设备安装角,以及车辆角速度、估计设备安装角、估计设备角速度之间的预设关系,确定车辆角速度与估计设备角速度之间的旋转矩阵;
根据车辆角速度、估计设备角速度与所述旋转矩阵之间的关系,构建估计状态量均值与车辆角速度之间的量测方程。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910887036.9A CN112525143B (zh) | 2019-09-19 | 2019-09-19 | 一种设备安装角的确定方法及车载终端 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910887036.9A CN112525143B (zh) | 2019-09-19 | 2019-09-19 | 一种设备安装角的确定方法及车载终端 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN112525143A CN112525143A (zh) | 2021-03-19 |
CN112525143B true CN112525143B (zh) | 2022-09-27 |
Family
ID=74974225
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910887036.9A Active CN112525143B (zh) | 2019-09-19 | 2019-09-19 | 一种设备安装角的确定方法及车载终端 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN112525143B (zh) |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101393028A (zh) * | 2008-11-07 | 2009-03-25 | 北京航空航天大学 | 斜置imu安装角的快速估计与补偿*** |
CN108007455A (zh) * | 2017-11-01 | 2018-05-08 | 千寻位置网络有限公司 | 惯导***的纠偏方法及装置、导航及服务终端、存储器 |
CN108592950A (zh) * | 2018-05-17 | 2018-09-28 | 北京航空航天大学 | 一种单目相机和惯性测量单元相对安装角标定方法 |
CN108731667A (zh) * | 2017-04-14 | 2018-11-02 | 百度在线网络技术(北京)有限公司 | 用于确定无人驾驶车辆的速度和位姿的方法和装置 |
CN109029503A (zh) * | 2018-08-08 | 2018-12-18 | 上海博泰悦臻网络技术服务有限公司 | 车载陀螺仪安装角自适应校准方法及***、存储介质及车载终端 |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP4191499B2 (ja) * | 2003-02-03 | 2008-12-03 | パイオニア株式会社 | 車載装置 |
-
2019
- 2019-09-19 CN CN201910887036.9A patent/CN112525143B/zh active Active
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101393028A (zh) * | 2008-11-07 | 2009-03-25 | 北京航空航天大学 | 斜置imu安装角的快速估计与补偿*** |
CN108731667A (zh) * | 2017-04-14 | 2018-11-02 | 百度在线网络技术(北京)有限公司 | 用于确定无人驾驶车辆的速度和位姿的方法和装置 |
CN108007455A (zh) * | 2017-11-01 | 2018-05-08 | 千寻位置网络有限公司 | 惯导***的纠偏方法及装置、导航及服务终端、存储器 |
CN108592950A (zh) * | 2018-05-17 | 2018-09-28 | 北京航空航天大学 | 一种单目相机和惯性测量单元相对安装角标定方法 |
CN109029503A (zh) * | 2018-08-08 | 2018-12-18 | 上海博泰悦臻网络技术服务有限公司 | 车载陀螺仪安装角自适应校准方法及***、存储介质及车载终端 |
Also Published As
Publication number | Publication date |
---|---|
CN112525143A (zh) | 2021-03-19 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN108731670A (zh) | 基于量测模型优化的惯性/视觉里程计组合导航定位方法 | |
KR100898169B1 (ko) | 관성항법시스템의 초기정렬 방법 | |
JP5074950B2 (ja) | 航法装置 | |
CN110346824B (zh) | 一种车辆导航方法、***、装置及可读存储介质 | |
CN102313535A (zh) | 坡度检测方法 | |
JP5164645B2 (ja) | カルマンフィルタ処理における繰り返し演算制御方法及び装置 | |
CN110440797A (zh) | 车辆姿态估计方法及*** | |
CN108871323B (zh) | 一种低成本惯性传感器在机动环境下的高精度导航方法 | |
CN116777984A (zh) | 用于校准自主交通载具中摄像头的外部参数的*** | |
JP5219547B2 (ja) | 車載ナビゲーションシステム及びナビゲーション方法 | |
CN110637209A (zh) | 估计机动车的姿势的方法、设备和具有指令的计算机可读存储介质 | |
CN115060257A (zh) | 一种基于民用级惯性测量单元的车辆变道检测方法 | |
CN102706364B (zh) | 一种汽车用微机械陀螺标度因子在线标定方法 | |
CN113008229B (zh) | 一种基于低成本车载传感器的分布式自主组合导航方法 | |
CN112046491B (zh) | 车轮侧偏刚度的估算方法、装置、车辆及可读存储介质 | |
CN109506674B (zh) | 一种加速度的校正方法及装置 | |
CN112525143B (zh) | 一种设备安装角的确定方法及车载终端 | |
CN112731320A (zh) | 车载雷达误差数据的估计方法、装置、设备及存储介质 | |
JP6555033B2 (ja) | 角速度センサ補正装置および角速度センサ補正方法 | |
Eising et al. | 2.5 D vehicle odometry estimation | |
CN108773378B (zh) | 一种基于移动终端的汽车行驶速度实时估计方法及装置 | |
CN109459769B (zh) | 一种自主定位方法与*** | |
CN106441282B (zh) | 一种星敏感器星跟踪方法 | |
CN113048987A (zh) | 一种车载导航***定位方法 | |
JP2017215153A (ja) | 角速度センサ補正装置および角速度センサ補正方法 |
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 | ||
TA01 | Transfer of patent application right | ||
TA01 | Transfer of patent application right |
Effective date of registration: 20220303 Address after: 100083 unit 501, block AB, Dongsheng building, No. 8, Zhongguancun East Road, Haidian District, Beijing Applicant after: BEIJING MOMENTA TECHNOLOGY Co.,Ltd. Address before: 100083 room 28, 4 / F, block a, Dongsheng building, 8 Zhongguancun East Road, Haidian District, Beijing Applicant before: BEIJING CHUSUDU TECHNOLOGY Co.,Ltd. |
|
GR01 | Patent grant | ||
GR01 | Patent grant |