CN111751857A - 一种车辆位姿的估算方法、装置、存储介质及系统 - Google Patents

一种车辆位姿的估算方法、装置、存储介质及系统 Download PDF

Info

Publication number
CN111751857A
CN111751857A CN202010652628.5A CN202010652628A CN111751857A CN 111751857 A CN111751857 A CN 111751857A CN 202010652628 A CN202010652628 A CN 202010652628A CN 111751857 A CN111751857 A CN 111751857A
Authority
CN
China
Prior art keywords
vehicle
moving distance
representing
moment
current moment
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
Application number
CN202010652628.5A
Other languages
English (en)
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.)
FAW Group Corp
Original Assignee
FAW Group Corp
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 FAW Group Corp filed Critical FAW Group Corp
Priority to CN202010652628.5A priority Critical patent/CN111751857A/zh
Publication of CN111751857A publication Critical patent/CN111751857A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/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
    • 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/393—Trajectory determination or predictive tracking, e.g. Kalman filtering
    • 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/53—Determining attitude

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)

Abstract

本发明公开了一种车辆位姿的估算方法、装置、存储介质及系统。方法包括：通过脉冲传感器获取在预设采样周期内车辆后轮的移动距离；根据车辆后轮的移动距离，计算在预设采样周期内车辆的航向角变化量及车辆的移动距离；根据预设采样周期内车辆的移动距离及车辆的航向角变化量，对上一时刻车辆的移动距离和航向角进行迭代计算，确定当前时刻车辆的移动距离和航向角；基于卡尔曼滤波算法，根据当前时刻车辆的移动距离对当前时刻车辆的全球定位系统GPS的观测位置进行校正，并将校正后的观测位置及当前时刻车辆的航向角作为当前时刻车辆的位姿信息。通过上述技术方案，提高获取的车辆位姿信息的准确性，从而进一步提高车辆行驶的安全性。

Description

一种车辆位姿的估算方法、装置、存储介质及系统
技术领域
本发明实施例涉及车辆控制技术领域，尤其涉及一种车辆位姿的估算方法、装置、存储介质及系统。
背景技术
自动驾驶技术的实现，需要实时获取车辆的位姿信息(位置和航向角)。目前，大多数车辆均是基于高精度GPS(Global Positioning System，全球定位系统)进行定位，另外结合陀螺仪、加速度计等进行车辆航向角的计算。然而，由于民用导航卫星的发射信号存在高频扰动信号，导致车辆定位存在一定误差。另外，目前的车辆位姿估算方法都是基于高精度传感器实现，这使得自动驾驶技术的实现成本过高，并且，单一传感器的使用并未使车辆位姿得到一个精准的数据信息，然而多传感器的数据融合算法较为复杂，且其算法的优劣直接影响了数据精度。再者，在车辆行驶过程中，避免不了遇到路面、湿度、温度的影响，存在着诸多影响车身稳定性的因素。因此，如何准确确定车辆的位姿数据变得至关重要。
发明内容
本发明提供一种车辆位姿的估算方法、装置、存储介质及系统，可以准确地获取车辆的位姿数据，提高车辆行驶过程中的安全性。
第一方面，本发明实施例提供了一种车辆位姿的估算方法，包括：
通过脉冲传感器获取在预设采样周期内车辆后轮的移动距离；
根据所述车辆后轮的移动距离，计算在所述预设采样周期内所述车辆的航向角变化量及所述车辆的移动距离；
根据所述预设采样周期内所述车辆的移动距离及所述车辆的航向角变化量，对上一时刻所述车辆的移动距离和航向角进行迭代计算，确定当前时刻所述车辆的移动距离和航向角；
基于卡尔曼滤波算法，根据当前时刻所述车辆的移动距离对当前时刻所述车辆的全球定位系统GPS的观测位置进行校正，并将校正后的观测位置及当前时刻所述车辆的航向角作为当前时刻所述车辆的位姿信息。
第二方面，本发明实施例还提供了一种车辆位姿的估算装置，包括：
获取模块，用于通过脉冲传感器获取在预设采样周期内车辆后轮的移动距离；
计算模块，用于根据所述车辆后轮的移动距离，计算在所述预设采样周期内所述车辆的航向角变化量及所述车辆的移动距离；
确定模块，用于根据所述预设采样周期内所述车辆的移动距离及所述车辆的航向角变化量，对上一时刻所述车辆的移动距离和航向角进行迭代计算，确定当前时刻所述车辆的移动距离和航向角；
校正模块，用于基于卡尔曼滤波算法，根据当前时刻所述车辆的移动距离对当前时刻所述车辆的全球定位系统GPS的观测位置进行校正，并将校正后的观测位置及当前时刻所述车辆的航向角作为当前时刻所述车辆的位姿信息。
第三方面，本发明实施例还提供了一种计算机存储介质，其上存储有计算机程序，该程序被处理器执行时实现本发明任意实施例所提供的车辆位姿的估算方法。
第四方面，本发明实施例提供了一种车辆位姿的估算系统，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现如本发明实施例提供的车辆位姿的估算方法。
本发明提供的车辆位姿的估算方案，包括：通过脉冲传感器获取在预设采样周期内车辆后轮的移动距离；根据所述车辆后轮的移动距离，计算在所述预设采样周期内所述车辆的航向角变化量及所述车辆的移动距离；根据所述预设采样周期内所述车辆的移动距离及所述车辆的航向角变化量，对上一时刻所述车辆的移动距离和航向角进行迭代计算，确定当前时刻所述车辆的移动距离和航向角；基于卡尔曼滤波算法，根据当前时刻所述车辆的移动距离对当前时刻所述车辆的全球定位系统GPS的观测位置进行校正，并将校正后的观测位置及当前时刻所述车辆的航向角作为当前时刻所述车辆的位姿信息。在本发明实施例提供的技术方案中，通过脉冲传感器获取车辆后轮的移动距离，利用迭代算法可估算出任意时刻车辆的移动距离和航向角，然后基于卡尔曼滤波算法，根据估算的当前时刻车辆的移动距离对GPS的观测位置进行校正，可有效消除GPS的系统扰动，提高获取的车辆位姿信息的准确性，从而进一步提高车辆行驶的安全性。
附图说明
图1为本发明实施例提供的一种车辆位姿的估算方法的流程示意图；
图2为本发明实施例提供的车辆模型坐标原理示意图；
图3为本发明实施例提供的另一种车辆位姿的估算方法的流程示意图；
图4为本发明实施例提供的一种车辆位姿的估算装置的结构框图；
图5为本发明实施例提供的一种车辆位姿的估算系统的结构框图。
具体实施方式
下面结合附图和实施例对本发明作进一步的详细说明。可以理解的是，此处所描述的具体实施例仅仅用于解释本发明，而非对本发明的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与本发明相关的部分而非全部结构。
图1为本发明实施例提供的一种车辆位姿的估算方法的流程示意图，该方法可以由车辆位姿的估算装置执行，其中该装置可由软件和/或硬件实现，一般可集成在车辆位姿的估算系统中，通常车辆位姿的估算系统可配置于车辆中。如图1所示，该方法包括：
步骤101、通过脉冲传感器获取在预设采样周期内车辆后轮的移动距离。
在本发明实施例中，在车辆行驶的过程中，通过脉冲传感器获取在预设采样周期内车辆后轮的移动距离。通常，车辆后轮包括车辆后右轮和车辆后左轮，通过脉冲传感器分别获取在预设采样周期内车辆后右轮和车辆后左轮的移动距离。
可选的，通过脉冲传感器获取在预设采样周期内车辆后轮的移动距离，包括：根据如下公式计算所述车辆后轮的移动距离：S＝n*m；其中，S表示所述车辆后轮的移动距离，n表示在预设采样周期内所述脉冲传感器采集的脉冲个数，m表示一个脉冲的距离。
步骤102、根据所述车辆后轮的移动距离，计算在所述预设采样周期内所述车辆的航向角变化量及所述车辆的移动距离。
可选的，根据所述车辆后轮的移动距离，计算在所述预设采样周期内所述车辆的航向角变化量及所述车辆的移动距离，包括：根据如下公式计算所述车辆的航向角变化量和所述车辆的移动距离：
Δθ＝(ΔSr-ΔSl)/L
ΔS＝(ΔSr+ΔSl)/2
其中，ΔSr表示后右轮的移动距离，ΔSl表示后左轮的移动距离，L表示后右轮与后左轮之间的距离，Δθ表示在所述预设采样周期内所述车辆的航向角变化量，ΔS表示在所述预设采样周期内所述车辆的移动距离。
步骤103、根据所述预设采样周期内所述车辆的移动距离及所述车辆的航向角变化量，对上一时刻所述车辆的移动距离和航向角进行迭代计算，确定当前时刻所述车辆的移动距离和航向角。
在本发明实施例中，根据预设采样周期内车辆的移动距离对上一时刻车辆的移动距离进行迭代计算，确定当前时刻车辆的移动距离；同时，根据预设采样周期内车辆的航向角变化量，对上一时刻车辆的航向角进行迭代计算，确定当前时刻车辆的航向角。其中，上一时刻可以理解为上一采样时刻，当前时刻可以理解为当前采样时刻。
可选的，根据所述预设采样周期内所述车辆的移动距离及所述车辆的航向角变化量，对上一时刻所述车辆的移动距离和航向角进行迭代计算，确定当前时刻所述车辆的移动距离和航向角，包括：根据如下公式计算当前时刻所述车辆的移动距离和航向角：
Figure BDA0002575547850000061
θ1＝θ0+Δθ
其中，ΔS1表示当前时刻所述车辆的移动距离，
Figure BDA0002575547850000062
表示当前时刻所述车辆的移动速度，a表示当前时刻所述车辆的加速度，w表示所述车辆的随机加速度，θ1表示当前时刻所述车辆的航向角，θ0表示上一时刻所述车辆的航向角，Δθ表示在预设采样周期内所述车辆的航向角变化量，t0表预设采样周期。图2为本发明实施例提供的车辆模型坐标原理示意图。基于图2所示的坐标原理，通过上述公式可计算出当前时刻车辆的移动距离和航向角。
示例性的，在车辆行驶过程中，会由于路面不平或环境因素对车辆的加速度造成影响，进而产生随机加速度，w为车辆的随机加速度。其中，可以通过确定GPS获取车辆当前所处位置，进而根据车辆当前所处位置确定车辆当前所处位置的路面等级，在路面等级与随机加速度的对应关系表中，查找与车辆当前所处的路面等级对应的随机加速度。
步骤104、基于卡尔曼滤波算法，根据当前时刻所述车辆的移动距离对当前时刻所述车辆的全球定位系统GPS的观测位置进行校正，并将校正后的观测位置及当前时刻所述车辆的航向角作为当前时刻所述车辆的位姿信息。
在本发明实施例中，由于车辆中的GPS存在系统扰动，导致GPS的观测位置与车辆的真实位置存在一定偏差，因此，基于卡尔曼滤波算法，根据当前时刻车辆的移动距离对当前时刻车辆的全球定位系统GPS的观测位置进行校正，以消除GPS的系统扰动，并将校正后的车辆的观测位置和通过步骤103确定的当前时刻车辆的航向角作为当前时刻车辆的位姿信息。
可选的，基于卡尔曼滤波算法，根据当前时刻所述车辆的移动距离对当前时刻所述车辆的GPS的观测位置进行校正，包括：基于当前时刻所述车辆的观测位置构建所述车辆的位置观测方程；在根据所述预设采样周期内所述车辆的移动距离，对上一时刻所述车辆的移动距离进行迭代计算的过程中，构建当前时刻所述车辆的位置状态方程；基于所述位置状态方程和所述位置观测方程构建所述车辆的状态空间模型；将所述状态空间模型扩展到四维空间，以确定卡尔曼滤波算法中的目标矩阵，其中，所述目标矩阵包括观测矩阵、第一状态转移矩阵和第二状态转移矩阵；基于当前时刻所述车辆的移动距离、当前时刻所述车辆的GPS的观测位置以及所述目标矩阵，采用卡尔曼滤波算法，获取当前时刻所述车辆的最优位置；基于所述最优位置对当前时刻所述车辆的GPS的观测位置进行校正。
可选的，基于当前时刻所述车辆的观测位置构建所述车辆的位置观测方程，包括：根据如下公式构建所述车辆的位置观测方程：
Figure BDA0002575547850000071
其中，z表示所述车辆的GPS的观测移动距离，v表示所述车辆中的GPS的系统扰动，Δs1表示无GPS系统扰动时所述车辆的真实移动距离，
Figure BDA0002575547850000072
表示所述车辆的GPS的观测移动速度。
在根据所述预设采样周期内所述车辆的移动距离，对上一时刻所述车辆的移动距离进行迭代计算的过程中，构建当前时刻所述车辆的位置状态方程，包括：根据如下公式构建所述车辆的位置状态方程：
Figure BDA0002575547850000073
其中，ΔS1表示当前时刻所述车辆的移动距离，
Figure BDA0002575547850000081
表示当前时刻所述车辆的移动速度，a表示当前时刻所述车辆的加速度，w表示所述车辆的随机加速度，t0表示预设采样周期。
基于所述位置状态方程和所述位置观测方程构建所述车辆的状态空间模型，包括：基于所述位置状态方程和所述位置观测方程构建的所述车辆的状态空间模型如下：
S(k)＝A′S(k-1)+Ba+Cw
z(k)＝H′s(k)+v
其中，
Figure BDA0002575547850000082
H′＝[1 0],S(k)表示相对于所述车辆的初始位置当前时刻所述车辆的移动距离，S(k-1)表示相对于所述车辆的初始位置上一时刻所述车辆的移动距离，z(k)表示当前时刻所述车辆的GPS的观测移动距离，v表示所述车辆中的GPS的系统扰动，w(k)表示当前时刻无GPS的系统扰动时所述车辆的真实移动距离；
将所述状态空间模型扩展到四维空间，以确定卡尔曼滤波算法中的目标矩阵，包括：根据如下公式将所述状态空间模型扩展到四维空间：
Figure BDA0002575547850000083
Figure BDA0002575547850000084
其中，X(k)表示对S(k)矢量分解后的状态量，x(k)表示相对于所述车辆的初始位置当前时刻所述车辆在水平方向的移动距离，y(k)表示相对于所述车辆的初始位置当前时刻所述车辆在竖直方向的移动距离，x(k-1)表示相对于所述车辆的初始位置上一时刻所述车辆在水平方向的移动距离，y(k-1)表示相对于所述车辆的初始位置当前时刻所述车辆在竖直方向的移动距离，
Figure BDA0002575547850000091
表示当前时刻所述车辆在水平方向的移动速度，
Figure BDA0002575547850000092
表示当前时刻所述车辆在竖直方向的移动速度，
Figure BDA0002575547850000093
表示上一时刻所述车辆在水平方向的移动速度，
Figure BDA0002575547850000094
表示上一时刻所述车辆在竖直方向的移动速度，Z(k)表示对z(k)矢量分解后的状态量,w2×1表示将当前时刻所述车辆的加速度a和随机加速度w进行归一化处理后生成的合成加速度，v2×1表示所述车辆中的GPS的系统扰动v扩展到二维空间的系统扰动，
Figure BDA0002575547850000095
A表示第一状态转移矩阵，Γ表示第二状态转移矩阵，H表示观测矩阵。
可选的，基于当前时刻所述车辆的移动距离、当前时刻所述车辆的观测位置以及所述目标矩阵，采用卡尔曼滤波算法，获取当前时刻所述车辆的最优位置，包括：根据如下公式确定当前时刻所述车辆的最优位置，包括：
Figure BDA0002575547850000096
其中，Xpre(k)表示当前时刻所述车辆的位置预测值，Xkf(k-1)表示上一时刻所述车辆的预估值，Ppre(k)表示当前时刻协方差预测值，Q表示过程噪声，Kg表示卡尔曼增益，R表示观测噪声，In表示单位矩阵，Xkf(k)表示当前时刻所述车辆的最优位置，P(k-1)表示上一时刻协方差预估值，Pk表示当前时刻协方差预估值。
在本发明实施例中，过程噪声Q为当前时刻车辆的合成加速度w2×1，观测噪声R为扩展到二维空间的系统扰动v2×1，当前时刻所述车辆的位置预测值Xpre(k)为当前时刻车辆的GPS的观测移动距离z(k)，上一时刻车辆的预估值Xkf(k-1)为相对于车辆的初始位置上一时刻所述车辆的移动距离S(k-1)的矢量分解后的状态量。在本发明实施例中，根据当前时刻车辆的最优位置Xkf(k)对当前时刻车辆的GPS的观测位置进行校正，并将校正后的观测位置作为当前时刻车辆的真实位置。
本发明提供的车辆位姿的估算方案，包括：通过脉冲传感器获取在预设采样周期内车辆后轮的移动距离；根据所述车辆后轮的移动距离，计算在所述预设采样周期内所述车辆的航向角变化量及所述车辆的移动距离；根据所述预设采样周期内所述车辆的移动距离及所述车辆的航向角变化量，对上一时刻所述车辆的移动距离和航向角进行迭代计算，确定当前时刻所述车辆的移动距离和航向角；基于卡尔曼滤波算法，根据当前时刻所述车辆的移动距离对当前时刻所述车辆的全球定位系统GPS的观测位置进行校正，并将校正后的观测位置及当前时刻所述车辆的航向角作为当前时刻所述车辆的位姿信息。在本发明实施例提供的技术方案中，通过脉冲传感器获取车辆后轮的移动距离，利用迭代算法可估算出任意时刻车辆的移动距离和航向角，然后基于卡尔曼滤波算法，根据估算的当前时刻车辆的移动距离对GPS的观测位置进行校正，可有效消除GPS的系统扰动，提高获取的车辆位姿信息的准确性，从而进一步提高车辆行驶的安全性。
图3为本发明实施例提供的另一种车辆位姿的估算方法的流程示意图，如图3所示，该方法包括如下步骤：
步骤301、通过脉冲传感器获取在预设采样周期内车辆后轮的移动距离。
可选的，通过脉冲传感器获取在预设采样周期内车辆后轮的移动距离，包括：根据如下公式计算所述车辆后轮的移动距离：
S＝n*m
其中，S表示所述车辆后轮的移动距离，n表示在预设采样周期内所述脉冲传感器采集的脉冲个数，m表示一个脉冲的距离。
步骤302、根据所述车辆后轮的移动距离，计算在所述预设采样周期内所述车辆的航向角变化量及所述车辆的移动距离。
可选的，根据所述车辆后轮的移动距离，计算在所述预设采样周期内所述车辆的航向角变化量及所述车辆的移动距离，包括：根据如下公式计算所述车辆的航向角变化量和所述车辆的移动距离：
Δθ＝(ΔSr-ΔSl)/L
ΔS＝(ΔSr+ΔSl)/2
其中，ΔSr表示后右轮的移动距离，ΔSl表示后左轮的移动距离，L表示后右轮与后左轮之间的距离，Δθ表示在所述预设采样周期内所述车辆的航向角变化量，ΔS表示在所述预设采样周期内所述车辆的移动距离。
步骤303、根据所述预设采样周期内所述车辆的移动距离及所述车辆的航向角变化量，对上一时刻所述车辆的移动距离和航向角进行迭代计算，确定当前时刻所述车辆的移动距离和航向角。
可选的，根据所述预设采样周期内所述车辆的移动距离及所述车辆的航向角变化量，对上一时刻所述车辆的移动距离和航向角进行迭代计算，确定当前时刻所述车辆的移动距离和航向角，包括：根据如下公式计算当前时刻所述车辆的移动距离和航向角：
Figure BDA0002575547850000121
θ1＝θ0+Δθ
其中，ΔS1表示当前时刻所述车辆的移动距离，
Figure BDA0002575547850000122
表示当前时刻所述车辆的移动速度，a表示当前时刻所述车辆的加速度，w表示所述车辆的随机加速度，θ1表示当前时刻所述车辆的航向角，θ0表示上一时刻所述车辆的航向角，Δθ表示在预设采样周期内所述车辆的航向角变化量，t0表预设采样周期。
步骤304、基于当前时刻所述车辆的观测位置构建所述车辆的位置观测方程。
可选的，基于当前时刻所述车辆的观测位置构建所述车辆的位置观测方程，包括：根据如下公式构建所述车辆的位置观测方程：
Figure BDA0002575547850000123
其中，z表示所述车辆的GPS的观测移动距离，v表示所述车辆中的GPS的系统扰动，Δs1表示无GPS系统扰动时所述车辆的真实移动距离，
Figure BDA0002575547850000124
表示所述车辆的GPS的观测移动速度。
步骤305、在根据所述预设采样周期内所述车辆的移动距离，对上一时刻所述车辆的移动距离进行迭代计算的过程中，构建当前时刻所述车辆的位置状态方程。
可选的，在根据所述预设采样周期内所述车辆的移动距离，对上一时刻所述车辆的移动距离进行迭代计算的过程中，构建当前时刻所述车辆的位置状态方程，包括：根据如下公式构建所述车辆的位置状态方程：
Figure BDA0002575547850000131
其中，ΔS1表示当前时刻所述车辆的移动距离，
Figure BDA0002575547850000132
表示当前时刻所述车辆的移动速度，a表示当前时刻所述车辆的加速度，w表示所述车辆的随机加速度，t0表示预设采样周期。
步骤306、基于所述位置状态方程和所述位置观测方程构建所述车辆的状态空间模型。
可选的，基于所述位置状态方程和所述位置观测方程构建所述车辆的状态空间模型，包括：基于所述位置状态方程和所述位置观测方程构建的所述车辆的状态空间模型如下：
S(k)＝A′S(k-1)+Ba+Cw
z(k)＝H′s(k)+v
其中，
Figure BDA0002575547850000133
H′＝[1 0],S(k)表示相对于所述车辆的初始位置当前时刻所述车辆的移动距离，S(k-1)表示相对于所述车辆的初始位置上一时刻所述车辆的移动距离，z(k)表示当前时刻所述车辆的GPS的观测移动距离，v表示所述车辆中的GPS的系统扰动，s(k)表示当前时刻无GPS的系统扰动时所述车辆的真实移动距离。
步骤307、将所述状态空间模型扩展到四维空间，以确定卡尔曼滤波算法中的目标矩阵，其中，所述目标矩阵包括观测矩阵、第一状态转移矩阵和第二状态转移矩阵。
可选的，将所述状态空间模型扩展到四维空间，以确定卡尔曼滤波算法中的目标矩阵，包括：根据如下公式将所述状态空间模型扩展到四维空间：
Figure BDA0002575547850000141
Figure BDA0002575547850000142
其中，X(k)表示对S(k)矢量分解后的状态量，x(k)表示相对于所述车辆的初始位置当前时刻所述车辆在水平方向的移动距离，y(k)表示相对于所述车辆的初始位置当前时刻所述车辆在竖直方向的移动距离，x(k-1)表示相对于所述车辆的初始位置上一时刻所述车辆在水平方向的移动距离，y(k-1)表示相对于所述车辆的初始位置当前时刻所述车辆在竖直方向的移动距离，
Figure BDA0002575547850000143
表示当前时刻所述车辆在水平方向的移动速度，
Figure BDA0002575547850000144
表示当前时刻所述车辆在竖直方向的移动速度，
Figure BDA0002575547850000145
表示上一时刻所述车辆在水平方向的移动速度，
Figure BDA0002575547850000146
表示上一时刻所述车辆在竖直方向的移动速度，Z(k)表示对z(k)矢量分解后的状态量,w2×1表示将当前时刻所述车辆的加速度a和随机加速度w进行归一化处理后生成的合成加速度，v2×1表示所述车辆中的GPS的系统扰动v扩展到二维空间的系统扰动，
Figure BDA0002575547850000147
A表示第一状态转移矩阵，Γ表示第二状态转移矩阵，H表示观测矩阵。
步骤308、基于当前时刻所述车辆的移动距离、当前时刻所述车辆的GPS的观测位置以及所述目标矩阵，采用卡尔曼滤波算法，获取当前时刻所述车辆的最优位置。
可选的，基于当前时刻所述车辆的移动距离、当前时刻所述车辆的观测位置以及所述目标矩阵，采用卡尔曼滤波算法，获取当前时刻所述车辆的最优位置，包括：根据如下公式确定当前时刻所述车辆的最优位置，包括：
Figure BDA0002575547850000151
其中，Xpre(k)表示当前时刻所述车辆的位置预测值，Xkf(k-1)表示上一时刻所述车辆的预估值，Ppre(k)表示当前时刻协方差预测值，Q表示过程噪声，Kg表示卡尔曼增益，R表示观测噪声，In表示单位矩阵，Xkf(k)表示当前时刻所述车辆的最优位置，P(k-1)表示上一时刻协方差预估值，Pk表示当前时刻协方差预估值。
步骤309、基于所述最优位置对当前时刻所述车辆的GPS的观测位置进行校正。
步骤310、将校正后的观测位置及当前时刻所述车辆的航向角作为当前时刻所述车辆的位姿信息。
本发明实施例提供的车辆位姿的估算方法，通过脉冲传感器获取车辆后轮的移动距离，利用迭代算法可估算出任意时刻车辆的移动距离和航向角，然后基于卡尔曼滤波算法，根据估算的当前时刻车辆的移动距离对GPS的观测位置进行校正，可有效消除GPS的系统扰动，并考虑路面、湿度及温度等对车辆加速度的影响，将因此产生的随机加速度量纲化为可辨识的噪声强度，进一步提高了获取的车辆位姿信息的准确性，从而进一步提高车辆行驶的安全性。
图4为本发明实施例提供的一种车辆位姿的估算装置的结构框图，该装置可由软件和/或硬件实现，一般集成在车辆位姿的估算系统中，可通过执行车辆位姿的估算方法来对车辆位姿进行估算。如图4所示，该装置包括：
获取模块401，用于通过脉冲传感器获取在预设采样周期内车辆后轮的移动距离；
计算模块402，用于根据所述车辆后轮的移动距离，计算在所述预设采样周期内所述车辆的航向角变化量及所述车辆的移动距离；
确定模块403，用于根据所述预设采样周期内所述车辆的移动距离及所述车辆的航向角变化量，对上一时刻所述车辆的移动距离和航向角进行迭代计算，确定当前时刻所述车辆的移动距离和航向角；
校正模块404，用于基于卡尔曼滤波算法，根据当前时刻所述车辆的移动距离对当前时刻所述车辆的全球定位系统GPS的观测位置进行校正，并将校正后的观测位置及当前时刻所述车辆的航向角作为当前时刻所述车辆的位姿信息。
本发明提供的车辆位姿的估算装置，通过脉冲传感器获取在预设采样周期内车辆后轮的移动距离；根据所述车辆后轮的移动距离，计算在所述预设采样周期内所述车辆的航向角变化量及所述车辆的移动距离；根据所述预设采样周期内所述车辆的移动距离及所述车辆的航向角变化量，对上一时刻所述车辆的移动距离和航向角进行迭代计算，确定当前时刻所述车辆的移动距离和航向角；基于卡尔曼滤波算法，根据当前时刻所述车辆的移动距离对当前时刻所述车辆的全球定位系统GPS的观测位置进行校正，并将校正后的观测位置及当前时刻所述车辆的航向角作为当前时刻所述车辆的位姿信息。在本发明实施例提供的技术方案中，通过脉冲传感器获取车辆后轮的移动距离，利用迭代算法可估算出任意时刻车辆的移动距离和航向角，然后基于卡尔曼滤波算法，根据估算的当前时刻车辆的移动距离对GPS的观测位置进行校正，可有效消除GPS的系统扰动，提高获取的车辆位姿信息的准确性，从而进一步提高车辆行驶的安全性。
可选的，所述获取模块，用于：
根据如下公式计算所述车辆后轮的移动距离：
S＝n*m
其中，S表示所述车辆后轮的移动距离，n表示在预设采样周期内所述脉冲传感器采集的脉冲个数，m表示一个脉冲的距离。
可选的，所述计算模块，用于：
根据如下公式计算所述车辆的航向角变化量和所述车辆的移动距离：
Δθ＝(ΔSr-ΔSl)/L
ΔS＝(ΔSr+ΔSl)/2
其中，ΔSr表示后右轮的移动距离，ΔSl表示后左轮的移动距离，L表示后右轮与后左轮之间的距离，Δθ表示在所述预设采样周期内所述车辆的航向角变化量，ΔS表示在所述预设采样周期内所述车辆的移动距离。
可选的，所述确定模块，用于：
根据如下公式计算当前时刻所述车辆的移动距离和航向角：
Figure BDA0002575547850000171
θ1＝θ0+Δθ
其中，ΔS1表示当前时刻所述车辆的移动距离，
Figure BDA0002575547850000172
表示当前时刻所述车辆的移动速度，a表示当前时刻所述车辆的加速度，w表示所述车辆的随机加速度，θ1表示当前时刻所述车辆的航向角，θ0表示上一时刻所述车辆的航向角，Δθ表示在预设采样周期内所述车辆的航向角变化量，t0表预设采样周期。
可选的，所述校正模块，包括：
观测方程构建单元，用于基于当前时刻所述车辆的观测位置构建所述车辆的位置观测方程；
状态方程构建单元，用于在根据所述预设采样周期内所述车辆的移动距离，对上一时刻所述车辆的移动距离进行迭代计算的过程中，构建当前时刻所述车辆的位置状态方程；
空间模型构建单元，用于基于所述位置状态方程和所述位置观测方程构建所述车辆的状态空间模型；
目标矩阵确定单元，用于将所述状态空间模型扩展到四维空间，以确定卡尔曼滤波算法中的目标矩阵，其中，所述目标矩阵包括观测矩阵、第一状态转移矩阵和第二状态转移矩阵；
最优位置获取单元，用于基于当前时刻所述车辆的移动距离、当前时刻所述车辆的GPS的观测位置以及所述目标矩阵，采用卡尔曼滤波算法，获取当前时刻所述车辆的最优位置；
位置校正单元，用于基于所述最优位置对当前时刻所述车辆的GPS的观测位置进行校正。
可选的，所述观测方程构建单元，用于：
根据如下公式构建所述车辆的位置观测方程：
Figure BDA0002575547850000181
其中，z表示所述车辆的GPS的观测移动距离，v表示所述车辆中的GPS的系统扰动，Δs1表示无GPS系统扰动时所述车辆的真实移动距离，
Figure BDA0002575547850000182
表示所述车辆的GPS的观测移动速度；
所述状态方程构建单元，用于，包括：
根据如下公式构建所述车辆的位置状态方程：
Figure BDA0002575547850000191
其中，ΔS1表示当前时刻所述车辆的移动距离，
Figure BDA0002575547850000192
表示当前时刻所述车辆的移动速度，a表示当前时刻所述车辆的加速度，w表示所述车辆的随机加速度，t0表示预设采样周期；
所述空间模型构建单元，用于：
基于所述位置状态方程和所述位置观测方程构建的所述车辆的状态空间模型如下：
S(k)＝A′S(k-1)+Ba+Cw
z(k)＝H′s(k)+v
其中，
Figure BDA0002575547850000193
H′＝[1 0],S(k)表示相对于所述车辆的初始位置当前时刻所述车辆的移动距离，S(k-1)表示相对于所述车辆的初始位置上一时刻所述车辆的移动距离，z(k)表示当前时刻所述车辆的GPS的观测移动距离，v表示所述车辆中的GPS的系统扰动，s(k)表示当前时刻无GPS的系统扰动时所述车辆的真实移动距离；
所述目标矩阵确定单元，用于：
根据如下公式将所述状态空间模型扩展到四维空间：
Figure BDA0002575547850000201
Figure BDA0002575547850000202
其中，X(k)表示对S(k)矢量分解后的状态量，x(k)表示相对于所述车辆的初始位置当前时刻所述车辆在水平方向的移动距离，y(k)表示相对于所述车辆的初始位置当前时刻所述车辆在竖直方向的移动距离，x(k-1)表示相对于所述车辆的初始位置上一时刻所述车辆在水平方向的移动距离，y(k-1)表示相对于所述车辆的初始位置当前时刻所述车辆在竖直方向的移动距离，
Figure BDA0002575547850000203
表示当前时刻所述车辆在水平方向的移动速度，
Figure BDA0002575547850000204
表示当前时刻所述车辆在竖直方向的移动速度，
Figure BDA0002575547850000205
表示上一时刻所述车辆在水平方向的移动速度，
Figure BDA0002575547850000206
表示上一时刻所述车辆在竖直方向的移动速度，Z(k)表示对z(k)矢量分解后的状态量,w2×1表示将当前时刻所述车辆的加速度a和随机加速度w进行归一化处理后生成的合成加速度，v2×1表示所述车辆中的GPS的系统扰动v扩展到二维空间的系统扰动，
Figure BDA0002575547850000207
A表示第一状态转移矩阵，Γ表示第二状态转移矩阵，H表示观测矩阵。
可选的，所述最优位置获取单元，用于：
根据如下公式确定当前时刻所述车辆的最优位置，包括：
Figure BDA0002575547850000211
其中，Xpre(k)表示当前时刻所述车辆的位置预测值，Xkf(k-1)表示上一时刻所述车辆的预估值，Ppre(k)表示当前时刻协方差预测值，Q表示过程噪声，Kg表示卡尔曼增益，R表示观测噪声，In表示单位矩阵，Xkf(k)表示当前时刻所述车辆的最优位置，P(k-1)表示上一时刻协方差预估值，Pk表示当前时刻协方差预估值。
本发明实施例还提供一种包含计算机可执行指令的存储介质，所述计算机可执行指令在由计算机处理器执行时用于执行车辆位姿的估算方法，该方法包括：
通过脉冲传感器获取在预设采样周期内车辆后轮的移动距离；
根据所述车辆后轮的移动距离，计算在所述预设采样周期内所述车辆的航向角变化量及所述车辆的移动距离；
根据所述预设采样周期内所述车辆的移动距离及所述车辆的航向角变化量，对上一时刻所述车辆的移动距离和航向角进行迭代计算，确定当前时刻所述车辆的移动距离和航向角；
基于卡尔曼滤波算法，根据当前时刻所述车辆的移动距离对当前时刻所述车辆的全球定位系统GPS的观测位置进行校正，并将校正后的观测位置及当前时刻所述车辆的航向角作为当前时刻所述车辆的位姿信息。
存储介质——任何的各种类型的存储器设备或存储设备。术语“存储介质”旨在包括：安装介质，例如CD-ROM、软盘或磁带装置；计算机系统存储器或随机存取存储器，诸如DRAM、DDRRAM、SRAM、EDORAM，兰巴斯(Rambus)RAM等；非易失性存储器，诸如闪存、磁介质(例如硬盘或光存储)；寄存器或其它相似类型的存储器元件等。存储介质可以还包括其它类型的存储器或其组合。另外，存储介质可以位于程序在其中被执行的第一计算机系统中，或者可以位于不同的第二计算机系统中，第二计算机系统通过网络(诸如因特网)连接到第一计算机系统。第二计算机系统可以提供程序指令给第一计算机用于执行。术语“存储介质”可以包括可以驻留在不同位置中(例如在通过网络连接的不同计算机系统中)的两个或更多存储介质。存储介质可以存储可由一个或多个处理器执行的程序指令(例如具体实现为计算机程序)。
当然，本发明实施例所提供的一种包含计算机可执行指令的存储介质，其计算机可执行指令不限于如上所述的车辆位姿的估算操作，还可以执行本发明任意实施例所提供的车辆位姿的估算方法中的相关操作。
本发明实施例提供了一种车辆位姿的估算系统，该车辆位姿的估算系统中可集成本发明实施例提供的车辆位姿的估算装置。图5为本发明实施例提供的一种车辆位姿的估算系统的结构框图。车辆位姿的估算系统500可以包括：存储器501，处理器502及存储在存储器501上并可在处理器运行的计算机程序，所述处理器502执行所述计算机程序时实现如本发明实施例所述的车辆位姿的估算方法。本发明实施例中提供的车辆位姿的估算系统，通过脉冲传感器获取在预设采样周期内车辆后轮的移动距离；根据所述车辆后轮的移动距离，计算在所述预设采样周期内所述车辆的航向角变化量及所述车辆的移动距离；根据所述预设采样周期内所述车辆的移动距离及所述车辆的航向角变化量，对上一时刻所述车辆的移动距离和航向角进行迭代计算，确定当前时刻所述车辆的移动距离和航向角；基于卡尔曼滤波算法，根据当前时刻所述车辆的移动距离对当前时刻所述车辆的全球定位系统GPS的观测位置进行校正，并将校正后的观测位置及当前时刻所述车辆的航向角作为当前时刻所述车辆的位姿信息。在本发明实施例提供的技术方案中，通过脉冲传感器获取车辆后轮的移动距离，利用迭代算法可估算出任意时刻车辆的移动距离和航向角，然后基于卡尔曼滤波算法，根据估算的当前时刻车辆的移动距离对GPS的观测位置进行校正，可有效消除GPS的系统扰动，提高获取的车辆位姿信息的准确性，从而进一步提高车辆行驶的安全性。
上述实施例中提供的车辆位姿的估算装置、存储介质及系统可执行本发明任意实施例所提供的车辆位姿的估算方法，具备执行该方法相应的功能模块和有益效果。未在上述实施例中详尽描述的技术细节，可参见本发明任意实施例所提供的车辆位姿的估算方法。
注意，上述仅为本发明的较佳实施例及所运用技术原理。本领域技术人员会理解，本发明不限于这里所述的特定实施例，对本领域技术人员来说能够进行各种明显的变化、重新调整和替代而不会脱离本发明的保护范围。因此，虽然通过以上实施例对本发明进行了较为详细的说明，但是本发明不仅仅限于以上实施例，在不脱离本发明构思的情况下，还可以包括更多其他等效实施例，而本发明的范围由所附的权利要求范围决定。

Claims (10)

1.一种车辆位姿的估算方法，其特征在于，包括：
通过脉冲传感器获取在预设采样周期内车辆后轮的移动距离；
根据所述车辆后轮的移动距离，计算在所述预设采样周期内所述车辆的航向角变化量及所述车辆的移动距离；
根据所述预设采样周期内所述车辆的移动距离及所述车辆的航向角变化量，对上一时刻所述车辆的移动距离和航向角进行迭代计算，确定当前时刻所述车辆的移动距离和航向角；
基于卡尔曼滤波算法，根据当前时刻所述车辆的移动距离对当前时刻所述车辆的全球定位系统GPS的观测位置进行校正，并将校正后的观测位置及当前时刻所述车辆的航向角作为当前时刻所述车辆的位姿信息。
2.根据权利要求1所述的方法，其特征在于，通过脉冲传感器获取在预设采样周期内车辆后轮的移动距离，包括：
根据如下公式计算所述车辆后轮的移动距离：
S＝n*m
其中，S表示所述车辆后轮的移动距离，n表示在预设采样周期内所述脉冲传感器采集的脉冲个数，m表示一个脉冲的距离。
3.根据权利要求1所述的方法，其特征在于，根据所述车辆后轮的移动距离，计算在所述预设采样周期内所述车辆的航向角变化量及所述车辆的移动距离，包括：
根据如下公式计算所述车辆的航向角变化量和所述车辆的移动距离：
Δθ＝(ΔSr-ΔSl)/L
ΔS＝(ΔSr+ΔSl)/2
其中，ΔSr表示后右轮的移动距离，ΔSl表示后左轮的移动距离，L表示后右轮与后左轮之间的距离，Δθ表示在所述预设采样周期内所述车辆的航向角变化量，ΔS表示在所述预设采样周期内所述车辆的移动距离。
4.根据权利要求1所述的方法，其特征在于，根据所述预设采样周期内所述车辆的移动距离及所述车辆的航向角变化量，对上一时刻所述车辆的移动距离和航向角进行迭代计算，确定当前时刻所述车辆的移动距离和航向角，包括：
根据如下公式计算当前时刻所述车辆的移动距离和航向角：
Figure FDA0002575547840000021
θ1＝θ0+Δθ
其中，ΔS1表示当前时刻所述车辆的移动距离，
Figure FDA0002575547840000022
表示当前时刻所述车辆的移动速度，a表示当前时刻所述车辆的加速度，w表示所述车辆的随机加速度，θ1表示当前时刻所述车辆的航向角，θ0表示上一时刻所述车辆的航向角，Δθ表示在预设采样周期内所述车辆的航向角变化量，t0表预设采样周期。
5.根据权利要求1所述的方法，其特征在于，基于卡尔曼滤波算法，根据当前时刻所述车辆的移动距离对当前时刻所述车辆的GPS的观测位置进行校正，包括：
基于当前时刻所述车辆的观测位置构建所述车辆的位置观测方程；
在根据所述预设采样周期内所述车辆的移动距离，对上一时刻所述车辆的移动距离进行迭代计算的过程中，构建当前时刻所述车辆的位置状态方程；
基于所述位置状态方程和所述位置观测方程构建所述车辆的状态空间模型；
将所述状态空间模型扩展到四维空间，以确定卡尔曼滤波算法中的目标矩阵，其中，所述目标矩阵包括观测矩阵、第一状态转移矩阵和第二状态转移矩阵；
基于当前时刻所述车辆的移动距离、当前时刻所述车辆的GPS的观测位置以及所述目标矩阵，采用卡尔曼滤波算法，获取当前时刻所述车辆的最优位置；
基于所述最优位置对当前时刻所述车辆的GPS的观测位置进行校正。
6.根据权利要求5所述的方法，其特征在于，基于当前时刻所述车辆的观测位置构建所述车辆的位置观测方程，包括：
根据如下公式构建所述车辆的位置观测方程：
Figure FDA0002575547840000031
其中，z表示所述车辆的GPS的观测移动距离，v表示所述车辆中的GPS的系统扰动，Δs1表示无GPS系统扰动时所述车辆的真实移动距离，
Figure FDA0002575547840000032
表示所述车辆的GPS的观测移动速度；
在根据所述预设采样周期内所述车辆的移动距离，对上一时刻所述车辆的移动距离进行迭代计算的过程中，构建当前时刻所述车辆的位置状态方程，包括：
根据如下公式构建所述车辆的位置状态方程：
Figure FDA0002575547840000033
其中，ΔS1表示当前时刻所述车辆的移动距离，
Figure FDA0002575547840000034
表示当前时刻所述车辆的移动速度，a表示当前时刻所述车辆的加速度，w表示所述车辆的随机加速度，t0表示预设采样周期；
基于所述位置状态方程和所述位置观测方程构建所述车辆的状态空间模型，包括：
基于所述位置状态方程和所述位置观测方程构建的所述车辆的状态空间模型如下：
S(k)＝A′S(k-1)+Ba+Cw
z(k)＝H′s(k)+v
其中，
Figure FDA0002575547840000041
H′＝[1 0]，S(k)表示相对于所述车辆的初始位置当前时刻所述车辆的移动距离，S(k-1)表示相对于所述车辆的初始位置上一时刻所述车辆的移动距离，z(k)表示当前时刻所述车辆的GPS的观测移动距离，v表示所述车辆中的GPS的系统扰动，s(k)表示当前时刻无GPS的系统扰动时所述车辆的真实移动距离；
将所述状态空间模型扩展到四维空间，以确定卡尔曼滤波算法中的目标矩阵，包括：
根据如下公式将所述状态空间模型扩展到四维空间：
Figure FDA0002575547840000042
Figure FDA0002575547840000043
其中，X(k)表示对S(k)矢量分解后的状态量，x(k)表示相对于所述车辆的初始位置当前时刻所述车辆在水平方向的移动距离，y(k)表示相对于所述车辆的初始位置当前时刻所述车辆在竖直方向的移动距离，x(k-1)表示相对于所述车辆的初始位置上一时刻所述车辆在水平方向的移动距离，y(k-1)表示相对于所述车辆的初始位置当前时刻所述车辆在竖直方向的移动距离，
Figure FDA0002575547840000051
表示当前时刻所述车辆在水平方向的移动速度，
Figure FDA0002575547840000052
表示当前时刻所述车辆在竖直方向的移动速度，
Figure FDA0002575547840000053
表示上一时刻所述车辆在水平方向的移动速度，
Figure FDA0002575547840000054
定示上一时刻所述车辆在竖直方向的移动速度，Z(k)表示对z(k)矢量分解后的状态量，w2×1表示将当前时刻所述车辆的加速度a和随机加速度w进行归一化处理后生成的合成加速度，v2×1表示所述车辆中的GPS的系统扰动v扩展到二维空间的系统扰动，
Figure FDA0002575547840000055
A表示第一状态转移矩阵，Γ表示第二状态转移矩阵，H表示观测矩阵。
7.根据权利要求6所述的方法，其特征在于，基于当前时刻所述车辆的移动距离、当前时刻所述车辆的观测位置以及所述目标矩阵，采用卡尔曼滤波算法，获取当前时刻所述车辆的最优位置，包括：
根据如下公式确定当前时刻所述车辆的最优位置，包括：
Figure FDA0002575547840000056
其中，Xpre(k)表示当前时刻所述车辆的位置预测值，Xkf(k-1)表示上一时刻所述车辆的预估值，Ppre(k)表示当前时刻协方差预测值，Q表示过程噪声，Kg表示卡尔曼增益，R表示观测噪声，In表示单位矩阵，Xkf(k)表示当前时刻所述车辆的最优位置，P(k-1)表示上一时刻协方差预估值，Pk表示当前时刻协方差预估值。
8.一种车辆位姿的估算装置，其特征在于，包括：
获取模块，用于通过脉冲传感器获取在预设采样周期内车辆后轮的移动距离；
计算模块，用于根据所述车辆后轮的移动距离，计算在所述预设采样周期内所述车辆的航向角变化量及所述车辆的移动距离；
确定模块，用于根据所述预设采样周期内所述车辆的移动距离及所述车辆的航向角变化量，对上一时刻所述车辆的移动距离和航向角进行迭代计算，确定当前时刻所述车辆的移动距离和航向角；
校正模块，用于基于卡尔曼滤波算法，根据当前时刻所述车辆的移动距离对当前时刻所述车辆的全球定位系统GPS的观测位置进行校正，并将校正后的观测位置及当前时刻所述车辆的航向角作为当前时刻所述车辆的位姿信息。
9.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该程序被处理器执行时实现如权利要求1-7任一所述的车辆位姿的估算方法。
10.一种车辆位姿的估算系统，其特征在于，包括存储器，处理器及存储在存储器上并可在处理器运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如权利要求1-7任一所述的车辆位姿的估算方法。
CN202010652628.5A 2020-07-08 2020-07-08 一种车辆位姿的估算方法、装置、存储介质及系统 Pending CN111751857A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010652628.5A CN111751857A (zh) 2020-07-08 2020-07-08 一种车辆位姿的估算方法、装置、存储介质及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010652628.5A CN111751857A (zh) 2020-07-08 2020-07-08 一种车辆位姿的估算方法、装置、存储介质及系统

Publications (1)

Publication Number Publication Date
CN111751857A true CN111751857A (zh) 2020-10-09

Family

ID=72709985

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010652628.5A Pending CN111751857A (zh) 2020-07-08 2020-07-08 一种车辆位姿的估算方法、装置、存储介质及系统

Country Status (1)

Country Link
CN (1) CN111751857A (zh)

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112433531A (zh) * 2021-01-27 2021-03-02 国汽智控(北京)科技有限公司 一种自动驾驶车辆的轨迹跟踪方法、装置及计算机设备
CN112558125A (zh) * 2021-02-22 2021-03-26 腾讯科技（深圳）有限公司 一种车辆定位的方法、相关装置、设备以及存储介质
CN113031660A (zh) * 2021-04-02 2021-06-25 中北大学 一种飞行器定向天线跟踪定位装置
CN114322978A (zh) * 2020-10-10 2022-04-12 广州汽车集团股份有限公司 一种车辆定位方法、计算机设备、计算机可读存储介质
WO2023071442A1 (zh) * 2021-10-29 2023-05-04 华为技术有限公司 一种数据处理方法和装置

Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102077259A (zh) * 2009-02-27 2011-05-25 丰田自动车株式会社 车辆相对位置推定装置及车辆相对位置推定方法
CN102928816A (zh) * 2012-11-07 2013-02-13 东南大学 一种面向隧道环境的车辆高可靠组合定位方法
CN104157167A (zh) * 2014-08-28 2014-11-19 银江股份有限公司 一种基于协同相对定位技术的车辆防碰撞方法
CN106123908A (zh) * 2016-09-08 2016-11-16 北京京东尚科信息技术有限公司 车辆导航方法和系统
CN109631886A (zh) * 2018-12-28 2019-04-16 江苏满运软件科技有限公司 车辆定位方法、装置、电子设备、存储介质
CN110296709A (zh) * 2019-07-23 2019-10-01 南京邮电大学 基于自适应里程计模型的车载定位导航方法
CN110435623A (zh) * 2019-08-28 2019-11-12 吉林大学 一种自动调整的电动车分级自动紧急制动控制系统
CN110764519A (zh) * 2018-07-26 2020-02-07 深圳市白麓嵩天科技有限责任公司 一种基于cs模型的无人机对地目标自适应跟踪方法
CN110780326A (zh) * 2019-09-26 2020-02-11 上海瀚所信息技术有限公司 一种车载组合导航系统和定位方法
CN110834642A (zh) * 2019-11-04 2020-02-25 中国第一汽车股份有限公司 车辆跑偏识别方法、装置、车辆及存储介质

Patent Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102077259A (zh) * 2009-02-27 2011-05-25 丰田自动车株式会社 车辆相对位置推定装置及车辆相对位置推定方法
CN102928816A (zh) * 2012-11-07 2013-02-13 东南大学 一种面向隧道环境的车辆高可靠组合定位方法
CN104157167A (zh) * 2014-08-28 2014-11-19 银江股份有限公司 一种基于协同相对定位技术的车辆防碰撞方法
CN106123908A (zh) * 2016-09-08 2016-11-16 北京京东尚科信息技术有限公司 车辆导航方法和系统
CN110764519A (zh) * 2018-07-26 2020-02-07 深圳市白麓嵩天科技有限责任公司 一种基于cs模型的无人机对地目标自适应跟踪方法
CN109631886A (zh) * 2018-12-28 2019-04-16 江苏满运软件科技有限公司 车辆定位方法、装置、电子设备、存储介质
CN110296709A (zh) * 2019-07-23 2019-10-01 南京邮电大学 基于自适应里程计模型的车载定位导航方法
CN110435623A (zh) * 2019-08-28 2019-11-12 吉林大学 一种自动调整的电动车分级自动紧急制动控制系统
CN110780326A (zh) * 2019-09-26 2020-02-11 上海瀚所信息技术有限公司 一种车载组合导航系统和定位方法
CN110834642A (zh) * 2019-11-04 2020-02-25 中国第一汽车股份有限公司 车辆跑偏识别方法、装置、车辆及存储介质

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114322978A (zh) * 2020-10-10 2022-04-12 广州汽车集团股份有限公司 一种车辆定位方法、计算机设备、计算机可读存储介质
CN114322978B (zh) * 2020-10-10 2024-03-15 广州汽车集团股份有限公司 一种车辆定位方法、计算机设备、计算机可读存储介质
CN112433531A (zh) * 2021-01-27 2021-03-02 国汽智控(北京)科技有限公司 一种自动驾驶车辆的轨迹跟踪方法、装置及计算机设备
CN112558125A (zh) * 2021-02-22 2021-03-26 腾讯科技（深圳）有限公司 一种车辆定位的方法、相关装置、设备以及存储介质
CN112558125B (zh) * 2021-02-22 2021-05-25 腾讯科技（深圳）有限公司 一种车辆定位的方法、相关装置、设备以及存储介质
CN113031660A (zh) * 2021-04-02 2021-06-25 中北大学 一种飞行器定向天线跟踪定位装置
WO2023071442A1 (zh) * 2021-10-29 2023-05-04 华为技术有限公司 一种数据处理方法和装置

Similar Documents

Publication Publication Date Title
CN111751857A (zh) 一种车辆位姿的估算方法、装置、存储介质及系统
US10634777B2 (en) Radar odometry for vehicle
CN103270543B (zh) 驾驶辅助装置
US9482536B2 (en) Pose estimation
CN101846734A (zh) 农用机械导航定位方法、系统及农用机械工控机
EP4345421A2 (en) Method for calibrating sensor parameters based on autonomous driving, apparatus, storage medium, and vehicle
JP7272910B2 (ja) 車載装置、状態推定方法及びプログラム
CN112433531A (zh) 一种自动驾驶车辆的轨迹跟踪方法、装置及计算机设备
CN111189454A (zh) 基于秩卡尔曼滤波的无人车slam导航方法
CN115060257B (zh) 一种基于民用级惯性测量单元的车辆变道检测方法
CN110637209B (zh) 估计机动车的姿势的方法、设备和具有指令的计算机可读存储介质
CN112326990A (zh) 一种作业车辆测速方法及系统
JP2022023388A (ja) 車両位置決定装置
JP2014240266A (ja) センサドリフト量推定装置及びプログラム
Guan et al. Multi-sensor fusion vehicle positioning based on Kalman Filter
JP7206883B2 (ja) ヨーレート補正装置
CN114019954B (zh) 航向安装角标定方法、装置、计算机设备和存储介质
CN112611377B (zh) 一种用于小车室外导航的状态预测方法、装置及存储介质
CN113306573B (zh) 一种自动驾驶车辆学习型路径跟踪预测控制方法
JP7028223B2 (ja) 自己位置推定装置
CN117015719A (zh) 用于确定刚体的运动状态的方法
CN113030504B (zh) 车辆测速方法、装置、车载计算机设备和存储介质
CN113048987A (zh) 一种车载导航系统定位方法
JP4831441B2 (ja) 方位センサの補正係数演算装置及び演算プログラム
TWI422825B (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
RJ01 Rejection of invention patent application after publication
RJ01 Rejection of invention patent application after publication

Application publication date: 20201009