CN111102978B - 一种车辆运动状态确定的方法、装置及电子设备 - Google Patents

一种车辆运动状态确定的方法、装置及电子设备 Download PDF

Info

Publication number
CN111102978B
CN111102978B CN201911235777.5A CN201911235777A CN111102978B CN 111102978 B CN111102978 B CN 111102978B CN 201911235777 A CN201911235777 A CN 201911235777A CN 111102978 B CN111102978 B CN 111102978B
Authority
CN
China
Prior art keywords
vehicle motion
vehicle
variable
predicted value
error
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
Application number
CN201911235777.5A
Other languages
English (en)
Other versions
CN111102978A (zh
Inventor
陈海波
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shenlan Robot Shanghai Co ltd
Original Assignee
Deep Blue Technology Shanghai Co Ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Deep Blue Technology Shanghai Co Ltd filed Critical Deep Blue Technology Shanghai Co Ltd
Priority to CN201911235777.5A priority Critical patent/CN111102978B/zh
Publication of CN111102978A publication Critical patent/CN111102978A/zh
Application granted granted Critical
Publication of CN111102978B publication Critical patent/CN111102978B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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/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/18—Stabilised platforms, e.g. by gyroscope
    • 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
    • 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
    • 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
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/02—Systems using the reflection of electromagnetic waves other than radio waves
    • G01S17/06—Systems determining position data of a target
    • G01S17/42—Simultaneous measurement of distance and other co-ordinates
    • 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
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/02—Systems using the reflection of electromagnetic waves other than radio waves
    • G01S17/50—Systems of measurement based on relative movement of target
    • G01S17/58—Velocity or trajectory determination systems; Sense-of-movement determination systems
    • 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

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Electromagnetism (AREA)
  • Automation & Control Theory (AREA)
  • Manufacturing & Machinery (AREA)
  • Navigation (AREA)

Abstract

本发明提供一种车辆运动状态确定的方法、装置及电子设备，包括：根据IMU输出的角速度及加速度，利用捷联惯性导航算法解算得到车辆运动状态预测值并通过序贯卡尔曼滤波及误差状态方程，得到车辆运动误差状态变量的预测值；获取GNSS确定的位置和速度、激光雷达确定的位置和航向角以及轮速仪输出的速度，通过序贯卡尔曼滤波对车辆运动误差状态变量的预测值进行更新，得到误差状态变量估计值后对车辆运动状态预测值进行反馈校正，得到车辆运动状态。本发明通过IMU、GNSS、激光雷达以及轮速仪等多传感器融合定位的方式，解决无人驾驶定位技术在复杂城市道路场景中精度低、定位不可靠的问题，并且计算负担小、工程实现性强、稳健性高。

Description

一种车辆运动状态确定的方法、装置及电子设备
技术领域
本发明涉及无人驾驶技术领域，具体涉及一种车辆运动状态确定的方法、装置及电子设备。
背景技术
随着自动驾驶技术的发展，自动驾驶成为未来出行的一种重要手段。现阶段的自动驾驶系统框架有感知模块、定位模块、规划决策模块、控制模块、高精地图模块及云计算模块等核心内容。感知模块通过传感器感知物理世界；传感器把物理世界按照一定的数据编码好，传送给感知模块，这样感知模块就可以通过相关的算法提取物理世界模型；定位模块利用感知模块的传感器数据，通过定位算法输出车辆相对于世界坐标系的位置、速度和姿态等运动状态；规划决策模块利用感知模块、定位模块、高精地图等数据进行整合和决策，最终将输出发送到控制模块，然后通过控制模块对车辆上的执行器进行控制；云计算模块也会实时地接收车辆上的所有数据，云计算模块确定车辆位置以后，会利用云端上的计算能力和数据库为车辆提供更好的驾驶能力。
在自动驾驶系统中，定位模块是核心功能模块。定位模块输出一方面用于路径规划和车辆控制，同时还辅助感知系统，得到更加准确的检测和跟踪结果。
在现有技术方案中，多数采用多传感器融合定位技术确定车辆运动状态如车辆位置、速度和姿态等。目前采用常规的惯性测量单元(Inertial Measurement Unit，IMU)、全球卫星导航系统(Global Navigation Satellite System，GNSS)融合的方式确定车辆运动状态，难以满足无人驾驶在城市峡谷、GNSS信号遮挡等典型城市道路场景下的高精度、高可靠性定位要求。
发明内容
本发明提供一种车辆运动状态确定方法、装置及电子设备，用以解决现有的定位技术难以满足无人驾驶在城市峡谷、GNSS信号遮挡等典型城市道路场景下的高精度、高可靠性定位要求的问题。
第一方面，本申请提供一种车辆运动状态确定方法，该方法包括：
根据惯性测量单元IMU输出的车辆角速度及车辆加速度，利用捷联惯性导航算法解算得到车辆运动状态预测值；
基于所述车辆运动状态预测值，通过序贯卡尔曼滤波及误差状态方程，得到车辆运动误差状态变量的预测值，其中，所述车辆运动误差状态变量为车辆运动状态预测值与车辆运动状态真实值的偏差；
获取利用全球卫星导航系统GNSS确定的位置和速度、利用激光雷达确定的位置和航向角以及轮速仪输出的速度，通过序贯卡尔曼滤波对车辆运动误差状态变量的预测值进行更新得到车辆运动误差状态变量估计值；
通过所述车辆运动误差状态变量估计值，对车辆运动状态预测值进行反馈校正，得到车辆运动状态。
可选地，所述车辆运动误差状态变量包括车辆位置误差变量、车辆速度误差变量以及车辆姿态误差变量。
可选地，获取利用全球卫星导航系统GNSS确定的位置和速度、利用激光雷达确定的位置和航向角以及轮速仪输出的速度之后，还包括：
根据IMU实时输出的车辆角速度及车辆加速度，分别对利用全球卫星导航系统GNSS确定的位置和速度、利用激光雷达确定的位置和航向角以及轮速仪输出的速度进行空间对齐操作；
根据IMU实时输出的车辆角速度及车辆加速度，分别对利用全球卫星导航系统GNSS确定的位置和速度、利用激光雷达确定的位置和航向角以及轮速仪输出的速度进行时延补偿操作。
可选地，通过序贯卡尔曼滤波对车辆运动误差状态变量的预测值进行更新得到车辆运动误差状态变量估计值，包括：
根据利用全球卫星导航系统GNSS确定的位置和速度、利用激光雷达确定的位置和航向角以及轮速仪输出的速度，确定序贯卡尔曼滤波的观测误差变量、与所述观测误差变量对应的噪声方差阵；
基于序贯卡尔曼滤波的观测误差变量、与观测误差变量对应的噪声方差阵、与车辆运动误差状态变量对应的协方差矩阵预测值，通过序贯卡尔曼滤波对车辆运动误差状态变量的预测值进行更新得到车辆运动误差状态变量估计值。
可选地，确定所述序贯卡尔曼滤波的观测误差变量，包括：
通过利用全球卫星导航系统GNSS确定的位置和速度、利用激光雷达确定的位置和航向角以及轮速仪输出的速度得到车辆运动状态真实值；
通过计算车辆运动状态预测值与所述车辆运动状态真实值的差值得到序贯卡尔曼滤波的观测误差变量。
可选地，确定与序贯卡尔曼滤波的观测误差变量对应的噪声方差阵，包括：
根据利用全球卫星导航系统GNSS确定位置和速度、利用激光雷达确定的位置和航向角以及轮速仪输出的速度，通过Sage-Husa自适应方差估计理论，得到观测误差变量对应的噪声方差阵。
可选地，确定与车辆运动误差状态变量对应的协方差矩阵预测值，包括：
基于所述车辆运动状态预测值，通过序贯卡尔曼滤波及误差状态方程，得到车辆运动误差状态变量对应的协方差矩阵预测值。
第二方面，本申请提供一种车辆运动状态确定装置，该装置包括：
预测值确定模块，用于根据惯性测量单元IMU输出的车辆角速度及车辆加速度，利用捷联惯性导航算法解算得到车辆运动状态预测值；
误差确定模块，用于基于所述车辆运动状态预测值，通过序贯卡尔曼滤波及误差状态方程，得到车辆运动误差状态变量的预测值，其中，所述车辆运动误差状态变量为车辆运动状态预测值与车辆运动状态真实值的偏差；
更新模块，用于获取利用全球卫星导航系统GNSS确定的位置和速度、利用激光雷达确定的位置和航向角以及轮速仪输出的速度，通过序贯卡尔曼滤波对车辆运动误差状态变量的预测值进行更新得到车辆运动误差状态变量估计值；
校正模块，用于通过所述车辆运动误差状态变量估计值，对车辆运动状态预测值进行反馈校正，得到车辆运动状态。
可选地，所述车辆运动误差状态变量包括车辆位置误差变量、车辆速度误差变量以及车辆姿态误差变量。
可选地，获取利用全球卫星导航系统GNSS确定的位置和速度、利用激光雷达确定的位置和航向角以及轮速仪输出的速度之后，还包括：
空间对齐模块，用于根据IMU实时输出的车辆角速度及车辆加速度，分别对利用全球卫星导航系统GNSS确定的位置和速度、利用激光雷达确定的位置和航向角以及轮速仪输出的速度进行空间对齐操作；
时延补偿模块，用于根据IMU实时输出的车辆角速度及车辆加速度，分别对利用全球卫星导航系统GNSS确定的位置和速度、利用激光雷达确定的位置和航向角以及轮速仪输出的速度进行时延补偿操作。
可选地，所述更新模块用于通过序贯卡尔曼滤波对车辆运动误差状态变量的预测值进行更新得到车辆运动误差状态变量估计值，具体用于：
根据利用全球卫星导航系统GNSS确定的位置和速度、利用激光雷达确定的位置和航向角以及轮速仪输出的速度，确定序贯卡尔曼滤波的观测误差变量、与所述观测误差变量对应的噪声方差阵；
基于序贯卡尔曼滤波的观测误差变量、与观测误差变量对应的噪声方差阵、与车辆运动误差状态变量对应的协方差矩阵预测值，通过序贯卡尔曼滤波对车辆运动误差状态变量的预测值进行更新得到车辆运动误差状态变量估计值。
可选地，所述更新模块用于确定所述序贯卡尔曼滤波的观测误差变量，具体用于：
通过利用全球卫星导航系统GNSS确定的位置和速度、利用激光雷达确定的位置和航向角以及轮速仪输出的速度得到车辆运动状态真实值；
通过计算车辆运动状态预测值与所述车辆运动状态真实值的差值得到序贯卡尔曼滤波的观测误差变量。
可选地，所述更新模块用于确定与序贯卡尔曼滤波的观测误差变量对应的噪声方差阵，具体用于：
根据利用全球卫星导航系统GNSS确定位置和速度、利用激光雷达确定的位置和航向角以及轮速仪输出的速度，通过Sage-Husa自适应方差估计理论，得到观测误差变量对应的噪声方差阵。
可选地，所述更新模块用于确定与车辆运动误差状态变量对应的协方差矩阵预测值，具体用于：
基于所述车辆运动状态预测值，通过序贯卡尔曼滤波及误差状态方程，得到车辆运动误差状态变量对应的协方差矩阵预测值。
第三方面，本申请提供一种车辆运动状态确定的电子设备，包括：存储单元、处理单元；
其中，所述存储单元用于存储程序；
所述处理单元用于执行所述存储单元中的程序，包括如下步骤：
根据惯性测量单元IMU输出的车辆角速度及车辆加速度，利用捷联惯性导航算法解算得到车辆运动状态预测值；
基于所述车辆运动状态预测值，通过序贯卡尔曼滤波及误差状态方程，得到车辆运动误差状态变量的预测值，其中，所述车辆运动误差状态变量为车辆运动状态预测值与车辆运动状态真实值的偏差；
获取利用全球卫星导航系统GNSS确定的位置和速度、利用激光雷达确定的位置和航向角以及轮速仪输出的速度，通过序贯卡尔曼滤波对车辆运动误差状态变量的预测值进行更新得到车辆运动误差状态变量估计值；
通过所述车辆运动误差状态变量估计值，对车辆运动状态预测值进行反馈校正，得到车辆运动状态。
第四方面，本申请还提供一种计算机存储介质，其上存储有计算机程序，该程序被处理单元执行时实现第一方面所述方法的步骤。
另外，第二方面至第四方面中任一种实现方式所带来的技术效果可参见第一方面中不同实现方式所带来的技术效果，此处不再赘述。
利用本发明提供的一种车辆运动状态确定的方法、装置及电子设备，具有以下有益效果：
本发明提供的一种车辆运动状态确定的方法、装置及电子设备，可以通过IMU、GNSS、激光雷达以及轮速仪多种传感器融合进行定位的方式，解决现有无人驾驶定位技术在复杂城市道路场景下精度低、可靠性差等问题。
附图说明
为了更清楚地说明本发明实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简要介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域的普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1为本发明实施例提供的一种车辆运动状态确定的系统示意图；
图2为本发明实施例提供的一种车辆运动状态确定方法示意图；
图3为本发明实施例提供的一种车辆运动状态确定方法示意图；
图4为本发明实施例提供的一种车辆运动状态确定的装置示意图；
图5为本发明实施例提供的一种车辆运动状态确定的电子设备示意图。
具体实施方式
为了使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明作进一步地详细描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。
本发明实施例中术语“和/或”，描述关联对象的关联关系，表示可以存在三种关系，例如，A和/或B，可以表示：单独存在A，同时存在A和B，单独存在B这三种情况。字符“/”一般表示前后关联对象是一种“或”的关系。
本发明实施例描述的应用场景是为了更加清楚的说明本发明实施例的技术方案，并不构成对于本发明实施例提供的技术方案的限定，本领域普通技术人员可知，随着新应用场景的出现，本发明实施例提供的技术方案对于类似的技术问题，同样适用。其中，在本发明的描述中，除非另有说明，“多个”的含义是两个或两个以上。
在多传感器融合定位技术中，需要确定车辆运动状态如车辆位置、速度和姿态等。基于目前采用常规的惯性测量单元(Inertial Measurement Unit，IMU)、全球卫星导航系统(Global Navigation Satellite System，GNSS)融合的方式确定车辆运动状态，难以满足无人驾驶在城市峡谷、GNSS信号遮挡等典型城市道路场景下的高精度、高可靠性定位要求的问题，其他常见融合定位方案具有计算负担大、工程实现性不强等缺点。基于上述问题，本申请提出一种新的车辆运动状态确定的方法。
实施例一
如图1所示为本申请提供的一种车辆运动状态确定的系统，包括：惯性测量单元(IMU)101、全球卫星导航系统(GNSS)102、激光雷达(Lidar)103、轮速仪(Wheel SpeedSensor，WSS)104、融合定位处理单元105以及天线106，其中：
惯性测量单元(IMU)101以高频输出车辆的角速度与加速度，输出频率典型值为100Hz；全球卫星导航系统(GNSS)102输出GNSS对应天线106相位中心处的位置和速度，输出频率典型值为5Hz；激光雷达(Lidar)103通过激光波束扫描车辆周围环境而获取到大量点云，通过与事先录制的高精点云地图匹配处理，可以输出激光头安装位置处的位置和航向角，输出频率典型值为10Hz；轮速仪(Wheel Speed Sensor，WSS)104输出轮胎与地面接触点处的前向运动速度，输出频率典型值为10Hz。
上述传感器的处理单元实时输出数据，数据经有线方式(如串口、网口、控制器局域网络(Controller Area Network，CAN)总线等)传输至融合定位处理单元105(如嵌入式平台等)，融合定位处理单元105通过本申请提出的车辆运动状态确定方法，得到车辆运动状态。
实施例二
本申请提出的一种车辆运动状态确定方法，如图2所示，包括：
步骤S201，根据惯性测量单元IMU输出的车辆角速度及车辆加速度，利用捷联惯性导航算法解算得到车辆运动状态预测值；
利用捷联惯性导航算法解算得到的车辆运动状态预测值，是利用在导航坐标系下(通常选为东北天地理坐标系，记为n系)捷联惯导力学编排方程后进行离散积分运算得到的。
其中，车辆运动状态预测值包括车辆位置预测值、车辆速度预测值以及车辆姿态预测值。
步骤S202，基于所述车辆运动状态预测值，通过序贯卡尔曼滤波及误差状态方程，得到车辆运动误差状态变量的预测值，其中，所述车辆运动误差状态变量为车辆运动状态预测值与车辆运动状态真实值的偏差；
车辆运动误差状态变量包括车辆位置误差变量、车辆速度误差变量以及车辆姿态误差变量，车辆位置误差变量为车辆位置预测值与对应真实值的偏差，车辆速度误差变量为车辆速度预测值与对应真实值的偏差，车辆姿态误差变量为车辆姿态预测值与对应真实值的偏差。
另外，为保证运动状态估计精度，所述车辆运动误差状态变量还包括IMU的加速度计零偏变量和IMU的陀螺仪零偏变量，通过IMU的加速度计零偏变量对车辆中IMU的加速度计进行反馈校正，通过IMU的陀螺仪零偏变量对车辆中IMU的陀螺仪进行反馈校正。
可选地，若此时除IMU外没有其他传感器的数据时，将车辆运动误差状态变量的预测值作为车辆运动误差状态变量的估计值。
步骤S203，获取利用全球卫星导航系统GNSS确定的位置和速度、利用激光雷达确定的位置和航向角以及轮速仪输出的速度，通过序贯卡尔曼滤波对车辆运动误差状态变量的预测值进行更新得到车辆运动误差状态变量估计值；
根据IMU实时输出的车辆的角速度及车辆的加速度，分别对利用全球卫星导航系统GNSS确定的位置和速度、利用激光雷达确定的位置和航向角以及轮速仪输出的速度进行空间对齐操作；
IMU、GNSS、Lidar和WSS等传感器安装的位置和姿态不同，导致其输出的数据通常不在同一参考基准下的，如上述基于IMU的捷联惯导解算是以IMU的几何中心为位置和速度解算的参考基准，GNSS以接收机天线的相位中心作为定位和定速的参考基准，Lidar以其激光发射装置的几何中心为位姿估计的参考基准，WSS以车轮与地面接触点为速度输出的参考基准，为能够对车辆运动误差状态变量的预测值进行更新处理，须将GNSS、Lidar和WSS等传感器输出转换至与捷联惯导一致的、统一的参考基准下，该步骤称为空间对齐。
根据IMU实时输出的车辆角速度及车辆加速度，分别对利用全球卫星导航系统GNSS确定的位置和速度、利用激光雷达确定的位置和航向角以及轮速仪输出的速度进行时延补偿操作；
由于需要经历信号采集、定位解算和通信传输等环节，GNSS、Lidar和WSS等传感器的数据到达融合定位处理单元时，相比所述传感器的数据采集时刻往往存在至几十毫秒到上百毫秒的延时。为能够对车辆运动误差状态变量的预测值进行更新处理，还需要将上述传感器的输出与IMU输出统一到同一时刻，即根据IMU的实时输出的车辆角速度及车辆加速度，将GNSS、Lidar和WSS等传感器的输出外推到IMU实时输出时刻。
根据利用全球卫星导航系统GNSS确定的位置和速度、利用激光雷达确定的位置和航向角以及轮速仪输出的速度，确定序贯卡尔曼滤波的观测误差变量、与观测误差变量对应的噪声方差阵；
基于序贯卡尔曼滤波的观测误差变量、与观测误差变量对应的噪声方差阵、与车辆运动误差状态变量对应的协方差矩阵预测值，通过序贯卡尔曼滤波对车辆运动误差状态变量的预测值进行更新得到车辆运动误差状态变量估计值，其中，上述数值通过以下方式得到：
1)序贯卡尔曼滤波的观测误差变量
通过利用全球卫星导航系统GNSS确定的位置和速度、利用激光雷达确定的位置和航向角以及轮速仪输出的速度得到车辆运动状态真实值；
通过计算车辆运动状态预测值与所述车辆运动状态真实值的差值得到序贯卡尔曼滤波的观测误差变量。
2)与序贯卡尔曼滤波的观测误差变量对应的噪声方差阵
根据利用全球卫星导航系统GNSS确定位置和速度、利用激光雷达确定的位置和航向角以及轮速仪输出的速度，通过Sage-Husa自适应方差估计理论，得到观测误差变量对应的噪声方差阵。
3)与车辆运动误差状态变量对应的协方差矩阵预测值
基于所述车辆运动状态预测值，通过序贯卡尔曼滤波及误差状态方程，得到车辆运动误差状态变量对应的协方差矩阵预测值。
步骤S204，通过所述车辆运动误差状态变量估计值，对车辆运动状态预测值进行反馈校正，得到车辆运动状态；
此时得到的车辆运动状态为车辆运动状态的精确估计值。
当重新有IMU的数据到达时，重新进行上述车辆运动状态确定的方法，直到接受指令不对车辆运动状态进行确定。
本申请提出的车辆运动状态确定方法，在融合IMU、GNSS、激光雷达以及轮速仪的同时，采用“序贯卡尔曼滤波、自适应方差估计、空间对齐及时延补偿”等方法，具有计算负担小、工程实现性强和系统稳健性高等优点。
实施例三
下面结合具体的实施例，对本申请提出的一种车辆运动状态确定方法进行详细描述，如图3所示，包括：
步骤S301，根据惯性测量单元IMU输出的车辆角速度及车辆加速度，利用捷联惯性导航解算得到车辆运动状态预测值；
接收当前的采样时刻惯性测量单元IMU输出的车辆角速度及车辆加速度，进行捷联惯导解算，得到车辆运动状态预测值，车辆运动状态预测值分别为车辆位置p、车辆速度vn以及车辆姿态
Figure BDA0002304838660000111
等量的预测值，具体的，利用在导航坐标系下(通常选为东北天地理坐标系，记为n系)捷联惯导如下的力学编排方程进行离散积分运算得到：
p＝Mpvvn
Figure BDA0002304838660000112
Figure BDA0002304838660000113
其中：
Figure BDA0002304838660000114
p＝(L,λ,h)T为车辆的纬度、经度和高度，vn为车辆在n系下的速度，
Figure BDA0002304838660000115
为车体坐标系(通常选为右-前-上坐标系，记为b系)相对于n系的姿态四元数；fb为IMU输出的加速度，
Figure BDA0002304838660000116
为b系相对于n系的角速度，并且有
Figure BDA0002304838660000117
Figure BDA0002304838660000118
为n系相对于惯性系(i系)的角速度，
Figure BDA0002304838660000119
为b系相对于i系的角速度即为IMU输出的角速度，
Figure BDA00023048386600001110
为地心地固坐标系(e系)相对于i系的角速度，
Figure BDA00023048386600001111
为n系相对于e系的角速度，gn为重力加速度在n系下的表示；
Figure BDA00023048386600001112
表示四元数相乘；
Figure BDA00023048386600001113
表示从b系到n系的坐标变换矩阵，RM为子午圈主曲率半径，RN为卯酉圈主曲率半径。
步骤S302，基于所述车辆运动状态预测值，通过序贯卡尔曼滤波及误差状态方程，得到车辆运动误差状态变量的预测值及对应的协方差矩阵预测值；
车辆运动误差状态变量包括车辆位置误差变量δp、车辆速度误差变量δvn、车辆姿态误差变量φ、IMU器件误差如陀螺零偏εb和加速度计零偏▽b，得到车辆运动误差状态变量为：
X＝[(δp)T (δvn)T φT (εb)T (▽b)T]T
对应的误差状态方程为：
Xk＝Φk/k-1Xk-1+Γk-1Wk-1
其中，Φk/k-1为误差状态转移矩阵，Γk-1为噪声分配矩阵，两者具体形式可由捷联惯导的力学编排方程进行小偏差线性化得到；Wk-1是均值为零、方差阵为Qk的高斯白噪声向量，Qk的值由IMU的性能指标决定；下标k或k-1表示对应的IMU采样时刻。
基于上述误差状态方程，进行序贯卡尔曼滤波时间更新，确定车辆运动误差状态变量的预测值及对应的协方差矩阵预测值。具体公式包括：
1)车辆运动误差状态变量的预测公式为：
Figure BDA0002304838660000121
2)车辆运动误差状态变量对应的协方差矩阵的预测公式为：
Figure BDA0002304838660000122
此时若没有其他传感器的数据，则将所述车辆运动误差状态变量预测值作为车辆运动误差状态变量估计值，车辆运动误差状态变量对应的协方差矩阵的预测值作为车辆运动误差状态变量对应的协方差矩阵的估计值。
步骤S303，根据IMU实时输出的车辆角速度及车辆加速度，分别对利用全球卫星导航系统GNSS确定的位置和速度、利用激光雷达确定的位置和航向角以及轮速仪输出的速度进行空间对齐操作及时延补偿操作；
1)对于GNSS的确定的位置和速度输出
Figure BDA0002304838660000123
执行如下变换：
Figure BDA0002304838660000131
Figure BDA0002304838660000132
其中，
Figure BDA0002304838660000133
为空间对齐和时延补偿后的GNSS的位置和速度，δlGNSS为天线相位中心相对于IMU几何中心的矢量在b系下的表示，ΔtGNSS为GNSS时间延时量，由当前IMU的GNSS时间标签与接收到的GNSS数据输出中的GNSS时间标签相减得到，
Figure BDA0002304838660000134
为b系相对于e系的角速度在b系下的表示，其值可近似认为是IMU输出的角速度。
2)对于激光雷达定位输出的位置
Figure BDA0002304838660000135
和航向角
Figure BDA0002304838660000136
执行如下变换：
Figure BDA0002304838660000137
Figure BDA0002304838660000138
其中，
Figure BDA0002304838660000139
为空间对齐和时延补偿后的激光雷达的位置和航向角输出，
Figure BDA00023048386600001310
为激光雷达坐标系相对于IMU坐标系的安装偏角，δlLidar为激光发射装置的几何中心相对于IMU几何中心的矢量在b系下的表示，ΔtLidar为激光雷达的时间延时量，由当前IMU的GNSS时间标签与接收到的激光雷达数据输出中GNSS时间标签相减得到，
Figure BDA00023048386600001311
可近似用IMU输出角速度的Z轴方向分量代替。
3)对于轮速仪的速度
Figure BDA00023048386600001312
执行如下变换
Figure BDA00023048386600001313
其中，
Figure BDA00023048386600001314
为空间对齐和时延补偿后轮速仪三维速度的输出，δlWSS是安装轮速仪的车轮与地面接触点相对于IMU几何中心的矢量在b系下的表示，ΔtWSS为WSS时间延时量，由当前IMU的GNSS时间标签与接收到的WSS数据输出中的GNSS时间标签相减得到。若WSS与GNSS的时间同步不易实现，这里可令ΔtWSS＝0。
步骤S304，通过利用全球卫星导航系统GNSS确定的位置和速度、利用激光雷达确定的位置和航向角以及轮速仪输出的三维速度得到车辆运动状态真实值；
其中，车辆运动状态真实值可能由上述3种传感器的数据输出得到，也可能由上述3种传感器中任意一种或者多种传感器的数据输出得到，可选地，车辆运动状态真实值的可能形式可以为
Figure BDA0002304838660000141
或
Figure BDA0002304838660000142
等。
步骤S305，通过计算车辆运动状态预测值与所述车辆运动状态真实值的差值得到序贯卡尔曼滤波的观测误差变量；
其中，序贯卡尔曼滤波的观测误差变量为：
Zk＝HkXk+Vk
Zk为观测误差变量；Hk为量测矩阵，由观测误差变量的具体形式确定；量测噪声Vk是均值为零、方差阵为Rk的高斯白噪声向量。
步骤S306，根据利用全球卫星导航系统GNSS确定位置和速度、利用激光雷达确定的位置和航向角以及轮速仪输出的三维速度，通过Sage-Husa自适应方差估计理论，得到车辆状态系统观测误差变量对应的噪声方差阵。
为进行序贯卡尔曼滤波更新过程，通常需已知Rk，但在实际工程中却很难精确已知Rk。这里采用Sage-Husa自适应方差估计理论，在线估计Zk对应的噪声方差阵，具体算法为：
Figure BDA0002304838660000143
Figure BDA0002304838660000144
其中，
Figure BDA0002304838660000145
为当前k时刻噪声方差阵的估计值；
Figure BDA0002304838660000146
β0＝1，渐消因子b为常数，在区间(0，1)上取值。
步骤S307，基于序贯卡尔曼滤波的观测误差变量、与观测误差变量对应的噪声方差阵、与车辆运动误差状态变量对应的协方差矩阵预测值，通过序贯卡尔曼滤波对车辆运动误差状态变量的预测值进行更新得到车辆运动误差状态变量估计值；
将Zk的各个分量视为测量时间间隔为零的一系列标量量测值，对各标量量测值依次进行量测更新处理。具体操作如下：
对于数据输出向量Zk为M维的情况，假设不同标量测量值的噪声互不相关，即Rk可写成如下对角阵的形式：
Figure BDA0002304838660000151
对于Zk＝[z1,k,z2,k,…,zM,k]T中的M个标量测量值zm,k(m＝1,2,…,M)，依次用以下公式进行车辆运动误差状态变量的预测值和对应的协方差矩阵预测值的更新：
Figure BDA0002304838660000152
Figure BDA0002304838660000153
Pm,k＝(I15-km,khm)Pm-1,k
其中，
Figure BDA0002304838660000154
P0,k＝Pk/k-1，hm为Hk的行向量，即
Figure BDA0002304838660000155
I15为15×15的单位矩阵。通过上述公式一次对z1,k，z2,k,z3,k,…进行处理，最后将
Figure BDA0002304838660000156
赋值给车辆运动误差状态变量的估计值，将PM,k赋值给k时刻的车辆运动误差状态变量对应的协方差矩阵的估计值，即令
Figure BDA0002304838660000157
Pk＝PM,k
步骤S308，通过所述车辆运动误差状态变量估计值，对车辆运动状态预测值以及IMU器件误差进行反馈校正，得到车辆运动状态的精确估计值。
其中，反馈校正过程中的公式如下：
Figure BDA0002304838660000158
Figure BDA0002304838660000159
Figure BDA0002304838660000161
Figure BDA0002304838660000162
Figure BDA0002304838660000163
其中，在k＝0时刻，公式中IMU器件误差εb和▽b的值可以设置为零或者由IMU器件指标等先验信息给出，
Figure BDA0002304838660000164
表示向量
Figure BDA0002304838660000165
的第i到j个分量，误差四元数
Figure BDA0002304838660000166
由
Figure BDA0002304838660000167
中的姿态失准角估计值
Figure BDA0002304838660000168
得到：
Figure BDA0002304838660000169
以上对本发明中一种车辆运动状态确定方法进行说明，以下对执行上述车辆运动状态确定方法的装置进行说明。
请参阅图4本发明实施例提供的一种车辆运动状态确定的装置，包括：
预测值确定模块401，用于根据惯性测量单元IMU输出的车辆角速度及车辆加速度，利用捷联惯性导航算法解算得到车辆运动状态预测值；
误差确定模块402，用于基于所述车辆运动状态预测值，通过序贯卡尔曼滤波及误差状态方程，得到车辆运动误差状态变量的预测值，其中，所述车辆运动误差状态变量为车辆运动状态预测值与车辆运动状态真实值的偏差；
更新模块403，用于获取利用全球卫星导航系统GNSS确定的位置和速度、利用激光雷达确定的位置和航向角以及轮速仪输出的速度，通过序贯卡尔曼滤波对车辆运动误差状态变量的预测值进行更新得到车辆运动误差状态变量估计值；
校正模块404，用于通过所述车辆运动误差状态变量估计值，对车辆运动状态预测值进行反馈校正，得到车辆运动状态。
可选地，所述车辆运动误差状态变量包括车辆位置误差变量、车辆速度误差变量以及车辆姿态误差变量。
可选地，获取利用全球卫星导航系统GNSS确定的位置和速度、利用激光雷达确定的位置和航向角以及轮速仪输出的速度之后，还包括：
空间对齐模块405，用于根据IMU实时输出的车辆角速度及车辆加速度，分别对利用全球卫星导航系统GNSS确定的位置和速度、利用激光雷达确定的位置和航向角以及轮速仪输出的速度进行空间对齐操作；
时延补偿模块406，用于根据IMU实时输出的车辆角速度及车辆加速度，分别对利用全球卫星导航系统GNSS确定的位置和速度、利用激光雷达确定的位置和航向角以及轮速仪输出的速度进行时延补偿操作。
可选地，所述更新模块用于通过序贯卡尔曼滤波对车辆运动误差状态变量的预测值进行更新得到车辆运动误差状态变量估计值，具体用于：
根据利用全球卫星导航系统GNSS确定的位置和速度、利用激光雷达确定的位置和航向角以及轮速仪输出的速度，确定序贯卡尔曼滤波的观测误差变量、与所述观测误差变量对应的噪声方差阵；
基于序贯卡尔曼滤波的观测误差变量、与观测误差变量对应的噪声方差阵、与车辆运动误差状态变量对应的协方差矩阵预测值，通过序贯卡尔曼滤波对车辆运动误差状态变量的预测值进行更新得到车辆运动误差状态变量估计值。
可选地，所述更新模块用于确定所述序贯卡尔曼滤波的观测误差变量，具体用于：
通过利用全球卫星导航系统GNSS确定的位置和速度、利用激光雷达确定的位置和航向角以及轮速仪输出的速度得到车辆运动状态真实值；
通过计算车辆运动状态预测值与所述车辆运动状态真实值的差值得到序贯卡尔曼滤波的观测误差变量。
可选地，所述更新模块用于确定与序贯卡尔曼滤波的观测误差变量对应的噪声方差阵，具体用于：
根据利用全球卫星导航系统GNSS确定位置和速度、利用激光雷达确定的位置和航向角以及轮速仪输出的速度，通过Sage-Husa自适应方差估计理论，得到观测误差变量对应的噪声方差阵。
可选地，所述更新模块用于确定与车辆运动误差状态变量对应的协方差矩阵预测值，具体用于：
基于所述车辆运动状态预测值，通过序贯卡尔曼滤波及误差状态方程，得到车辆运动误差状态变量对应的协方差矩阵预测值。
上面从模块化功能实体的角度对本申请实施例中的一种车辆运动状态确定的装置进行了描述，下面从硬件处理的角度对本申请实施例中的车辆运动状态确定的电子设备进行描述。
请参阅图5，本申请实施例中一种车辆运动状态确定的电子设备，包括：
至少一个处理单元501和至少一个存储单元502，以及总线系统509；
其中，所述存储单元存储有程序代码，当所述程序代码被所述处理单元执行时，使得所述处理单元执行下列过程：
根据惯性测量单元IMU输出的车辆角速度及车辆加速度，利用捷联惯性导航算法解算得到车辆运动状态预测值；
基于所述车辆运动状态预测值，通过序贯卡尔曼滤波及误差状态方程，得到车辆运动误差状态变量的预测值，其中，所述车辆运动误差状态变量为车辆运动状态预测值与车辆运动状态真实值的偏差；
获取利用全球卫星导航系统GNSS确定的位置和速度、利用激光雷达确定的位置和航向角以及轮速仪输出的速度，通过序贯卡尔曼滤波对车辆运动误差状态变量的预测值进行更新得到车辆运动误差状态变量估计值；
通过所述车辆运动误差状态变量估计值，对车辆运动状态预测值进行反馈校正，得到车辆运动状态。
图5是本公开实施例提供的一种车辆运动状态确定的电子设备示意图，该设备500可因配置或性能不同而产生比较大的差异，可以包括一个或一个以上处理单元(英文全称：central processing units，英文简称：CPU)501(例如，一个或一个以上处理单元)和存储单元502，一个或一个以上存储应用程序504或数据505的存储介质503(例如一个或一个以上海量存储设备)。其中，存储单元502和存储介质503可以是短暂存储或持久存储。存储在存储介质503的程序可以包括一个或一个以上模块(图示没标出)，每个模块可以包括对信息处理装置中的一系列指令操作。更进一步地，处理单元501可以设置为与存储介质503通信，在设备500上执行存储介质503中的一系列指令操作。
设备500还可以包括一个或一个以上有线或无线网络接口507，一个或一个以上输入输出接口508，和/或，一个或一个以上操作系统506，例如Windows Server，Mac OS X，Unix，Linux，FreeBSD等。
可选地，所述车辆运动误差状态变量包括车辆位置误差变量、车辆速度误差变量以及车辆姿态误差变量。
可选地，所述处理单元用于获取利用全球卫星导航系统GNSS确定的位置和速度、利用激光雷达确定的位置和航向角以及轮速仪输出的速度之后，还包括：
根据IMU实时输出的车辆角速度及车辆加速度，分别对利用全球卫星导航系统GNSS确定的位置和速度、利用激光雷达确定的位置和航向角以及轮速仪输出的速度进行空间对齐操作；
根据IMU实时输出的车辆角速度及车辆加速度，分别对利用全球卫星导航系统GNSS确定的位置和速度、利用激光雷达确定的位置和航向角以及轮速仪输出的速度进行时延补偿操作。
可选地，所述处理单元用于通过序贯卡尔曼滤波对车辆运动误差状态变量的预测值进行更新得到车辆运动误差状态变量估计值，包括：
根据利用全球卫星导航系统GNSS确定的位置和速度、利用激光雷达确定的位置和航向角以及轮速仪输出的速度，确定序贯卡尔曼滤波的观测误差变量、与所述观测误差变量对应的噪声方差阵；
基于序贯卡尔曼滤波的观测误差变量、与观测误差变量对应的噪声方差阵、与车辆运动误差状态变量对应的协方差矩阵预测值，通过序贯卡尔曼滤波对车辆运动误差状态变量的预测值进行更新得到车辆运动误差状态变量估计值。
可选地，所述处理单元用于确定所述序贯卡尔曼滤波的观测误差变量，包括：
通过利用全球卫星导航系统GNSS确定的位置和速度、利用激光雷达确定的位置和航向角以及轮速仪输出的速度得到车辆运动状态真实值；
通过计算车辆运动状态预测值与所述车辆运动状态真实值的差值得到序贯卡尔曼滤波的观测误差变量。
可选地，所述处理单元用于确定与序贯卡尔曼滤波的观测误差变量对应的噪声方差阵，包括：
根据利用全球卫星导航系统GNSS确定位置和速度、利用激光雷达确定的位置和航向角以及轮速仪输出的速度，通过Sage-Husa自适应方差估计理论，得到观测误差变量对应的噪声方差阵。
可选地，所述处理单元用于确定与车辆运动误差状态变量对应的协方差矩阵预测值，包括：
基于所述车辆运动状态预测值，通过序贯卡尔曼滤波及误差状态方程，得到车辆运动误差状态变量对应的协方差矩阵预测值。
本发明实施例还提供一种计算机可读存储介质，包括指令，当其在计算机上运行时，使得计算机执行上述实施例提供的车辆运动状态确定的方法。
本公开实施例还提供一种计算机程序产品，包括计算机程序，所述计算机程序包括程序指令，当所述程序指令被电子设备执行时，使所述电子设备执行上述实施例提供的车辆运动状态确定方法。
所属领域的技术人员可以清楚地了解到，为描述的方便和简洁，上述描述的装置和模块的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。
在本申请所提供的几个实施例中，应该理解到，所揭露的系统，装置和方法，可以通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如，所述模块的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个模块或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，装置或模块的间接耦合或通信连接，可以是电性，机械或其它的形式。
所述作为分离部件说明的模块可以是或者也可以不是物理上分开的，作为模块显示的部件可以是或者也可以不是物理模块，即可以位于一个地方，或者也可以分布到多个网络模块上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。
另外，在本申请各个实施例中的各功能模块可以集成在一个处理模块中，也可以是各个模块单独物理存在，也可以两个或两个以上模块集成在一个模块中。上述集成的模块既可以采用硬件的形式实现，也可以采用软件功能模块的形式实现。所述集成的模块如果以软件功能模块的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。
在上述实施例中，可以全部或部分地通过软件、硬件、固件或者其任意组合来实现。当使用软件实现时，可以全部或部分地以计算机程序产品的形式实现。
所述计算机程序产品包括一个或多个计算机指令。在计算机上加载和执行所述计算机程序指令时，全部或部分地产生按照本申请实施例所述的流程或功能。所述计算机可以是通用计算机、专用计算机、计算机网络、或者其他可编程装置。所述计算机指令可以存储在计算机可读存储介质中，或者从一个计算机可读存储介质向另一计算机可读存储介质传输，例如，所述计算机指令可以从一个网站站点、计算机、服务器或数据中心通过有线(例如同轴电缆、光纤、数字用户线(DSL))或无线(例如红外、无线、微波等)方式向另一个网站站点、计算机、服务器或数据中心进行传输。所述计算机可读存储介质可以是计算机能够存储的任何可用介质或者是包含一个或多个可用介质集成的服务器、数据中心等数据存储设备。所述可用介质可以是磁性介质，(例如，软盘、硬盘、磁带)、光介质(例如，DVD)、或者半导体介质(例如固态硬盘(solid state disk，SSD))等。
以上对本申请所提供的技术方案进行了详细介绍，本申请中应用了具体个例对本申请的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本申请的方法及其核心思想；同时，对于本领域的一般技术人员，依据本申请的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本申请的限制。

Claims (9)

1.一种车辆运动状态确定方法，其特征在于，该方法包括：
根据惯性测量单元IMU输出的车辆角速度及车辆加速度，利用捷联惯性导航算法解算得到车辆运动状态预测值；
基于所述车辆运动状态预测值，通过序贯卡尔曼滤波及误差状态方程，得到车辆运动误差状态变量的预测值，其中，所述车辆运动误差状态变量为车辆运动状态预测值与车辆运动状态真实值的偏差；
获取利用全球卫星导航系统GNSS确定的位置和速度、利用激光雷达确定的位置和航向角以及轮速仪输出的速度，根据利用全球卫星导航系统GNSS确定的位置和速度、利用激光雷达确定的位置和航向角以及轮速仪输出的速度，确定序贯卡尔曼滤波的观测误差变量、与所述观测误差变量对应的噪声方差阵；
基于序贯卡尔曼滤波的观测误差变量、与观测误差变量对应的噪声方差阵、与车辆运动误差状态变量对应的协方差矩阵预测值，通过序贯卡尔曼滤波对车辆运动误差状态变量的预测值进行更新得到车辆运动误差状态变量估计值；
通过所述车辆运动误差状态变量估计值，对车辆运动状态预测值进行反馈校正，得到车辆运动状态。
2.根据权利要求1所述的方法，其特征在于，所述车辆运动误差状态变量包括车辆位置误差变量、车辆速度误差变量以及车辆姿态误差变量。
3.根据权利要求1所述的方法，其特征在于，获取利用全球卫星导航系统GNSS确定的位置和速度、利用激光雷达确定的位置和航向角以及轮速仪输出的速度之后，还包括：
根据IMU实时输出的车辆角速度及车辆加速度，分别对利用全球卫星导航系统GNSS确定的位置和速度、利用激光雷达确定的位置和航向角以及轮速仪输出的速度进行空间对齐操作；
根据IMU实时输出的车辆角速度及车辆加速度，分别对利用全球卫星导航系统GNSS确定的位置和速度、利用激光雷达确定的位置和航向角以及轮速仪输出的速度进行时延补偿操作。
4.根据权利要求1所述的方法，其特征在于，确定所述序贯卡尔曼滤波的观测误差变量，包括：
通过利用全球卫星导航系统GNSS确定的位置和速度、利用激光雷达确定的位置和航向角以及轮速仪输出的速度得到车辆运动状态真实值；
通过计算车辆运动状态预测值与所述车辆运动状态真实值的差值得到序贯卡尔曼滤波的观测误差变量。
5.根据权利要求1所述的方法，其特征在于，确定与序贯卡尔曼滤波的观测误差变量对应的噪声方差阵，包括：
根据利用全球卫星导航系统GNSS确定位置和速度、利用激光雷达确定的位置和航向角以及轮速仪输出的速度，通过Sage-Husa自适应方差估计理论，得到观测误差变量对应的噪声方差阵。
6.根据权利要求1所述的方法，其特征在于，确定与车辆运动误差状态变量对应的协方差矩阵预测值，包括：
基于所述车辆运动状态预测值，通过序贯卡尔曼滤波及误差状态方程，得到车辆运动误差状态变量对应的协方差矩阵预测值。
7.一种车辆运动状态确定装置，其特征在于，该装置包括：
预测值确定模块，用于根据惯性测量单元IMU输出的车辆角速度及车辆加速度，利用捷联惯性导航算法解算得到车辆运动状态预测值；
误差确定模块，用于基于所述车辆运动状态预测值，通过序贯卡尔曼滤波及误差状态方程，得到车辆运动误差状态变量的预测值，其中，所述车辆运动误差状态变量为车辆运动状态预测值与车辆运动状态真实值的偏差；
确定模块，用于获取利用全球卫星导航系统GNSS确定的位置和速度、利用激光雷达确定的位置和航向角以及轮速仪输出的速度，根据利用全球卫星导航系统GNSS确定的位置和速度、利用激光雷达确定的位置和航向角以及轮速仪输出的速度，确定序贯卡尔曼滤波的观测误差变量、与所述观测误差变量对应的噪声方差阵；
更新模块，用于基于序贯卡尔曼滤波的观测误差变量、与观测误差变量对应的噪声方差阵、与车辆运动误差状态变量对应的协方差矩阵预测值，通过序贯卡尔曼滤波对车辆运动误差状态变量的预测值进行更新得到车辆运动误差状态变量估计值；
校正模块，用于通过所述车辆运动误差状态变量估计值，对车辆运动状态预测值进行反馈校正，得到车辆运动状态。
8.一种车辆运动状态确定的电子设备，其特征在于，包括：存储单元、处理单元；
其中，所述存储单元用于存储程序；
所述处理单元用于执行所述存储单元中的程序，包括如下步骤：
根据惯性测量单元IMU输出的车辆角速度及车辆加速度，利用捷联惯性导航算法解算得到车辆运动状态预测值；
基于所述车辆运动状态预测值，通过序贯卡尔曼滤波及误差状态方程，得到车辆运动误差状态变量的预测值，其中，所述车辆运动误差状态变量为车辆运动状态预测值与车辆运动状态真实值的偏差；
获取利用全球卫星导航系统GNSS确定的位置和速度、利用激光雷达确定的位置和航向角以及轮速仪输出的速度，根据利用全球卫星导航系统GNSS确定的位置和速度、利用激光雷达确定的位置和航向角以及轮速仪输出的速度，确定序贯卡尔曼滤波的观测误差变量、与所述观测误差变量对应的噪声方差阵；
基于序贯卡尔曼滤波的观测误差变量、与观测误差变量对应的噪声方差阵、与车辆运动误差状态变量对应的协方差矩阵预测值，通过序贯卡尔曼滤波对车辆运动误差状态变量的预测值进行更新得到车辆运动误差状态变量估计值；
通过所述车辆运动误差状态变量估计值，对车辆运动状态预测值进行反馈校正，得到车辆运动状态。
9.一种计算机可读存储介质，其特征在于，包括计算机程序指令，当其在计算机上运行时，使得计算机执行如权利要求1至6中任一项所述的方法。
CN201911235777.5A 2019-12-05 2019-12-05 一种车辆运动状态确定的方法、装置及电子设备 Active CN111102978B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911235777.5A CN111102978B (zh) 2019-12-05 2019-12-05 一种车辆运动状态确定的方法、装置及电子设备

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911235777.5A CN111102978B (zh) 2019-12-05 2019-12-05 一种车辆运动状态确定的方法、装置及电子设备

Publications (2)

Publication Number Publication Date
CN111102978A CN111102978A (zh) 2020-05-05
CN111102978B true CN111102978B (zh) 2022-03-29

Family

ID=70421662

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911235777.5A Active CN111102978B (zh) 2019-12-05 2019-12-05 一种车辆运动状态确定的方法、装置及电子设备

Country Status (1)

Country Link
CN (1) CN111102978B (zh)

Families Citing this family (16)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111595592B (zh) * 2020-05-21 2021-10-12 东南大学 一种自适应巡航控制系统性能测评方法
CN111693723B (zh) * 2020-05-29 2022-04-22 阿波罗智联(北京)科技有限公司 速度预测方法、装置和电子设备
CN111721298A (zh) * 2020-06-24 2020-09-29 重庆赛迪奇智人工智能科技有限公司 一种slam室外大场景精准定位方法
CN111721289B (zh) * 2020-06-28 2022-06-03 阿波罗智能技术(北京)有限公司 自动驾驶中车辆定位方法、装置、设备、存储介质及车辆
CN111879309A (zh) * 2020-06-30 2020-11-03 同济大学 一种车载定位数据集成装置及其方法
CN111950504B (zh) * 2020-08-21 2024-04-16 东软睿驰汽车技术（沈阳）有限公司 车辆检测方法、装置以及电子设备
CN112268557B (zh) * 2020-09-22 2024-03-05 宽凳(湖州)科技有限公司 一种城市场景实时高精定位方法
CN112902951B (zh) * 2021-01-21 2024-07-26 深圳市镭神智能系统有限公司 一种行驶设备的定位方法、装置、设备及存储介质
CN112880664A (zh) * 2021-01-21 2021-06-01 深圳市镭神智能系统有限公司 一种行驶设备的定位方法、装置、设备及存储介质
CN113029139B (zh) * 2021-04-07 2023-07-28 中国电子科技集团公司第二十八研究所 基于运动检测的机场飞行区车辆差分北斗/sins组合导航方法
CN113188539B (zh) * 2021-04-27 2024-07-16 深圳亿嘉和科技研发有限公司 一种巡检机器人的组合定位方法
CN113109851A (zh) * 2021-06-15 2021-07-13 北京三快在线科技有限公司 一种异常检测方法、装置、存储介质及电子设备
CN113465628B (zh) * 2021-06-17 2024-07-09 杭州鸿泉物联网技术股份有限公司 惯性测量单元数据补偿方法及系统
CN114323706B (zh) * 2021-11-22 2024-04-12 卡斯柯信号有限公司 一种列车ato控车故障检测方法、装置、设备及介质
CN113820516B (zh) * 2021-11-22 2022-04-22 禾多阡陌科技(北京)有限公司 车辆轮速矫正方法、装置、电子设备和计算机可读介质
CN113936044B (zh) * 2021-12-17 2022-03-18 武汉锐科光纤激光技术股份有限公司 激光设备运动状态的检测方法、装置、计算机设备及介质

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108303710A (zh) * 2018-06-12 2018-07-20 江苏中科院智能科学技术应用研究院 基于三维激光雷达的无人机多场景定位建图方法
CN108646764A (zh) * 2018-07-25 2018-10-12 吉林大学 基于规定路线的无人驾驶车辆及控制方法、装置和系统
CN108732603A (zh) * 2017-04-17 2018-11-02 百度在线网络技术（北京）有限公司 用于定位车辆的方法和装置
CN110307836A (zh) * 2019-07-10 2019-10-08 北京智行者科技有限公司 一种用于无人清扫车辆贴边清扫的精确定位方法

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108732603A (zh) * 2017-04-17 2018-11-02 百度在线网络技术（北京）有限公司 用于定位车辆的方法和装置
CN108303710A (zh) * 2018-06-12 2018-07-20 江苏中科院智能科学技术应用研究院 基于三维激光雷达的无人机多场景定位建图方法
CN108646764A (zh) * 2018-07-25 2018-10-12 吉林大学 基于规定路线的无人驾驶车辆及控制方法、装置和系统
CN110307836A (zh) * 2019-07-10 2019-10-08 北京智行者科技有限公司 一种用于无人清扫车辆贴边清扫的精确定位方法

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
A Robust Vehicle Localization Approach Based on GNSS/IMU/DMI/LiDAR Sensor Fusion for Autonomous Vehicles;Xiaoli Meng 等;《sensors》;20170918;第1-19页 *
城市环境下车辆智能融合定位技术研究;徐启敏;《中国博士学位论文全文数据库》;20181015;第1-132页 *
基于序贯式卡尔曼滤波的车辆定位新方法;汤代佳 等;《科技与创新》;20180630;第34-36页 *
基于扩展卡尔曼滤波的智能车辆多传感器融合定位系统研究;邹勇松;《中国优秀硕士学位论文全文数据库》;20190115;第1-61页 *

Also Published As

Publication number Publication date
CN111102978A (zh) 2020-05-05

Similar Documents

Publication Publication Date Title
CN111102978B (zh) 一种车辆运动状态确定的方法、装置及电子设备
CN111721289B (zh) 自动驾驶中车辆定位方法、装置、设备、存储介质及车辆
CN109931926B (zh) 一种基于站心坐标系的小型无人机无缝自主式导航方法
Garratt et al. Integration of GPS/INS/vision sensors to navigate unmanned aerial vehicles
Fakharian et al. Adaptive Kalman filtering based navigation: An IMU/GPS integration approach
CN102506868B (zh) 基于联邦滤波的sins/smans/trns组合导航方法及系统
Crocoll et al. Model‐aided navigation for a quadrotor helicopter: A novel navigation system and first experimental results
CN112113582A (zh) 时间同步处理方法、电子设备及存储介质
CN111982106A (zh) 导航方法、装置、存储介质及电子装置
CN112146655B (zh) 一种BeiDou/SINS紧组合导航系统弹性模型设计方法
CN109031269A (zh) 基于毫米波雷达的定位方法、系统、设备及存储介质
CN111338383A (zh) 基于gaas的自主飞行方法及系统、存储介质
CN112946681B (zh) 融合组合导航信息的激光雷达定位方法
CN110988949A (zh) 定位方法、定位装置、计算机可读存储介质与可移动设备
CN109506660A (zh) 一种用于仿生导航的姿态最优化解算方法
CN115711616B (zh) 一种室内室外穿越无人机的平顺定位方法及装置
CN113009816B (zh) 时间同步误差的确定方法及装置、存储介质及电子装置
CN113483759A (zh) 一种GNSS、INS、Vision组合导航系统完好性保护级计算方法
CN117213480A (zh) 一种传递对准方法、系统、设备及存储介质
CN117320148A (zh) 多源数据融合定位方法、系统、电子设备及存储介质
Forno et al. Techniques for improving localization applications running on low-cost IoT devices
CN116222541A (zh) 利用因子图的智能多源组合导航方法及装置
CN114088104B (zh) 一种自动驾驶场景下的地图生成方法
CN115790603A (zh) 一种用于信息拒止环境下的无人机动态目标估计方法
CN111473786A (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
TR01 Transfer of patent right

Effective date of registration: 20240508

Address after: Room 6227, No. 999, Changning District, Shanghai 200050

Patentee after: Shenlan robot (Shanghai) Co.,Ltd.

Country or region after: China

Address before: Unit 1001, 369 Weining Road, Changning District, Shanghai, 200336 (9th floor of actual floor)

Patentee before: DEEPBLUE TECHNOLOGY (SHANGHAI) Co.,Ltd.

Country or region before: China

TR01 Transfer of patent right