CN106053879A - 通过数据融合的失效操作的车辆速度估计 - Google Patents
通过数据融合的失效操作的车辆速度估计 Download PDFInfo
- Publication number
- CN106053879A CN106053879A CN201610212098.6A CN201610212098A CN106053879A CN 106053879 A CN106053879 A CN 106053879A CN 201610212098 A CN201610212098 A CN 201610212098A CN 106053879 A CN106053879 A CN 106053879A
- Authority
- CN
- China
- Prior art keywords
- speed
- vehicle
- signal
- estimation
- estimate
- 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.)
- Granted
Links
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01P—MEASURING LINEAR OR ANGULAR SPEED, ACCELERATION, DECELERATION, OR SHOCK; INDICATING PRESENCE, ABSENCE, OR DIRECTION, OF MOVEMENT
- G01P7/00—Measuring speed by integrating acceleration
-
- 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
- G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
- G01S13/02—Systems using reflection of radio waves, e.g. primary radar systems; Analogous systems
- G01S13/50—Systems of measurement based on relative movement of target
- G01S13/58—Velocity or trajectory determination systems; Sense-of-movement determination systems
- G01S13/588—Velocity or trajectory determination systems; Sense-of-movement determination systems deriving the velocity value from the range 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
- G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
- G01S13/86—Combinations of radar systems with non-radar systems, e.g. sonar, direction finder
-
- 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
-
- 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/52—Determining velocity
-
- 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
- G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
- G01S7/02—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S13/00
- G01S7/42—Diversity systems specially adapted for radar
-
- G—PHYSICS
- G08—SIGNALLING
- G08G—TRAFFIC CONTROL SYSTEMS
- G08G1/00—Traffic control systems for road vehicles
- G08G1/01—Detecting movement of traffic to be counted or controlled
- G08G1/0104—Measuring and analyzing of parameters relative to traffic conditions
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- Navigation (AREA)
- Control Of Driving Devices And Active Controlling Of Vehicle (AREA)
Abstract
本发明涉及通过数据融合的失效操作车辆速度估计,其中数据融合包括6-DOF IMU、GPS和雷达数据融合。公开了一种用于提供冗余车辆速度估计的方法。该方法包括从多个主要传感器提供传感器输出信号和从惯性测量单元提供惯性测量信号。该方法也包括利用主要传感器信号在主要模块中估计车辆速度,并且在预定时间段内缓冲来自主要模块的所估计的车辆速度值。该方法还包括确定主要传感器或主要模块中的一个或多个已失效,并且如果如此,则利用缓冲的车辆速度值和惯性测量信号在次要模块中估计车辆速度。如果GPS信号数据和/或静态物体的测距数据可用,该方法能够使用上述数据以在次要模块中改进估计的车辆速度。
Description
技术领域
本发明总体上涉及用于估计车辆速度的系统和方法,并且更具体地涉及用于在主要速度估计模块失效的情况中车辆速度的次要或冗余估计的系统和方法,其中次要速度估计模块采用来自6自由度(DOF)惯性测量单元(IMU)、GPS接收器和距离传感器的传感器输入。
背景技术
车辆正变得更加自主或者认识到目标是完全自主驾驶的车辆,即能够在最小限度的或没有驾驶员干预的情况下提供驾驶控制的车辆。自适应巡航控制系统已经可用多年,其中系统不仅维持设定速度,而且也将在检测到在主体车辆前方存在更缓慢地运动的车辆的情况中自动地使车辆减慢。当前存在的车辆控制系统包括自主停泊,其中车辆将自动地提供用于停泊车辆的转向控制。而且,如果驾驶员做出可以影响车辆稳定性和车道居中能力的生硬的转向改变,则存在的控制系统可以干预,其中车辆系统试图使车辆保持在行驶车道的中间附近。未来的车辆将有可能采用用于变道、通过、转离交通、转入交通、汇入交通、在交叉路口处穿过或转向等的自主系统。
车辆上的各种主动安全控制系统、驾驶员辅助系统和自主驾驶操作(诸如电子稳定性控制(ECS)、自适应巡航控制(ACC)、车道保持(LK)、变道(LC)等等),需要高度稳健且精确的模块以便估计各种车辆动力学。这样的模块对于提供关于车辆位置和速度的知识以控制车辆而言是必要的。
用于上述系统及其它的主动安全控制依赖于准确的车辆速度估计以便获得恰当的性能。现今,所提出的系统的这些类型依赖于车轮转速传感器和其它车辆运动学输入来提供车辆速度估计。但是,有时确定车辆速度的传感器和控制模块失效或者不正确地操作,其中车辆速度的损失可能是严重的。某些机动车辆安全要求(诸如ASIL-D)在如果发生主要估计处理器失效时,需要冗余车辆速度估计方法。例如,对于那些需要主动控制的系统,需要控制系统在失效事件之后五秒内提供准确的速度估计以便为驾驶员提供时间来掌控车辆。期望的将是利用车辆上的现有硬件提供这样的冗余速度估计方法以降低车辆成本。
发明内容
本公开描述了用于提供冗余车辆速度估计的系统和方法。所述方法包括提供来自多个主要传感器的传感器输出信号和提供来自惯性测量单元的惯性测量信号。所述方法也包括利用主要传感器信号在主要模块中估计车辆速度,并且在预定时间段中缓冲来自主要模块的所估计的车辆速度值。所述方法进一步包括确定主要传感器中的一个或多个或主要模块是否失效,并且如果是这样,则利用缓冲的车辆速度值和惯性测量信号在次要模块中估计车辆速度。所述方法能够使用GPS信号数据和/或由距离传感器(诸如雷达、光检测与测距和视觉系统)提供的距静态物体的速度数据,以便改善次要模块中的所估计的车辆速度(如果它们是可用的)。
本发明还公开了以下技术方案:
方案1. 一种用于提供移动平台的速度的冗余估计的方法,所述方法包括:
从多个主要传感器提供传感器输出信号;
从惯性测量单元提供惯性测量信号;
利用所述主要传感器信号在主要模块中估计所述速度;
在预定时间段内缓冲来自所述主要模式的估计的速度值;
确定所述主要传感器或所述主要模块中的一个或多个已失效;以及
如果所述主要传感器或所述主要模块已失效,则利用所述缓冲的速度值和所述惯性测量信号在次要模块中估计所述速度。
方案2. 根据方案1所述的方法,其中,提供传感器输出信号包括从车轮转速传感器、转向角传感器、偏航率传感器或者纵向加速度传感器提供传感器输出信号。
方案3. 根据方案1所述的方法,其中,从惯性测量单元提供惯性测量信号包括从6自由度惯性测量单元提供惯性测量信号,包括三个加速度值,包括横向、纵向和上/下加速度值;以及包括滚动、俯仰和偏航值的三个角速率值。
方案4. 根据方案1所述的方法,还包括提供GPS信号从而识别所述移动平台的位置,其中在次要模块中估计所述速度包括利用来自所述GPS信号的移动平台位置数据来改进所述速度的估计。
方案5. 根据方案1所述的方法,还包括提供从所述移动平台至检测到的静态物体的距离传感器信息,其中在次要模块中估计所述速度包括利用所述距离传感器信息来改进所述速度的估计。
方案6. 根据方案1所述的方法,其中,在次要模块中估计所述速度包括在主体坐标系中利用运动学。
方案7. 根据方案6所述的方法,其中在主体坐标系中利用运动学包括使用等式:
其中是所述主体坐标系b中所述惯性测量单元的中心处的速度,其对应于纵向、横向和上/下速度,即,其中假定所述移动平台总是接触地面,具体地在卡尔曼滤波中“虚拟”测量值;Ω是所述b-坐标系中角速度ω的斜对称矩阵;是惯性坐标系a中的地球重力向量,具体地;是从所述b-坐标系到惯性坐标系a的旋转矩阵;是所述b-坐标系中的加速度;并且ω是所述b-坐标系中的角速率;并且其中所述斜矩阵Ω是:
,
并且所述角速率ω是:
,
并且其中φ是滚动角度,θ是俯仰角度并且ψ是偏航角度。
方案8. 根据方案6所述的方法,其中,在所述次要模块中估计所述速度包括从平台运动的所述惯性测量信号中去除沿上/下方向的数据。
方案9. 根据方案6所述的方法,其中,在所述次要模块中估计所述速度包括从所述惯性测量信号中去除偏航数据。
方案10. 根据方案1所述的方法,其中,所述预定时间段是五秒。
方案11. 根据方案1所述的方法,其中,所述移动平台是车辆。
方案12. 一种用于提供车辆速度的冗余估计的方法,所述方法包括:
从多个主要传感器提供传感器输出信号;
从6自由度惯性测量单元提供惯性测量信号,包括三个加速度值,包括横向、纵向和上/下加速度值;以及包括滚动、俯仰和偏航值的三个角速率值;
提供识别所述车辆的位置的GPS信号;
提供从所述车辆至检测到的静态物体的距离传感器信息;
利用所述主要传感器信号在主要模块中估计所述车辆速度;
在预定时间段内缓冲来自所述主要模式的估计的所述车辆速度;
确定所述主要传感器或所述主要模块中的一个或多个已失效;以及
利用缓冲的所述车辆速度值和所述惯性测量信号在次要模块中估计所述车辆速度,包括利用来自所述GPS信号的车辆位置数据和所述距离传感器信息以改进所述车辆速度的估计。
方案13. 根据方案12所述的方法,其中,提供传感器输出信号包括从车轮转速传感器、转向角传感器、偏航率传感器或者纵向加速度传感器提供传感器输出信号。
方案14. 根据方案12所述的方法,其中,在次要模块中估计所述车辆速度包括在车辆主体坐标系中利用车辆运动学。
方案15. 根据方案14所述的方法,其中,在车辆主体坐标系中利用车辆运动学包括利用等式:
其中是所述车辆主体坐标系b中的所述惯性测量单元的中心处的车辆速度,其对应于车辆纵向、横向和上/下速度,即,其中假定所述车辆的车轮总是接触地面,具体地在卡尔曼滤波中“虚拟”测量值;Ω是所述b-坐标系中角速度ω的斜对称矩阵;是惯性坐标系a中的地球重力向量,具体地;是从所述b-坐标系到所述惯性坐标系a的旋转矩阵;是所述b-坐标系中的加速度;并且ω是所述b-坐标系中的角速率;并且其中所述斜矩阵Ω是:
,
并且所述角速率ω是:
,
并且其中φ是滚动角度,θ是俯仰角度并且ψ是偏航角度。
方案16. 根据方案14所述的方法,其中,在所述次要模块中估计所述车辆速度包括从车辆运动的所述惯性测量信号中去除沿上/下方向的数据。
方案17. 根据方案14所述的方法,其中,在所述次要模块中估计所述车辆速度包括从所述惯性测量信号中去除偏航数据。
方案18. 一种用于提供车辆速度的冗余估计的系统,所述系统包括:
提供传感器输出信号的多个主要传感器;
提供惯性测量信号的惯性测量单元;
利用所述主要传感器信号估计所述车辆速度的主要器件;
用于在预定时间段内缓冲所估计的车辆速度值的器件;
用于确定所述主要传感器或所述主要器件中的一个或多个已失效的器件;以及
用于利用所述缓冲的车辆速度值和所述惯性测量信号在次要模块中估计所述车辆速度的次要器件。
方案19. 根据方案18所述的系统,其中,所述惯性测量单元是6自由度惯性测量单元,其提供包括横向、纵向和上/下加速度值的三个加速度值以及包括滚动、俯仰和偏航值的三个角速率值。
方案20. 根据方案18所述的系统,还包括用于提供识别所述车辆的位置的GPS信号的GPS接收器和提供从所述车辆至检测到的静态物体的距离信息的距离传感器,其中用于在次要模块中估计所述速度的所述器件包括利用来自所述GPS信号的位置数据和所述距离传感器信息来改进所述速度的估计。
结合附图从以下描述和所附权利要求中将显而易见到本发明的附加特征。
附图说明
图1是包括用于估计车辆速度的冗余系统的车辆的简单示意图;
图2是包括用于估计车辆速度的主要模块和用于估计车辆速度的次要模块的控制系统的框图;
图3是示出用于估计车辆速度的车辆运动学的流程图;
图4是示出用于在主要模块失效之前估计车辆姿态的过程的流程图;
图5是沿道路行驶的车辆的图示,其示出用于利用测距数据改善车辆速度的估计的技术中所采用的变量;以及
图6是示出用于利用GPS信号或测距数据来测量车辆纵向速度的过程的流程图。
具体实施方式
本发明实施例的下述讨论涉及用于在主要车辆速度估计失效的情况下估计车辆速度的次要技术,其在本质上仅是示例性的并且不以任何方式试图限制本发明或其应用或使用。例如,如所讨论的那样,系统和方法具有用于估计车辆速度的具体应用。但是,如本领域技术人员将认识到的那样,系统和方法可以具有用于其它移动平台(诸如火车、机器、拖拉机、船舶、娱乐车辆等上)的应用。
如下文将详细描述的那样,本发明提出了用于在用于估计车辆速度的主要系统失效的情况下估计车辆速度的次要系统和方法。在一种实施例中,次要速度估计系统具有下述应用:用于在主要技术已失效之后的特定时间段(诸如五秒)内提供车辆速度估计,如车辆自主或半自主驾驶控制系统的机动车辆标准所要求的那样,以允许车辆驾驶员有时间掌控车辆。次要速度估计技术采用来自在主要速度估计系统中未被用于确定车辆速度的现有车辆传感器的数据。在一种实施例中,次要速度估计系统采用来自6-DOF IMU、GPS接收器和距离传感器(诸如雷达、光检测和测距、视觉系统等等)的信号,它们能够融合在一起以确定车辆速度。在主要速度估计系统运转的期间,针对若干样本点缓冲所计算的车辆速度,其中针对每个样本周期不断地更新缓冲的速度信息。通过利用由主要系统计算的缓冲的车辆速度数据,在主要速度估计系统失效之后的某些时间段内,6-DOF IMU传感器数据被用于推算车辆速度。来自6-DOF IMU的这种数据能够结合GPS信号(如果它们是可用的)来使用。此外,来自车辆周围的静止物体的速度测量数据能够结合所推算的6-DOF IUM数据和GPS信号中的一者或二者来使用。
图1是包括冗余车辆速度估计系统12的车辆10的简化的示意图。系统12包括主要车辆速度估计模块14和次要车辆速度估计模块16,每个车辆速度估计模块均能够生成对车辆横向和纵向速度的估计。由模块14和16生成的车辆横向和纵向速度被提供至一个或多个控制器18,所述控制器18控制车辆的一些方面(诸如自主驾驶系统、主动安全控制系统、稳定性控制系统等等)。主要模块14接收来自若干运动学传感器的信号(总体上在框20处表示),其使用这些信号来估计车辆速度。这样的传感器可以包括车轮转速传感器、方向盘角度传感器、偏航率传感器、纵向和横向加速度传感器等等。如果这些传感器中的一个或多个失效或者模块14本身失效,则系统12将检测到该失效,并且使用来自次要模块16而不是主要模块14的车辆速度估计信号。次要模块16使用不同于主要模块14的传感器,诸如6-DOF IMU、GPS接收器、距离传感器等等,总体上在框22处表示。
图2是类似于系统12的车辆速度估计系统30的示意性框图。系统30包括主要车辆速度估计模块32和次要车辆速度估计模块34,其向主动安全控制器36提供所估计的车辆横向和纵向速度信号。模块32包括计算车辆速度估计信号的主要函数处理器38和确定模块32的任何部分或其输入是否已发生失效的失效-安全判定(decider)处理器40。同样地,次要模块34包括次要函数处理器42,其计算仅当主要模块32失效时使用的车辆速度估计信号。次要模块34也包括失效-安全判定处理器44,其确定次要模块34是否恰当地运行。主要模块32和次要模块34从车辆通信信道或总线48接收传感器信号。若干传感器46(诸如车轮转速传感器、转向角传感器、偏航率传感器、纵向和纬度加速度传感器等等)在总线48上提供由主要模块32获取的传感器信号。此外,系统30包括6-DOF IMU 50、GPS接收器52和距离传感器54,它们也向通信总线48提供它们的信号并且可用于次要模块34。
次要模块34也接收来自主要函数处理器38的速度信号并且在至少某些滚动时间段(诸如五秒)缓冲那些速度信号。如果检测到主要模块32中的故障,则次要模块34读取缓冲的数据并且利用来自6-DOF IMU 50的角速率和加速度信号推算该数据以提供车辆速度估计。6-DOF IMU 50是某些车辆上可用的公知传感器,其提供六个变化速率的测量值,具体地,车辆滚动、俯仰和偏航的角速率测量值ω,以及纵向加速度、横向加速度和上/下加速度的加速度测量值f。在次要模块34中利用储存的速度值和6-DOF IMU数据对车辆速度的推算能够利用来自GPS接收器52的GPS数据和/或来自检测静止物体(如果是可用的)的距离传感器54的数据来校正或扩充,其中车辆和所检测的静止物体之间的间距的距离变化率能够被用于帮助确定车辆速度。如果主要模块32从失效或故障恢复,则系统30的操作转变回主要模块32,其中主要模块32的状态变量(诸如车辆纵向速度、车辆横向速度、滚动角度和俯仰角度)使用通信总线48上的来自次要模块34的最近的状态变量来初始化。
以下讨论描述了来自6-DOF IMU 50的速率变化数据如何能够用于在主要模块32已失效之后的一些时间段内利用缓冲的车辆速度数据预测车辆速度。车辆主体坐标系b中的车辆速度的常微分方程(ODE)的车辆运动学限定为:
,(1)
其中是车辆主体坐标系b中IMU 50的中心处的车辆速度,其对应于车辆纵向、横向和上/下速度,即,其中假定车辆的车轮总是接触地面,具体地在卡尔曼滤波中“虚拟”测量值;Ω是在b-坐标系中的角速度ω的斜对称矩阵;是在惯性坐标系a中的地球重力向量,具体地;是从b-坐标系到惯性坐标系a的旋转矩阵;是由b-坐标系中的加速度计测量的车辆加速度(IMU数据);并且ω是由b-坐标系中的陀螺仪测量的角速率(IMU数据);并且其中斜矩阵Ω是:
,(2)
并且角速率ω是:
,(3)
其中Φ是滚动角度,θ是俯仰角度并且ψ是偏航角度。
如果值是车辆的重心(CG)在惯性坐标系(a-坐标系)中的速度,则由于牛顿定律:
,(4)
因为:
,(5)
(6)
注意旋转矩阵的性质以及车辆主体坐标系中观察到的加速度向量。使等式(6)乘以旋转矩阵得出车辆主体坐标系b中的车辆速度的ODE。
图3是示出等式(1)如何被用于预测车辆速度值的流程图60。在框62处提供来自主要模块32的最后缓冲的车辆速度值,其具有三个分量,即,纵向速度、横向速度和车辆上/下速度,如:
。(7)。
在框64处对主体坐标系中的车辆速度求积分以获得车辆速度值。在框66处速度值被用于获得上速度误差,其在乘法器70中乘以在框68处提供的卡尔曼滤波增益K以获得。然后,在减法器72中从车辆速度值减去滤波的上速度误差,以在线74上获得估计的纵向和横向车辆速度信号作为来自次要函数处理器42的输出。
关于如何获得速度值的讨论提供如下。在框76处提供来自6-DOF IMU 50的俯仰、滚动和偏航角速率ω,并且在框78处将其用于计算斜矩阵Ω。斜矩阵Ω在乘法器80中乘以车辆速度值并且由逆变器82求逆以获得作为等式(1)的第一分量的值。
在次要模块34中由次要函数处理器42预测车辆速度需要车辆姿态(即滚动、俯仰和偏航)的初始值,其确定了从惯性坐标系统(a-坐标系)到车辆坐标系统(b-坐标系)的旋转矩阵。在这种示例中,值对于方位旋转是不变的,并且因此仅需要车辆的滚动和俯仰角,其中偏航角被设定为零。对于静止车辆,车辆的滚动和俯仰姿态能够从重力向量和IMU测量的加速度之间的二维角度计算出。然而,对于运动的车辆,能够利用来自主要模块32的车辆速度向量值确定滚动和俯仰的姿态。来自框78的斜矩阵Ω由逆变器58求逆以获得斜矩阵的逆矩阵-Ω,其在乘法器98中乘以线56上提供的旋转矩阵的估计以获得值。向积分算子86提供在检测到主要模块32的故障之前在框84处提供的最后缓冲的旋转矩阵和来自乘法器98的值以生成新的旋转矩阵。图4和下述讨论示出了当主要模块32起作用时如何确定这些缓冲的旋转矩阵。
由旋转矩阵形成的空间是非欧几里得(non-Euclidean)的,并且因此当处理车辆姿态估计时需要特别小心。例如,来自积分算子86的值可以不是旋转矩阵。需要奇异值分解(SVD)投影来提供值的归一化以将其识别为旋转矩阵。计算SVD矩阵U、S和V,使得:
,(8)
。(9)。
在框88处通过如下计算获得归一化的旋转矩阵:
。(10)。
从旋转矩阵获得包括滚动角φ、俯仰角θ和偏航角ψ的车辆姿态。车辆坐标系统(b-坐标系)附接至车辆的主体,并且a-坐标系是惯性坐标系统。
旋转矩阵能够由三个欧拉角表示为:
。(11)。
从a-坐标系至b-坐标系的旋转矩阵被提供为:
,(12)
其中并且。
从b-坐标系到a-坐标系的旋转矩阵的导数确定如下。考虑到车辆的滚动φ、俯仰θ和偏航ψ运动的无穷小的角度,对应的旋转矩阵R能够由近似,其中是旋转角度的斜对称矩阵表示:
,(13)
其中x代表斜对称矩阵算子,即,。
在时间t,b-坐标系中的向量p能够在a-坐标系中表示为。现在考虑在时间时:
。(14)。
旋转矩阵的时间导数限定为:
,(15)
其中是斜对称矩阵角速率ω=,即。注意到等式(15)等价于车辆姿态:
(16)
来自框88的旋转矩阵和来自框90的重力向量由乘法器92相乘以获得等式(1)的第二分量,具体地。在框94处提供来自6-DOF IMU 50的三个加速度值以用于等式(1)的第三分量,并且所有分量、、和由加法器96加在一起以获得发送至积分框64的速度值。
图4是流程图100,其示出用于利用在框102处由主要模块32提供的速度向量确定或估计失效前的车辆姿态θ的过程。速度向量在框104处被低通滤波,并且由求导框106提供其变化率以获得速度向量值的变化率。在框108处提供IMU角速率ω,并且在框110处提供IMU加速度变化率。在框112处利用角速率ω计算斜矩阵Ω,如与上文描述的框78中相同的那样。然后在乘法器114中斜矩阵Ω乘以速度值以获得值。加速度变化率由逆变器116求逆,并且值、和由加法器118相加以获得,在框120处利用滚动-俯仰角β和来自框122的重力向量从该值如下估计旋转矩阵:
,(17)
其中:
。(18)
SVD矩阵U、S和V被计算为:
,(19)
其中:
(20)
旋转矩阵的估计是:
。 (21)。
针对滚动角φ和俯仰角θ的车辆姿态θ被计算为:
,(22)
。
(23) 。
如上所述,如果来自GPS接收器52的GPS数据可用,则其能够被用于结合由IMU 50提供的数据来改善预测的车辆速度,原因在于随着误差累积,预测性能降低。GPS信号根据被用作车辆纵向速度的测量值,其中是车辆纵向速度,是车辆横向速度,并且ε是零均值高斯分布误差。当新的GPS数据到达且信号接收高于预定的信噪比时,当发生GPS校正时应用卡尔曼-布西滤波器来校正车辆速度估计。
而且,如上所述,来自距离传感器的速度数据能够被用于改善车辆速度估计量。作为示例实施例,下文参考图5讨论利用雷达数据的校正,其中图5示出沿道路130行驶的车辆128的图示126。车辆128包括前雷达传感器132和后雷达传感器134,其检测沿着道路132的物体,例如分别检测道路标志136和138。利用以下状态变量:车辆纵向速度和横向速度、来自IMU
50的车辆偏航率、二维斜对称矩阵、由雷达传感器132检测到的在车辆坐标系中位于点处的第i个物体、距离变化率(range rate)、由雷达传感器132检测到的在车辆坐标系中位于点处的第j个物体,以及距离变化率,能够获得两个测量值等式,如:
,(24)
,(25)
其中和是由雷达多普勒测量值引入的零均值高斯分布误差项。因此卡尔曼-布西滤波器能够被设计成基于测量值等式校正车辆速度预测。
图6是流程图140,其示出用于利用GPS数据和/或雷达数据(如果其可用)以帮助确定所估计的车辆速度的过程。算法开始于框142并且在决策菱形144处确定新的GPS数据或雷达数据是否可用。如果在决策菱形144处新的GPS或雷达数据不可用,则在框146处算法使用如上讨论的来自IMU数据的预测的车辆速度。如果在决策菱形144处新的GPS信号或雷达数据是可用的,则在决策菱形148处算法确定信号接收是否足够良好或者雷达静态目标是否存在。如果在决策菱形148处GPS信号接收并不令人满意或者雷达静态目标不存在,则算法进行到框146以使用车辆预测速度数据。如果在决策菱形148处GPS信号良好或者雷达传感器正拾取静态目标,则在框150处算法使用那些值中的一个或两个以从所预测的车辆速度信号中去除误差。由框150提供的基于卡尔曼-布西滤波的校正在框68处提供为误差校正。
如本领域技术人员将很好地理解的,本文讨论的用以描述本发明的多个和各种步骤和过程可以指的是由计算机、处理器或利用电气现象操纵和/或传输数据的其它电子计算装置来执行的操作。那些计算机和电子装置可以采用各种易失性和/或非易失性存储器,包括带有储存于其上的可执行程序的非瞬时计算机可读介质,所述可执行程序包括能够由计算机或处理器执行的各种代码或者可执行指令,其中存储器和/或计算机可读介质可以包括所有形式和类型的存储器和其它计算机可读介质。
前述讨论仅公开并描述了本发明的示例性实施例。本领域技术人员从这样的讨论和从附图和权利要求中将容易地认识到,能够在不背离如所附权利要求所限定的本发明的精神和范围的情况下在其中做出各种改变、改型和变型。
Claims (10)
1. 一种用于提供移动平台的速度的冗余估计的方法,所述方法包括:
从多个主要传感器提供传感器输出信号;
从惯性测量单元提供惯性测量信号;
利用所述主要传感器信号在主要模块中估计所述速度;
在预定时间段内缓冲来自所述主要模式的估计的速度值;
确定所述主要传感器或所述主要模块中的一个或多个已失效;以及
如果所述主要传感器或所述主要模块已失效,则利用所述缓冲的速度值和所述惯性测量信号在次要模块中估计所述速度。
2. 根据权利要求1所述的方法,其中,提供传感器输出信号包括从车轮转速传感器、转向角传感器、偏航率传感器或者纵向加速度传感器提供传感器输出信号。
3. 根据权利要求1所述的方法,其中,从惯性测量单元提供惯性测量信号包括从6自由度惯性测量单元提供惯性测量信号,包括三个加速度值,包括横向、纵向和上/下加速度值;以及包括滚动、俯仰和偏航值的三个角速率值。
4. 根据权利要求1所述的方法,还包括提供GPS信号从而识别所述移动平台的位置,其中在次要模块中估计所述速度包括利用来自所述GPS信号的移动平台位置数据来改进所述速度的估计。
5. 根据权利要求1所述的方法,还包括提供从所述移动平台至检测到的静态物体的距离传感器信息,其中在次要模块中估计所述速度包括利用所述距离传感器信息来改进所述速度的估计。
6. 根据权利要求1所述的方法,其中,在次要模块中估计所述速度包括在主体坐标系中利用运动学。
7. 根据权利要求6所述的方法,其中在主体坐标系中利用运动学包括使用等式:
其中是所述主体坐标系b中所述惯性测量单元的中心处的速度,其对应于纵向、横向和上/下速度,即,其中假定所述移动平台总是接触地面,具体地在卡尔曼滤波中“虚拟”测量值;Ω是所述b-坐标系中角速度ω的斜对称矩阵;是惯性坐标系a中的地球重力向量,具体地;是从所述b-坐标系到惯性坐标系a的旋转矩阵;是所述b-坐标系中的加速度;并且ω是所述b-坐标系中的角速率;并且其中所述斜矩阵Ω是:
,
并且所述角速率ω是:
,
并且其中φ是滚动角度,θ是俯仰角度并且ψ是偏航角度。
8. 根据权利要求6所述的方法,其中,在所述次要模块中估计所述速度包括从平台运动的所述惯性测量信号中去除沿上/下方向的数据。
9. 一种用于提供车辆速度的冗余估计的方法,所述方法包括:
从多个主要传感器提供传感器输出信号;
从6自由度惯性测量单元提供惯性测量信号,包括三个加速度值,包括横向、纵向和上/下加速度值;以及包括滚动、俯仰和偏航值的三个角速率值;
提供识别所述车辆的位置的GPS信号;
提供从所述车辆至检测到的静态物体的距离传感器信息;
利用所述主要传感器信号在主要模块中估计所述车辆速度;
在预定时间段内缓冲来自所述主要模式的估计的所述车辆速度;
确定所述主要传感器或所述主要模块中的一个或多个已失效;以及
利用缓冲的所述车辆速度值和所述惯性测量信号在次要模块中估计所述车辆速度,包括利用来自所述GPS信号的车辆位置数据和所述距离传感器信息以改进所述车辆速度的估计。
10. 一种用于提供车辆速度的冗余估计的系统,所述系统包括:
提供传感器输出信号的多个主要传感器;
提供惯性测量信号的惯性测量单元;
利用所述主要传感器信号估计所述车辆速度的主要器件;
用于在预定时间段内缓冲所估计的车辆速度值的器件;
用于确定所述主要传感器或所述主要器件中的一个或多个已失效的器件;以及
用于利用所述缓冲的车辆速度值和所述惯性测量信号在次要模块中估计所述车辆速度的次要器件。
Applications Claiming Priority (2)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
US14/680,894 US9645250B2 (en) | 2015-04-07 | 2015-04-07 | Fail operational vehicle speed estimation through data fusion of 6-DOF IMU, GPS, and radar |
US14/680894 | 2015-04-07 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN106053879A true CN106053879A (zh) | 2016-10-26 |
CN106053879B CN106053879B (zh) | 2019-08-20 |
Family
ID=56986211
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201610212098.6A Active CN106053879B (zh) | 2015-04-07 | 2016-04-07 | 通过数据融合的失效操作的车辆速度估计 |
Country Status (3)
Country | Link |
---|---|
US (1) | US9645250B2 (zh) |
CN (1) | CN106053879B (zh) |
DE (1) | DE102016106294A1 (zh) |
Cited By (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109421739A (zh) * | 2017-08-28 | 2019-03-05 | 通用汽车环球科技运作有限责任公司 | 用于监控自主车辆的方法和设备 |
CN109664922A (zh) * | 2018-12-29 | 2019-04-23 | 交控科技股份有限公司 | 一种基于加速度计的融合测速处理方法及装置 |
CN110100151A (zh) * | 2017-01-04 | 2019-08-06 | 高通股份有限公司 | 在视觉惯性测距中使用全球定位系统速度的系统及方法 |
CN112396828A (zh) * | 2019-08-16 | 2021-02-23 | 通用汽车环球科技运作有限责任公司 | 在交通工具之间用于感知共享的方法和设备 |
WO2021196983A1 (zh) * | 2020-03-30 | 2021-10-07 | 华为技术有限公司 | 一种自运动估计的方法及装置 |
WO2021217604A1 (en) * | 2020-04-30 | 2021-11-04 | Baidu.Com Times Technology (Beijing) Co., Ltd. | Dual inertial measurement units for inertial navigation system |
CN113767264A (zh) * | 2020-03-05 | 2021-12-07 | 深圳市大疆创新科技有限公司 | 参数标定方法、装置、系统和存储介质 |
CN114126944A (zh) * | 2019-07-02 | 2022-03-01 | 三菱电机株式会社 | 滚动时域状态估计器 |
CN114370919A (zh) * | 2022-01-10 | 2022-04-19 | 浙江东鼎电子股份有限公司 | 一种动态公路车辆专用平板称重系统及其称重方法 |
CN114660330A (zh) * | 2018-08-16 | 2022-06-24 | Aptiv技术有限公司 | 确定估计速度的不确定性估计的方法 |
Families Citing this family (20)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP6021189B2 (ja) | 2011-02-21 | 2016-11-09 | トランスロボティックス,インク. | 距離および/または移動を感知するためのシステムおよび方法 |
EP2904420A4 (en) | 2012-10-05 | 2016-05-25 | Transrobotics Inc | SYSTEMS AND METHODS FOR HIGH RESOLUTION DISTANCE DETECTION AND APPLICATIONS |
US10690493B2 (en) * | 2016-08-18 | 2020-06-23 | Myriad Sensors, Inc. | Wireless sensor device and software system for measuring linear position of a rotating object |
FR3055986B1 (fr) * | 2016-09-13 | 2018-10-12 | Peugeot Citroen Automobiles Sa | Dispositif de controle de la reinitialisation d'un calculateur embarque automobile |
JP6465319B2 (ja) * | 2017-03-31 | 2019-02-06 | 株式会社Subaru | 車両の走行制御装置 |
DE102017218643A1 (de) | 2017-10-19 | 2019-04-25 | Volkswagen Aktiengesellschaft | Funktionsmodul, Steuereinheit für ein Betriebsassistenzsystem und Arbeitsvorrichtung |
WO2019150483A1 (ja) * | 2018-01-31 | 2019-08-08 | パイオニア株式会社 | 速度算出装置、速度算出方法、及び、プログラム |
US20200132832A1 (en) * | 2018-10-25 | 2020-04-30 | TransRobotics, Inc. | Technologies for opportunistic synthetic aperture radar |
CN109703466B (zh) * | 2019-01-03 | 2022-04-12 | 京东方科技集团股份有限公司 | 抬头显示装置、驱动方法、车辆及电子设备 |
US11703593B2 (en) | 2019-04-04 | 2023-07-18 | TransRobotics, Inc. | Technologies for acting based on object tracking |
DE102019111642B3 (de) * | 2019-05-06 | 2020-06-04 | Sick Ag | Absichern der Umgebung eines Fahrzeugs |
US11821990B2 (en) | 2019-11-07 | 2023-11-21 | Nio Technology (Anhui) Co., Ltd. | Scene perception using coherent doppler LiDAR |
CN110906923B (zh) * | 2019-11-28 | 2023-03-14 | 重庆长安汽车股份有限公司 | 车载多传感器紧耦合融合定位方法、系统、存储介质及车辆 |
WO2021116944A1 (en) | 2019-12-09 | 2021-06-17 | Thales Canada Inc. | Stationary status resolution system |
CN111522001B (zh) * | 2020-04-30 | 2022-10-04 | 深圳承泰科技有限公司 | 一种汽车测速方法、汽车雷达以及汽车 |
US11332152B2 (en) * | 2020-05-29 | 2022-05-17 | GM Global Technology Operations LLC | Method and apparatus for determining a velocity of a vehicle |
KR102480506B1 (ko) * | 2020-10-21 | 2022-12-22 | 현대모비스 주식회사 | 차량정보에 기반한 hud 보호모드 해제 장치 및 방법 |
DE102020131669B3 (de) | 2020-11-30 | 2021-12-23 | Daimler Ag | Verfahren zur Erkennung von Fehlfunktionen in Inertialmesseinheiten |
US20220242442A1 (en) * | 2021-02-03 | 2022-08-04 | John S. McNeely | Drive trajectory system and device |
DE102021203944A1 (de) * | 2021-04-21 | 2022-10-27 | Robert Bosch Gesellschaft mit beschränkter Haftung | Verfahren zum Regeln eines Antiblockiersystems |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101320089A (zh) * | 2007-06-05 | 2008-12-10 | 通用汽车环球科技运作公司 | 用于车辆动力估计的雷达、激光雷达和摄像机增强的方法 |
CN101526552A (zh) * | 2008-02-08 | 2009-09-09 | 凯尔西-海耶斯公司 | 用于运动传感器的故障安全测试 |
CN101855116A (zh) * | 2007-11-09 | 2010-10-06 | 米其林技术公司 | 利用确定车辆相对于地面的瞬时速度控制车辆的系统 |
CN101915586A (zh) * | 2010-07-22 | 2010-12-15 | 北京全路通信信号研究设计院 | 一种轨道车辆测速测距系统及方法 |
US20140278206A1 (en) * | 2013-03-15 | 2014-09-18 | Cambridge Mobile Telematics | Inference of vehicular trajectory characteristics with personal mobile devices |
CN104122410A (zh) * | 2013-04-24 | 2014-10-29 | 精工爱普生株式会社 | 物理量检测电路、物理量检测装置、电子设备以及移动体 |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US6718248B2 (en) * | 2002-06-19 | 2004-04-06 | Ford Global Technologies, Llc | System for detecting surface profile of a driving road |
-
2015
- 2015-04-07 US US14/680,894 patent/US9645250B2/en active Active
-
2016
- 2016-04-06 DE DE102016106294.4A patent/DE102016106294A1/de active Pending
- 2016-04-07 CN CN201610212098.6A patent/CN106053879B/zh active Active
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101320089A (zh) * | 2007-06-05 | 2008-12-10 | 通用汽车环球科技运作公司 | 用于车辆动力估计的雷达、激光雷达和摄像机增强的方法 |
CN101855116A (zh) * | 2007-11-09 | 2010-10-06 | 米其林技术公司 | 利用确定车辆相对于地面的瞬时速度控制车辆的系统 |
CN101526552A (zh) * | 2008-02-08 | 2009-09-09 | 凯尔西-海耶斯公司 | 用于运动传感器的故障安全测试 |
CN101915586A (zh) * | 2010-07-22 | 2010-12-15 | 北京全路通信信号研究设计院 | 一种轨道车辆测速测距系统及方法 |
US20140278206A1 (en) * | 2013-03-15 | 2014-09-18 | Cambridge Mobile Telematics | Inference of vehicular trajectory characteristics with personal mobile devices |
CN104122410A (zh) * | 2013-04-24 | 2014-10-29 | 精工爱普生株式会社 | 物理量检测电路、物理量检测装置、电子设备以及移动体 |
Cited By (18)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110100151A (zh) * | 2017-01-04 | 2019-08-06 | 高通股份有限公司 | 在视觉惯性测距中使用全球定位系统速度的系统及方法 |
CN109421739A (zh) * | 2017-08-28 | 2019-03-05 | 通用汽车环球科技运作有限责任公司 | 用于监控自主车辆的方法和设备 |
CN109421739B (zh) * | 2017-08-28 | 2022-04-12 | 通用汽车环球科技运作有限责任公司 | 用于监控自主车辆的方法和设备 |
CN114660330B (zh) * | 2018-08-16 | 2024-02-13 | Aptiv技术有限公司 | 确定估计速度的不确定性估计的方法 |
CN114660330A (zh) * | 2018-08-16 | 2022-06-24 | Aptiv技术有限公司 | 确定估计速度的不确定性估计的方法 |
CN109664922A (zh) * | 2018-12-29 | 2019-04-23 | 交控科技股份有限公司 | 一种基于加速度计的融合测速处理方法及装置 |
CN109664922B (zh) * | 2018-12-29 | 2020-12-18 | 交控科技股份有限公司 | 一种基于加速度计的融合测速处理方法及装置 |
CN114126944A (zh) * | 2019-07-02 | 2022-03-01 | 三菱电机株式会社 | 滚动时域状态估计器 |
CN114126944B (zh) * | 2019-07-02 | 2024-03-05 | 三菱电机株式会社 | 滚动时域状态估计器 |
CN112396828A (zh) * | 2019-08-16 | 2021-02-23 | 通用汽车环球科技运作有限责任公司 | 在交通工具之间用于感知共享的方法和设备 |
US11574538B2 (en) | 2019-08-16 | 2023-02-07 | GM Global Technology Operations LLC | Method and apparatus for perception-sharing between vehicles |
CN113767264A (zh) * | 2020-03-05 | 2021-12-07 | 深圳市大疆创新科技有限公司 | 参数标定方法、装置、系统和存储介质 |
WO2021196983A1 (zh) * | 2020-03-30 | 2021-10-07 | 华为技术有限公司 | 一种自运动估计的方法及装置 |
CN113874680A (zh) * | 2020-04-30 | 2021-12-31 | 百度时代网络技术(北京)有限公司 | 用于惯性导航系统的双惯性测量单元 |
US11592581B2 (en) | 2020-04-30 | 2023-02-28 | Baidu Usa Llc | Dual inertial measurement units for inertial navigation system |
WO2021217604A1 (en) * | 2020-04-30 | 2021-11-04 | Baidu.Com Times Technology (Beijing) Co., Ltd. | Dual inertial measurement units for inertial navigation system |
CN114370919A (zh) * | 2022-01-10 | 2022-04-19 | 浙江东鼎电子股份有限公司 | 一种动态公路车辆专用平板称重系统及其称重方法 |
CN114370919B (zh) * | 2022-01-10 | 2023-08-22 | 浙江东鼎电子股份有限公司 | 一种动态公路车辆专用平板称重系统及其称重方法 |
Also Published As
Publication number | Publication date |
---|---|
US20160299234A1 (en) | 2016-10-13 |
DE102016106294A1 (de) | 2016-10-13 |
CN106053879B (zh) | 2019-08-20 |
US9645250B2 (en) | 2017-05-09 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN106053879B (zh) | 通过数据融合的失效操作的车辆速度估计 | |
US9753144B1 (en) | Bias and misalignment compensation for 6-DOF IMU using GNSS/INS data | |
US9183463B2 (en) | Orientation model for a sensor system | |
CN105984464B (zh) | 车辆控制装置 | |
KR102042945B1 (ko) | 자율 주행 차량을 위한 차로 연석 보조의 차로 이탈 점검 및 차로 유지 시스템 | |
US8200452B2 (en) | Attitude-angle detecting apparatus and attitude-angle detecting method | |
US8280586B2 (en) | Determination of the actual yaw angle and the actual slip angle of a land vehicle | |
Park et al. | Integrated observer approach using in-vehicle sensors and GPS for vehicle state estimation | |
US11874660B2 (en) | Redundant lateral velocity determination and use in secondary vehicle control systems | |
CN110316197B (zh) | 倾斜估算方法、倾斜估算装置以及存储程序的非暂时性计算机可读存储介质 | |
JP7036080B2 (ja) | 慣性航法装置 | |
KR102331312B1 (ko) | 차량 내부 센서, 카메라, 및 gnss 단말기를 이용한 3차원 차량 항법 시스템 | |
US10723360B2 (en) | Apparatus and method for estimating radius of curvature of vehicle | |
CN107076559B (zh) | 用于匹配导航系统的方法和系统 | |
US11946746B2 (en) | Method for satellite-based detection of a vehicle location by means of a motion and location sensor | |
JP4899626B2 (ja) | 走行制御装置 | |
JP2009198185A (ja) | 車両位置姿勢推測装置 | |
EP4001844A1 (en) | Method and apparatus with localization | |
JP6080998B1 (ja) | 車両制御情報生成装置および車両制御情報生成方法 | |
US10775804B1 (en) | Optical array sensor for use with autonomous vehicle control systems | |
CN107107913B (zh) | 用于确定单辙车辆位置的方法以及用于实施该方法的装置 | |
JP7433363B2 (ja) | 車両制御装置、及び車両制御方法 | |
JP7241801B2 (ja) | 車両制御装置、及び車両制御方法 | |
CN117163060A (zh) | 开环与闭环混合路径规划系统与方法 | |
JP2007139680A (ja) | 車両位置検出装置および車両位置検出方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
C10 | Entry into substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |