[go: up one dir, main page]
More Web Proxy on the site http://driver.im/

CN111458733B - 一种结合gps定位与车身信息的位姿校正方法及装置 - Google Patents

一种结合gps定位与车身信息的位姿校正方法及装置 Download PDF

Info

Publication number
CN111458733B
CN111458733B CN202010073141.1A CN202010073141A CN111458733B CN 111458733 B CN111458733 B CN 111458733B CN 202010073141 A CN202010073141 A CN 202010073141A CN 111458733 B CN111458733 B CN 111458733B
Authority
CN
China
Prior art keywords
vector
time
value
gps positioning
state
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
CN202010073141.1A
Other languages
English (en)
Other versions
CN111458733A (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.)
Wuhan Kotei Technology Corp
Original Assignee
Wuhan Kotei Technology 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 Wuhan Kotei Technology Corp filed Critical Wuhan Kotei Technology Corp
Priority to CN202010073141.1A priority Critical patent/CN111458733B/zh
Publication of CN111458733A publication Critical patent/CN111458733A/zh
Application granted granted Critical
Publication of CN111458733B publication Critical patent/CN111458733B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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/40—Correcting position, velocity or attitude
    • 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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/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/53—Determining attitude

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Automation & Control Theory (AREA)
  • Navigation (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

本发明公开一种结合GPS定位与车身信息的位姿校正方法及装置，所述方法包括：建立扩展卡尔曼滤波模型；实时获取GPS定位信息和车身信息，并通过获取的GPS定位信息和车身信息设置当前时刻的观测向量和控制向量的值；对扩展卡尔曼滤波模型进行初始化；使用扩展卡尔曼滤波模型进行迭代运算，根据上一时刻的状态向量的最优估计值和当前时刻控制向量的值预测当前时刻的状态向量的最优估计值；根据预测得到的当前时刻状态向量的最优估计值和当前时刻观测向量的值，更新当前时刻的状态向量的最优估计值；输出更新后的状态向量的最优估计值，得到当前时刻校正后的车辆位姿。本发明能够显著降低GPS信号遮挡时车辆定位的误差，保证自动驾驶系统的正常运行。

Description

一种结合GPS定位与车身信息的位姿校正方法及装置
技术领域
本发明属于自动驾驶技术领域，具体涉及一种结合GPS定位与车身信息的位姿校正方法及装置。
背景技术
自动驾驶系统在运行过程中，需要实时地获取车辆的位姿信息，位姿信息包含经度、纬度、速度、航向角等。
自动驾驶系统根据车辆的位姿信息，以及事先录制或者规划好的一次规划路径，生成车辆的期望方向盘转角、期望油门量和期望刹车量。车辆位姿在自动驾驶过程中起着重要的作用。
车辆位姿的一个主要的获取途径是从GPS定位装置获取，如星网宇达、迈普时空等厂商生产的组合惯导。但在遇到道路两旁存在树叶遮挡等情况时，GPS定位装置的GPS信号会被部分遮挡，造成从GPS定位装置获取的车辆位姿不准，进而造成自动驾驶车辆压线甚至行驶到车道外。
发明内容
本发明结合GPS定位与车身信息，对车辆位姿进行校正，解决GPS信号遮挡时车辆定位误差较大的问题。
本发明第一方面，提出一种结合GPS定位与车身信息的位姿校正方法，所述方法包括：
S1，以车辆位置和航向角作为状态向量，以车速和横摆角速度作为控制向量，以从GPS定位装置获取的位置和航向角作为观测向量，建立扩展卡尔曼滤波模型；
S2，实时获取GPS定位信息和车身信息，并通过获取的GPS定位信息和车身信息设置当前时刻的观测向量和控制向量的值；
S3，对扩展卡尔曼滤波模型进行初始化，得到初始时刻状态向量的最优估计值对应的协方差；
S4，使用扩展卡尔曼滤波模型进行迭代运算，根据上一时刻的状态向量的最优估计值和当前时刻控制向量的值预测当前时刻的状态向量的最优估计值X(k|k-1)；
S5，根据预测得到的当前时刻状态向量的最优估计值和当前时刻观测向量的值，更新当前时刻的状态向量的最优估计值；
S6，输出更新后的状态向量的最优估计值，得到校正后的车辆位姿。
优选的，所述步骤S1中，记状态向量为X，控制向量为U，观测向量为Z，这三个向量的具体形式如下：
Figure BDA0002377777770000021
Figure BDA0002377777770000022
Figure BDA0002377777770000023
系统的状态转移方程如下：
Xk＝g(Uk,Xk-1)+εk
系统的观测方程如下：
Zk＝h(Xk)+δk
Figure BDA0002377777770000024
其中，Xk-1表示k-1时刻的状态向量，Xk和(x y θ)k T均表示k时刻的状态向量，Uk和(vk ωk)T均表示k时刻的控制向量，Zk和(zx zy zθ)k T均表示k时刻的观测向量；εk表示k时刻时状态转移方程中的系统噪声，δk表示k时刻时观测方程中的观测噪声，g(Uk,Xk-1)为系统状态函数，h(Xk)为观测函数。
优选的，系统的状态转移方程中，系统状态函数g(Uk,Xk-1)计算公式如下：
当ωk的绝对值小于预设阈值Thresholdstraightline时，
Figure BDA0002377777770000031
当ωk的绝对值大于或等于阈值Thresholdstraightline时，
Figure BDA0002377777770000032
其中，xk-1表示k-1时刻的状态向量的横向坐标值，yk-1表示k-1时刻状态向量的纵向坐标值，θk-1表示k-1时刻状态向量的车辆行驶方向与横轴的夹角；vk表示k时刻控制向量的车速，ωk表示k时刻控制向量的横摆角速度，Δt表示k时刻与k-1时刻的时间差。
优选的，所述步骤S2具体包括：
S21、实时获取GPS定位信息，并使用获取的GPS定位信息设置当前时刻的观测向量的值；
获取的GPS定位信息包括经度、纬度、航向角、横摆角速度、经度的方差、纬度的方差和航向角的方差，将所述GPS定位信息由GPS坐标转换为UTM坐标；将转换后的基于UTM坐标系的位置和航向角的值作为当前时刻的观测向量的值；
S22，实时获取车身信息，并使用获取的车身信息中车速的值设置当前时刻的控制向量的车速的值；
从车身获取车速vchassis、方向盘转角
Figure BDA0002377777770000033
当前时刻的控制向量的车速vk从车身获取，也即：
vk＝vchassis
S23，结合从GPS定位装置获取的横摆角速度的值和从车身获取的方向盘转角计算得到的横摆角速度的值，来设置当前时刻的控制向量的横摆角速度的值；
将从GPS定位装置获取的横摆角速度记作ωimu，将由方向盘转角计算得到的横摆角速度记作ωchassis，将当前时刻的控制向量的横摆角速度记作ωk。
根据车辆的方向盘转角计算横摆角速度的公式如下：
Figure BDA0002377777770000041
上式中，vchassis表示从车身获取的车速，tan表示正切函数，
Figure BDA0002377777770000042
表示方向盘转角，i表示转向传动比，L表示轴距；
当车速vchassis小于预设车速阈值Thresholdvelocity时，当前时刻的控制向量的横摆角速度取从车辆的方向盘转角计算得到的横摆角速度ωchassis，也即：
ωk＝ωchassis
当车速vchassis大于或等于预设车速阈值Thresholdvelocity时，当前时刻的控制向量的横摆角速度取从GPS定位装置获取的横摆角速度ωimu和根据车辆的方向盘转角计算得到的横摆角速度ωchassis的均值，也即：
Figure BDA0002377777770000043
优选的，所述步骤S3中，设当前时刻为k，当k＝0时为初始时刻，将当前时刻的观测向量Z0的值赋值给状态向量的最优估计值X(0|0)，使用以下公式计算协方差P(0|0)的值：
Figure BDA0002377777770000044
其中，σx 2和σy 2为从GPS定位装置获取的经度的方差、纬度的方差的数值转换到UTM坐标系下的数值，
Figure BDA0002377777770000047
为从GPS定位装置获取的航向角的方差。
优选的，所述步骤S4具体为：
根据k-1时刻的状态向量的最优估计值X(k-1|k-1)和k时刻的控制向量的值Uk预测当前时刻的状态向量的最优估计值X(k|k-1)，预测当前时刻的状态向量的最优估计值的数学公式如下：
X(k|k-1)＝g(Uk，X(k-1|k-1))
Figure BDA0002377777770000051
过程噪声的协方差Rk使用下式进行计算：
Figure BDA0002377777770000052
Gk为k时刻系统状态函数g对状态向量X的偏导数，计算公式如下：
Figure BDA0002377777770000053
其中，k为正整数，X(k-1|k-1)是k-1时刻状态向量的最优估计值，Uk为k时刻控制向量；P(k|k-1)是X(k|k-1)对应的协方差，P(k-1|k-1)是X(k-1|k-1)对应的协方差；w1，w2，w3，w4为固定的系数，vk表示控制向量的车速，ωk表示控制向量的横摆角速度。
优选的，所述步骤S5具体为：
根据预测得到的当前时刻状态向量的最优估计值X(k|k-1)和观测向量Zk的值，更新当前时刻的状态向量的最优估计值X(k|k)，更新当前时刻状态向量的最优估计值的数学公式如下：
Figure BDA0002377777770000054
X(k|k)＝X(k|k-1)+Kg(k)(Zk-h(X(k|k-1)))
P(k|k)＝(1-Kg(k)Hk)X(k|k)
观测噪声的协方差Qk使用下式进行计算：
Figure BDA0002377777770000055
Hk为k时刻观测函数h对状态向量X的偏导数，计算公式如下：
Figure BDA0002377777770000061
Kg为卡尔曼增益。
本发明第二方面，提供一种结合GPS定位与车身信息的位姿校正装置，所述装置包括：
模型构建模块：用于以车辆位置和航向角作为状态向量，以车速和横摆角速度作为控制向量，以从GPS定位装置获取的位置和航向角作为观测向量，建立扩展卡尔曼滤波模型；
参量设置模块：用于实时获取GPS定位信息和车身信息，并使用获取的GPS定位信息和车身信息设置当前时刻的观测向量和控制向量的值；
位姿校正模块：用于初始化扩展卡尔曼滤波模型并使用扩展卡尔曼滤波模型进行迭代运算，根据上一时刻的状态向量的最优估计值和当前时刻控制向量的值预测当前时刻的状态向量的最优估计值；根据预测得到的当前时刻状态向量的最优估计值和当前时刻观测向量的值，更新当前时刻的状态向量的最优估计值；
位姿输出模块：输出更新后的的状态向量的最优估计值，得到校正后的车辆位姿。
优选的，所述模型构建模块中，记状态向量为X，控制向量为U，观测向量为Z，
系统的状态转移方程如下：
Xk＝g(Uk,Xk-1)+εk
系统的观测方程如下：
Zk＝h(Xk)+δk
Figure BDA0002377777770000062
其中，Xk-1表示k-1时刻的状态向量，Xk和(x y θ)k T均表示k时刻的状态向量，Uk和(vk ωk)T均表示k时刻的控制向量，Zk和(zx zy zθ)k T均表示k时刻的观测向量；εk表示k时刻时状态转移方程中的系统噪声，δk表示k时刻时观测方程中的观测噪声，g(Uk,Xk-1)为系统状态函数，h(Xk)为观测函数；
系统状态函数g(Uk,Xk-1)计算公式如下：
当ωk的绝对值小于预设阈值Thresholdstraightline时，
Figure BDA0002377777770000071
当ωk的绝对值大于或等于阈值Thresholdstraightline时，
Figure BDA0002377777770000072
其中，xk-1表示k-1时刻的状态向量的横向坐标值，yk-1表示k-1时刻状态向量的纵向坐标值，θk-1表示k-1时刻状态向量的车辆行驶方向与横轴的夹角；vk表示k时刻控制向量的车速，ωk表示k时刻控制向量的横摆角速度，Δt表示k时刻与k-1时刻的时间差。
优选的，所述参量设置模块具体包括：
观测向量设置单元：获取GPS定位信息，并使用获取的GPS定位信息设置当前时刻的观测向量的值；
获取的GPS定位信息包括经度、纬度、航向角、横摆角速度、经度的方差、纬度的方差和航向角的方差，将所述GPS定位信息由GPS坐标转换为UTM坐标；将转换后的基于UTM坐标系的位置和航向角的值作为当前时刻的观测向量的值；
控制向量设置单元：获取车身信息，并使用获取的车身信息中车速的值设置当前时刻的控制向量的车速的值；
从车身获取车速vchassis、方向盘转角
Figure BDA0002377777770000073
当前时刻的控制向量的车速vk从车身获取，也即：
vk＝vchassis
结合从GPS定位装置获取的横摆角速度的值和从车身获取的方向盘转角计算得到的横摆角速度的值，来设置当前时刻的控制向量的横摆角速度的值；
将从GPS定位装置获取的横摆角速度记作ωimu，将由方向盘转角计算得到的横摆角速度记作ωchassis，将当前时刻的控制向量的横摆角速度记作ωk。
根据车辆的方向盘转角计算横摆角速度的公式如下：
Figure BDA0002377777770000081
上式中，vchassis表示从车身获取的车速，tan表示正切函数，
Figure BDA0002377777770000082
表示方向盘转角，i表示转向传动比，L表示轴距；
当车速小于预设车速阈值Thresholdvelocity时，当前时刻的控制向量的横摆角速度取从车辆的方向盘转角计算得到的横摆角速度ωchassis，也即：
ωk＝ωchassis
当车速大于或等于预设车速阈值Thresholdvelocity时，当前时刻的控制向量的横摆角速度取从GPS定位装置获取的横摆角速度ωimu和根据车辆的方向盘转角计算得到的横摆角速度的均值，也即：
Figure BDA0002377777770000083
优选的，所述位姿校正模块具体包括：
状态预测单元：用于预测当前时刻的状态向量的最优估计值X(k|k-1)，预测当前时刻的状态向量的最优估计值的数学公式如下：
X(k|k-1)＝g(Uk，X(k-1|k-1))
Figure BDA0002377777770000084
过程噪声的协方差Rk使用下式进行计算：
Figure BDA0002377777770000085
Gk为k时刻系统状态函数g对状态向量X的偏导数，计算公式如下：
Figure BDA0002377777770000091
其中，X(k-1|k-1)是k-1时刻状态向量的最优估计值，X(k|k)是k时刻状态向量的最优估计值，Uk为k时刻控制向量；P(k|k-1)是X(k|k-1)对应的协方差，P(k-1|k-1)是X(k-1|k-1)对应的协方差；w1，w2，w3，w4为固定的系数，vk表示控制向量的车速，ωk表示控制向量的横摆角速度；
状态更新单元：用于更新状态向量的最优估计值X(k|k)，更新当前时刻状态向量的最优估计值的数学公式如下：
Figure BDA0002377777770000092
X(k|k)＝X(k|k-1)+Kg(k)(Zk-h(X(k|k-1)))
P(k|k)＝(1-Kg(k)Hk)X(k|k)
观测噪声的协方差Qk使用下式进行计算：
Figure BDA0002377777770000093
Hk为k时刻观测函数h对状态向量X的偏导数，计算公式如下：
Figure BDA0002377777770000094
Kg(k)为卡尔曼增益。
本发明的有益效果是：
本发明的位姿矫正方法能够显著降低车辆定位的误差，进而可以保证自动驾驶系统的正常运行。经过实测，在GPS信号被连续遮挡20秒时，普通的定位方法获得的车辆的位置误差超过1米，在应用本发明中的位姿校正方法之后，相同场景下车辆的位置误差不超过0.3米。
附图说明
为了更清楚地说明本发明的技术方案，下面将对本发明技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1为本发明提供的结合GPS定位与车身信息的位姿校正方法的流程示意图。
具体实施方式
本发明利用从车身获取的车速信息和方向盘转角信息，来对从GPS定位装置获取的GPS定位信息进行校正，使得在遇到GPS信号遮挡的地方，得到的车辆的GPS位置更加准确。校正的方法是根据从车身获取的车速、方向盘转角，以及车身参数轴距和转向传动比，来计算得到当前时刻车辆的横摆角速度。再假设车辆为刚性物体，在间隔很小的时间段内，车速和横摆角速度维持不变，车辆沿着直线或者圆弧行驶，只考虑车辆在水平方向的移动，建立车辆动力学模型。然后应用扩展卡尔曼滤波来对车辆当前时刻的位姿进行预测和更新，达到校正的目的。
为使得本发明的发明目的、特征、优点能够更加的明显和易懂，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，下面所描述的实施例仅仅是本发明一部分实施例，而非全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。
请参阅图1，本发明提出一种结合GPS定位与车身信息的位姿校正方法，所述方法包括：
S1，以车辆位置和航向角作为状态向量，以车速和横摆角速度作为控制向量，以从GPS定位装置获取的位置和航向角作为观测向量，建立扩展卡尔曼滤波模型；
具体的，本发明使用的状态向量为车辆的横向坐标x，纵向坐标y，车辆行驶方向与横轴的夹角θ。控制向量为车速v，横摆角速度ω。观测向量为横向坐标zx，纵向坐标zy，车辆行驶方向与横轴的夹角zθ。基于的假设是车速v、横摆角速度ω在间隔很小的时间段Δt(实际项目中，取值为40毫秒)内维持不变。这样得到的动力系统为一个非线性系统，状态转移方程为非线性方程。本发明在实际实施时，取的坐标系为UTM坐标系，也即横轴x为东向，纵轴y为北向，坐标原点位于本初子午线与赤道交点，单位为米的坐标系。
记状态向量为X，控制向量为U，观测向量为Z。这三个向量的具体形式如下：
Figure BDA0002377777770000111
Figure BDA0002377777770000112
Figure BDA0002377777770000113
系统的状态转移方程如下：
Xk＝g(Uk,Xk-1)+εk
系统的观测方程如下：
Zk＝h(Xk)+δk
Figure BDA0002377777770000114
其中，k表示当前时刻，当k>0时，k-1表示上一时刻。Xk-1表示k-1时刻的状态向量，Xk和(x y θ)k T均表示k时刻的状态向量，Uk和(vk ωk)T均表示k时刻的控制向量，Zk和(zx zyzθ)k T均表示k时刻的观测向量；εk表示k时刻时状态转移方程中的系统噪声，并假设其服从均值为(0 0 0)T，协方差为Rk的正态分布；δk表示k时刻时观测方程中的观测噪声，并假设其服从均值为(0 0 0)T，协方差为Qk的正态分布。sin表示正弦函数，cos表示余弦函数。g(Uk,Xk-1)为系统状态函数，h(Xk)为观测函数。
当ωk的绝对值小于预设阈值Thresholdstraightline时，系统的状态转移方程中的函数g为：
Figure BDA0002377777770000121
当ωk的绝对值大于或等于预设阈值Thresholdstraightline时，系统的状态转移方程中的函数g为：
Figure BDA0002377777770000122
其中，xk-1表示k-1时刻的状态向量的横向坐标值，yk-1表示k-1时刻状态向量的纵向坐标值，θk-1表示k-1时刻状态向量的车辆行驶方向与横轴的夹角；vk表示k时刻控制向量的车速，ωk表示k时刻控制向量的横摆角速度，Δt表示k时刻与k-1时刻的时间差。
具体实施时，可取Thresholdstraightline＝0.0001弧度/秒。
S2，实时获取GPS定位信息和车身信息，并通过获取的GPS定位信息和车身信息设置当前时刻的观测向量Zk和控制向量Uk的值；
进一步地，所述步骤S2具体包括：
S21，实时获取GPS定位信息，并使用获取的GPS定位信息设置当前时刻的观测向量Zk的值；
具体的，获取的GPS定位信息包括经度、纬度、航向角、横摆角速度、经度的方差、纬度的方差和航向角的方差，将从GPS定位装置获取的位置坐标由GPS坐标转换为UTM坐标，航向角转换为与UTM坐标中x轴方向(东向)的夹角。将转换获取后的基于UTM坐标系的位置和航向角的值作为当前时刻的观测向量的值。
将从GPS定位装置获取的横摆角速度记作ωimu。将从GPS定位装置获取的经度的方差、纬度的方差的数值转换为UTM坐标系下的数值ζx 2和ζy 2，将从GPS定位装置获取的航向角的方差记作
Figure BDA0002377777770000131
S22，实时获取车身信息，并使用获取的车身信息中车速的值设置当前时刻的控制向量Uk的车速的值；
从车身获取车速vchassis、方向盘转角
Figure BDA0002377777770000132
当前时刻的控制向量的车速vk从车身获取，也即：
vk＝vchassis
S23，结合从GPS定位装置获取的横摆角速度的值和从车身获取的方向盘转角计算得到的横摆角速度的值，来设置当前时刻的控制向量Uk的横摆角速度的值；
将从GPS定位装置获取的横摆角速度记作ωimu，将由方向盘转角计算得到的横摆角速度记作ωchassis，将当前时刻的控制向量的横摆角速度记作ωk。
根据车辆的方向盘转角计算横摆角速度的公式如下：
Figure BDA0002377777770000133
上式中，vchassis表示从车身获取的车速，tan表示正切函数，
Figure BDA0002377777770000134
表示方向盘转角，i表示转向传动比，L表示车辆轴距；
当车速vchassis小于预设车速阈值Thresholdvelocity时，当前时刻的控制向量的横摆角速度取从车辆的方向盘转角计算得到的横摆角速度ωchassis，也即：
ωk＝ωchassis
当车速vchassis大于或等于预设车速阈值Thresholdvelocity时，当前时刻的控制向量的横摆角速度取从GPS定位装置获取的横摆角速度ωimu和根据车辆的方向盘转角计算得到的横摆角速度ωchassis的均值，也即：
Figure BDA0002377777770000135
具体实施时，可取Thresholdvelocity＝1.389米/秒。
S3，对扩展卡尔曼滤波模型进行初始化，得到初始时刻状态向量的最优估计值及对应的协方差；
设当前时刻为k，当k＝0时为初始时刻，将当前时刻的观测向量Z0的值赋值给状态向量的最优估计值X(0|0)，使用以下公式计算协方差P(0|0)的值：
Figure BDA0002377777770000141
转至步骤S6，保存并直接输出X(0|0)作为当前时刻状态向量的最优估计值。当k>0时，转至步骤S4。
S4，使用扩展卡尔曼滤波模型进行迭代运算，根据上一时刻的状态向量的最优估计和当前时刻控制向量的值预测当前时刻的状态向量的最优估计值；
当k为正整数时，根据k-1时刻的状态向量的最优估计值X(k-1|k-1)和k时刻的控制向量的值Uk预测当前时刻的状态向量的最优估计值X(k|k-1)，预测当前时刻的状态向量的最优估计值的数学公式如下：
X(k|k-1)＝g(Uk，X(k-1|k-1))
Figure BDA0002377777770000142
过程噪声的协方差Rk使用下式进行计算：
Figure BDA0002377777770000143
Gk为k时刻系统状态函数g对状态向量X的偏导数，计算公式如下：
Figure BDA0002377777770000144
其中，X(k-1|k-1)是k-1时刻状态向量的最优估计值，Uk为k时刻控制向量；P(k|k-1)是X(k|k-1)对应的协方差，P(k-1|k-1)是X(k-1|k-1)对应的协方差；vk表示控制向量的车速，ωk表示控制向量的横摆角速度，右上角的T表示转置。w1，w2，w3，w4为固定的系数，具体实施时，可取w1＝0.005，w2＝0.05，w3＝0.005，w4＝0.05。
S5，根据预测得到的当前时刻状态向量的最优估计值和当前时刻观测向量的值，更新当前时刻的状态向量的最优估计值；
根据预测得到的当前时刻状态向量的最优估计值X(k|k-1)和k时刻观测向量Zk的值，更新k时刻的状态向量的最优估计值X(k|k)，更新当前时刻状态向量的最优估计值的数学公式如下：
Figure BDA0002377777770000151
X(k|k)＝X(k|k-1)+Kg(k)(Zk-h(X(k|k-1)))
P(k|k)＝(1-Kg(k)Hk)X(k|k)
观测噪声的协方差Qk使用下式进行计算：
Figure BDA0002377777770000152
Hk为k时刻观测函数h对状态向量X的偏导数，计算公式如下：
Figure BDA0002377777770000153
Kg(k)为卡尔曼增益。
S6，输出更新后的状态向量的最优估计值，得到校正后的车辆位姿。
将当前时刻的状态向量的最优估计值X(k|k)进行保存并输出，也即将得到的当前时刻的状态向量的最优估计值X(k|k)作为校正后的位姿输出给自动驾驶系统其他模块使用。
等待下一时刻，将k的值加1，并跳转到步骤S2进行下一周期的位姿校正。
与所述方法实施例相对应，本发明还提供一种结合GPS定位与车身信息的位姿校正装置，所述装置包括：
模型构建模块：用于以车辆位置和航向角作为状态向量，以车速和横摆角速度作为控制向量，以从GPS定位装置获取的位置和航向角作为观测向量，建立扩展卡尔曼滤波模型；
参量设置模块：用于实时获取GPS定位信息和车身信息，并使用获取的GPS定位信息和车身信息设置当前时刻的观测向量和控制向量的值；
位姿校正模块：用于初始化扩展卡尔曼滤波模型并使用扩展卡尔曼滤波模型进行迭代运算，根据上一时刻的状态向量的最优估计值和当前时刻控制向量的值预测当前时刻的状态向量的最优估计值；根据预测得到的当前时刻状态向量的最优估计值和当前时刻观测向量的值，更新当前时刻的状态向量的最优估计值；
位姿输出模块：输出更新后的的状态向量的最优估计值，得到校正后的车辆位姿。
优选的，所述模型构建模块中，记状态向量为X，控制向量为U，观测向量为Z，
系统的状态转移方程如下：
Xk＝g(Uk,Xk-1)+εk
系统的观测方程如下：
Zk＝h(Xk)+δk
Figure BDA0002377777770000161
其中，Xk-1表示k-1时刻的状态向量，Xk和(x y θ)k T均表示k时刻的状态向量，Uk和(vk ωk)T均表示k时刻的控制向量，Zk和(zx zy zθ)k T均表示k时刻的观测向量；εk表示k时刻时状态转移方程中的系统噪声，δk表示k时刻时观测方程中的观测噪声，g(Xk-1,Uk)为系统状态函数，h(Xk)为观测函数；
系统状态函数g(Xk-1,Uk)计算公式如下：
当ωk的绝对值小于预设阈值Thresholdstraightline时，
Figure BDA0002377777770000162
当ωk的绝对值大于或等于阈值Thresholdstraightline时，
Figure BDA0002377777770000171
其中，xk-1表示k-1时刻的状态向量的横向坐标值，yk-1表示k-1时刻状态向量的纵向坐标值，θk-1表示k-1时刻状态向量的车辆行驶方向与横轴的夹角；vk表示k时刻控制向量的车速，ωk表示k时刻控制向量的横摆角速度，Δt表示k时刻与k-1时刻的时间差。
优选的，所述参量设置模块具体包括：
观测向量设置单元：获取GPS定位信息，并使用获取的GPS定位信息设置当前时刻的观测向量的值；
获取的GPS定位信息包括经度、纬度、航向角、横摆角速度、经度的方差、纬度的方差和航向角的方差，将所述GPS定位信息由GPS坐标转换为UTM坐标；将转换后的基于UTM坐标系的位置和航向角的值作为当前时刻的观测向量的值；
控制向量设置单元：获取车身信息，并使用获取的车身信息中车速的值设置当前时刻的控制向量的车速的值；
从车身获取车速vchassis、方向盘转角
Figure BDA0002377777770000172
当前时刻的控制向量的车速vk从车身获取，也即：
vk＝vchassis
结合从GPS定位装置获取的横摆角速度的值和从车身获取的方向盘转角计算得到的横摆角速度的值，来设置当前时刻的控制向量的横摆角速度的值；
将从GPS定位装置获取的横摆角速度记作ωimu，将由方向盘转角计算得到的横摆角速度记作ωchassis，将当前时刻的控制向量的横摆角速度记作ωk。
根据车辆的方向盘转角计算横摆角速度的公式如下：
Figure BDA0002377777770000181
上式中，vchassis表示从车身获取的车速，tan表示正切函数，
Figure BDA0002377777770000182
表示方向盘转角，i表示转向传动比，L表示轴距；
当车速小于Thresholdvelocity时，当前时刻的控制向量的横摆角速度取从车辆的方向盘转角计算得到的横摆角速度ωchassis，也即：
ωk＝ωchassis
当车速大于或等于Thresholdvelocity时，当前时刻的控制向量的横摆角速度取从GPS定位装置获取的横摆角速度ωimu和根据车辆的方向盘转角计算得到的横摆角速度的均值，也即：
Figure BDA0002377777770000183
优选的，所述位姿校正模块具体包括：
状态预测单元：用于预测当前时刻的状态向量的最优估计值X(k|k-1)，预测当前时刻的状态向量的最优估计值的数学公式如下：
X(k|k-1)＝g(Uk，X(k-1|k-1))
Figure BDA0002377777770000184
过程噪声的协方差Rk使用下式进行计算：
Figure BDA0002377777770000185
Gk为k时刻系统状态函数g对状态向量X的偏导数，计算公式如下：
Figure BDA0002377777770000186
其中，X(k-1|k-1)是k-1时刻状态向量的最优估计值，X(k|k)是k时刻状态向量的最优估计值，Uk为k时刻控制向量；P(k|k-1)是X(k|k-1)对应的协方差，P(k-1|k-1)是X(k-1|k-1)对应的协方差；w1，w2，w3，w4为固定的系数，vk表示控制向量的车速，ωk表示控制向量的横摆角速度；
状态更新单元：用于更新状态向量的最优估计值X(k|k)，更新当前时刻状态向量的最优估计值的数学公式如下：
Figure BDA0002377777770000191
X(k|k)＝X(k|k-1)+Kg(k)(Zk-h(X(k|k-1)))
P(k|k)＝(1-Kg(k)Hk)X(k|k)
观测噪声的协方差Qk使用下式进行计算：
Figure BDA0002377777770000192
Hk为k时刻观测函数h对状态向量X的偏导数，计算公式如下：
Figure BDA0002377777770000193
Kg(k)为卡尔曼增益。
本发明结合GPS定位与车身信息，通过扩展卡尔曼滤波模型来对车辆当前时刻的位姿进行预测和更新，达到校正的目的。所述扩展卡尔曼滤波模型以位置和航向角作为状态向量，以车速和横摆角速度作为控制向量，以位置和航向角作为观测向量。以从车身获取的车速作为控制向量的车速。当车速较低时，以由车身方向盘转角计算而来的横摆角速度作为控制向量的横摆角速度；当车速较高时，以从GPS定位装置获取的横摆角速度和由车身方向盘转角计算而来的横摆角速度的均值作为控制向量的横摆角速度。以从GPS定位装置获取的位置和航向角作为观测向量。
由于从车身获取的车速和方向盘信息是不受GPS信号遮挡影响的，这样也意味着输入的信息更加准确，结合从GPS定位装置获取的GPS定位信息，从而可以使得获取的GPS位置更加准确。
以上装置实施例与方法实施例是一一对应的，装置实施例简略之处，参见方法实施例即可。
本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可，说明书中未阐述的部分均为现有技术或公知常识。
专业人员还可以进一步意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、计算机软件或者二者的结合来实现，为了清楚地说明硬件和软件的可互换性，在上述说明中已经按照功能性一般性地描述了各示例的组成及步骤。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应超过本发明的范围。
结合本文中所公开的实施例描述的方法或算法的步骤可以直接用硬件、处理器执行的软件模块，或者二者的结合来实施。软件模块可以置于随机储存器、内存、只读存储器、电可编程ROM、电可擦除可编程ROM、寄存器、硬盘、可移动磁盘、CD-ROM、或技术领域内所公知的任意其他形式的存储介质中。
以上所述，以上实施例仅用以说明本发明的技术方案，而非对其限制，尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (10)

1.一种结合GPS定位与车身信息的位姿校正方法，其特征在于，所述方法包括：
S1，以车辆位置和航向角作为状态向量，以车速和横摆角速度作为控制向量，以从GPS定位装置获取的位置和航向角作为观测向量，建立扩展卡尔曼滤波模型；
S2，实时获取GPS定位信息和车身信息，并通过获取的GPS定位信息和车身信息设置当前时刻的观测向量和控制向量的值；
S3，对扩展卡尔曼滤波模型进行初始化，得到初始时刻状态向量的最优估计值及对应的协方差；
S4，使用扩展卡尔曼滤波模型进行迭代运算，根据上一时刻的状态向量的最优估计值和当前时刻控制向量的值预测当前时刻的状态向量的最优估计值；
S5，根据预测得到的当前时刻状态向量的最优估计值和当前时刻观测向量的值，更新当前时刻的状态向量的最优估计值；
S6，输出更新后的状态向量的最优估计值，得到当前时刻校正后的车辆位姿。
2.根据权利要求1所述结合GPS定位与车身信息的位姿校正方法，其特征在于，所述步骤S1中，记状态向量为X，控制向量为U，观测向量为Z，这三个向量的具体形式如下：
Figure FDA0002377777760000011
Figure FDA0002377777760000012
Figure FDA0002377777760000013
系统的状态转移方程如下：
Xk＝g(Uk,Xk-1)+εk
系统的观测方程如下：
Zk＝h(Xk)+δk
Figure FDA0002377777760000021
其中，Xk-1表示k-1时刻的状态向量，Xk和(x y θ)k T均表示k时刻的状态向量，Uk和(vkωk)T均表示k时刻的控制向量，Zk和(zx zy zθ)k T均表示k时刻的观测向量；εk表示k时刻时状态转移方程中的系统噪声，δk表示k时刻时观测方程中的观测噪声，g(Uk,Xk-1)为系统状态函数，h(Xk)为观测函数；
所述系统状态函数g(Uk,Xk-1)计算公式如下：
当ωk的绝对值小于预设阈值Thresholdstraightline时，
Figure FDA0002377777760000022
当ωk的绝对值大于或等于预设阈值Thresholdstraightline时，
Figure FDA0002377777760000023
其中，xk-1表示k-1时刻的状态向量的横向坐标值，yk-1表示k-1时刻状态向量的纵向坐标值，θk-1表示k-1时刻状态向量的车辆行驶方向与横轴的夹角；vk表示k时刻控制向量的车速，ωk表示k时刻控制向量的横摆角速度，Δt表示k时刻与k-1时刻的时间差。
3.根据权利要求2所述结合GPS定位与车身信息的位姿校正方法，其特征在于，所述步骤S2具体包括：
S21、实时获取GPS定位信息，并使用获取的GPS定位信息设置当前时刻的观测向量Zk的值；
获取的GPS定位信息包括经度、纬度、航向角、横摆角速度、经度的方差、纬度的方差和航向角的方差，将所述GPS定位信息由GPS坐标转换为UTM坐标；将转换后的基于UTM坐标系的位置和航向角的值作为当前时刻的观测向量的值；
S22，实时获取车身信息，并使用获取的车身信息中车速的值设置当前时刻的控制向量Uk的车速的值；
从车身获取车速vchassis、方向盘转角
Figure FDA0002377777760000031
当前时刻的控制向量的车速vk从车身获取，也即：
vk＝vchassis
S23，结合从GPS定位装置获取的横摆角速度的值和从车身获取的方向盘转角计算得到的横摆角速度的值，来设置当前时刻的控制向量Uk的横摆角速度的值；
将从GPS定位装置获取的横摆角速度记作ωimu，将由方向盘转角计算得到的横摆角速度记作ωchassis，将当前时刻的控制向量的横摆角速度记作ωk；
根据车辆的方向盘转角计算横摆角速度的公式如下：
Figure FDA0002377777760000032
上式中，vchassis表示从车身获取的车速，tan表示正切函数，
Figure FDA0002377777760000033
表示方向盘转角，i表示转向传动比，L表示车辆轴距；
当车速vchassis小于预设车速阈值Thresholdvelocity时，当前时刻的控制向量的横摆角速度取从车辆的方向盘转角计算得到的横摆角速度ωchassis，也即：
ωk＝ωchassis
当车速vchassis大于或等于预设车速阈值Thresholdvelocity时，当前时刻的控制向量的横摆角速度取从GPS定位装置获取的横摆角速度ωimu和根据车辆的方向盘转角计算得到的横摆角速度ωchassis的均值，也即：
Figure FDA0002377777760000041
4.根据权利要求1所述结合GPS定位与车身信息的位姿校正方法，其特征在于，所述步骤S3中，设k表示当前时刻，当k＝0时为初始时刻，将当前时刻的观测向量Z0的值赋值给状态向量的最优估计值X(0|0)，使用以下公式计算协方差P(0|0)的值：
Figure FDA0002377777760000042
其中，ζx 2和ζy 2为从GPS定位装置获取的经度的方差、纬度的方差的数值转换到UTM坐标系下的数值，
Figure FDA0002377777760000043
为从GPS定位装置获取的航向角的方差。
5.根据权利要求4所述结合GPS定位与车身信息的位姿校正方法，其特征在于，所述步骤S4具体为：
当k为正整数时，根据k-1时刻的状态向量的最优估计值X(k-1|k-1)和k时刻的控制向量的值Uk预测当前时刻的状态向量的最优估计值X(k|k-1)，预测当前时刻的状态向量的最优估计值的数学公式如下：
X(k|k-1)＝g(Uk，X(k-1|k-1))
Figure FDA0002377777760000044
过程噪声的协方差Rk使用下式进行计算：
Figure FDA0002377777760000045
Gk为k时刻系统状态函数g对状态向量X的偏导数，计算公式如下：
Figure FDA0002377777760000046
其中，X(k-1|k-1)是k-1时刻状态向量的最优估计值，Uk为k时刻控制向量；P(k|k-1)是X(k|k-1)对应的协方差，P(k-1|k-1)是X(k-1|k-1)对应的协方差；w1，w2，w3，w4为固定的系数，vk表示控制向量的车速，ωk表示控制向量的横摆角速度。
6.根据权利要求5所述结合GPS定位与车身信息的位姿校正方法，其特征在于，所述步骤S5具体为：
根据预测得到的当前时刻状态向量的最优估计值X(k|k-1)和观测向量Zk的值，更新当前时刻的状态向量的最优估计值X(k|k)，更新当前时刻状态向量的最优估计值的数学公式如下：
Figure FDA0002377777760000051
X(k|k)＝X(k|k-1)+Kg(k)(Zk-h(X(k|k-1)))
P(k|k)＝(1-Kg(k)Hk)X(k|k)
观测噪声的协方差Qk使用下式进行计算：
Figure FDA0002377777760000052
Hk为k时刻观测函数h对状态向量X的偏导数，计算公式如下：
Figure FDA0002377777760000053
Kg为卡尔曼增益。
7.一种结合GPS定位与车身信息的位姿校正装置，其特征在于，所述装置包括：
模型构建模块：用于以车辆位置和航向角作为状态向量，以车速和横摆角速度作为控制向量，以从GPS定位装置获取的位置和航向角作为观测向量，建立扩展卡尔曼滤波模型；
参量设置模块：用于实时获取GPS定位信息和车身信息，并使用获取的GPS定位信息和车身信息设置当前时刻的观测向量和控制向量的值；
位姿校正模块：用于初始化扩展卡尔曼滤波模型并使用扩展卡尔曼滤波模型进行迭代运算，根据上一时刻的状态向量的最优估计值和当前时刻控制向量的值预测当前时刻的状态向量的最优估计值；根据预测得到的当前时刻状态向量的最优估计值和当前时刻观测向量的值，更新当前时刻的状态向量的最优估计值；
位姿输出模块：输出更新后的的状态向量的最优估计值，得到校正后的车辆位姿。
8.根据权利要求7所述结合GPS定位与车身信息的位姿校正装置，其特征在于，所述模型构建模块中，记状态向量为X，控制向量为U，观测向量为Z，
系统的状态转移方程如下：
Xk＝g(Uk,Xk-1)+εk
系统的观测方程如下：
Zk＝h(Xk)+δk
Figure FDA0002377777760000061
其中，Xk-1表示k-1时刻的状态向量，Xk和(x y θ)k T均表示k时刻的状态向量，Uk和(vkωk)T均表示k时刻的控制向量，Zk和(zx zy zθ)k T均表示k时刻的观测向量；εk表示k时刻时状态转移方程中的系统噪声，δk表示k时刻时观测方程中的观测噪声，g(Uk,Xk-1)为系统状态函数，h(Xk)为观测函数；
系统状态函数g(Uk,Xk-1)计算公式如下：
当ωk的绝对值小于预设阈值Thresholdstraightline时，
Figure FDA0002377777760000062
当ωk的绝对值大于或等于阈值Thresholdstraightline时，
Figure FDA0002377777760000063
其中，xk-1表示k-1时刻的状态向量的横向坐标值，yk-1表示k-1时刻状态向量的纵向坐标值，θk-1表示k-1时刻状态向量的车辆行驶方向与横轴的夹角；vk表示k时刻控制向量的车速，ωk表示k时刻控制向量的横摆角速度，Δt表示k时刻与k-1时刻的时间差。
9.根据权利要求7所述结合GPS定位与车身信息的位姿校正装置，其特征在于，所述参量设置模块具体包括：
观测向量设置单元：用于获取GPS定位信息，并使用获取的GPS定位信息设置当前时刻的观测向量Zk的值；
获取的GPS定位信息包括经度、纬度、航向角、横摆角速度、经度的方差、纬度的方差和航向角的方差，将所述GPS定位信息由GPS坐标转换为UTM坐标；将转换后的基于UTM坐标系的位置和航向角的值作为当前时刻的观测向量的值；
控制向量设置单元：用于获取车身信息，并使用获取的车身信息中车速的值设置当前时刻的控制向量Uk的车速的值；
从车身获取车速vchassis、方向盘转角
Figure FDA0002377777760000073
当前时刻的控制向量的车速vk从车身获取，也即：
vk＝vchassis
结合从GPS定位装置获取的横摆角速度的值和从车身获取的方向盘转角计算得到的横摆角速度的值，来设置当前时刻的控制向量的横摆角速度的值；
将从GPS定位装置获取的横摆角速度记作ωimu，将由方向盘转角计算得到的横摆角速度记作ωchassis，将当前时刻的控制向量的横摆角速度记作ωk；
根据车辆的方向盘转角计算横摆角速度的公式如下：
Figure FDA0002377777760000071
上式中，vchassis表示从车身获取的车速，tan表示正切函数，
Figure FDA0002377777760000072
表示方向盘转角，i表示转向传动比，L表示车辆轴距；
当车速vchassis小于预设车速阈值Thresholdvelocity时，当前时刻的控制向量的横摆角速度取从车辆的方向盘转角计算得到的横摆角速度ωchassis，也即：
ωk＝ωchassis
当车速vchassis大于或等于预设车速阈值Thresholdvelocity时，当前时刻的控制向量的横摆角速度取从GPS定位装置获取的横摆角速度ωimu和根据车辆的方向盘转角计算得到的横摆角速度的均值，也即：
Figure FDA0002377777760000081
10.根据权利要求7所述结合GPS定位与车身信息的位姿校正装置，其特征在于，所述位姿校正模块具体包括：
状态预测单元：用于根据上一时刻的状态向量的最优估计值和当前时刻控制向量的值预测当前时刻的状态向量的最优估计值，预测当前时刻的状态向量的最优估计值X(k|k-1)的数学公式如下：
X(k|k-1)＝g(Uk，X(k-1|k-1))
Figure FDA0002377777760000082
过程噪声的协方差Rk使用下式进行计算：
Figure FDA0002377777760000083
Gk为k时刻系统状态函数g对状态向量X的偏导数，计算公式如下：
Figure FDA0002377777760000084
其中，X(k-1|k-1)是k-1时刻状态向量的最优估计值，Uk为k时刻控制向量；P(k|k-1)是X(k|k-1)对应的协方差，P(k-1|k-1)是X(k-1|k-1)对应的协方差；w1，w2，w3，w4为固定的系数，vk表示控制向量的车速；
状态更新单元：用于更新当前时刻状态向量的最优估计值，更新当前时刻状态向量的最优估计值X(k|k)的数学公式如下：
Figure FDA0002377777760000085
X(k|k)＝X(k|k-1)+Kg(k)(Zk-h(X(k|k-1)))
P(k|k)＝(1-Kg(k)Hk)x(k|k)
观测噪声的协方差Qk使用下式进行计算：
Figure FDA0002377777760000091
Hk为k时刻观测函数h对状态向量X的偏导数，计算公式如下：
Figure FDA0002377777760000092
Kg(k)为卡尔曼增益。
CN202010073141.1A 2020-01-22 2020-01-22 一种结合gps定位与车身信息的位姿校正方法及装置 Active CN111458733B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010073141.1A CN111458733B (zh) 2020-01-22 2020-01-22 一种结合gps定位与车身信息的位姿校正方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010073141.1A CN111458733B (zh) 2020-01-22 2020-01-22 一种结合gps定位与车身信息的位姿校正方法及装置

Publications (2)

Publication Number Publication Date
CN111458733A CN111458733A (zh) 2020-07-28
CN111458733B true CN111458733B (zh) 2022-05-24

Family

ID=71683191

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010073141.1A Active CN111458733B (zh) 2020-01-22 2020-01-22 一种结合gps定位与车身信息的位姿校正方法及装置

Country Status (1)

Country Link
CN (1) CN111458733B (zh)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112611377B (zh) * 2020-11-30 2024-03-12 武汉理工大学 一种用于小车室外导航的状态预测方法、装置及存储介质
CN112433531A (zh) * 2021-01-27 2021-03-02 国汽智控(北京)科技有限公司 一种自动驾驶车辆的轨迹跟踪方法、装置及计算机设备
CN114167470A (zh) * 2021-10-29 2022-03-11 华为技术有限公司 一种数据处理方法和装置

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105759289A (zh) * 2014-12-15 2016-07-13 国际商业机器公司 用于处理gps漂移的方法和系统
CN107621264A (zh) * 2017-09-30 2018-01-23 华南理工大学 车载微惯性/卫星组合导航系统的自适应卡尔曼滤波方法
CN107702727A (zh) * 2017-09-04 2018-02-16 武汉光庭科技有限公司 一种自动驾驶过程中使车辆位置平滑的装置及方法
CN109115225A (zh) * 2018-08-31 2019-01-01 江苏大学 一种无人作业谷物联合收割机导航方法与导航装置

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102498415B (zh) * 2009-09-19 2014-04-16 天宝导航有限公司 具有漫游器模糊度固定的gnss信号处理

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105759289A (zh) * 2014-12-15 2016-07-13 国际商业机器公司 用于处理gps漂移的方法和系统
CN107702727A (zh) * 2017-09-04 2018-02-16 武汉光庭科技有限公司 一种自动驾驶过程中使车辆位置平滑的装置及方法
CN107621264A (zh) * 2017-09-30 2018-01-23 华南理工大学 车载微惯性/卫星组合导航系统的自适应卡尔曼滤波方法
CN109115225A (zh) * 2018-08-31 2019-01-01 江苏大学 一种无人作业谷物联合收割机导航方法与导航装置

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
Vehicle Localization in Mountainous Gravelled Paths;Yoichi Morales et al.;《2012 IEEE/RSJ International Conference on Intelligent Robots and Systems》;20121231;第4342-4347页 *
农机导航自校正模型控制方法研究;白晓平等;《农业机械学报》;20150228;第46卷(第2期);第1-7页 *

Also Published As

Publication number Publication date
CN111458733A (zh) 2020-07-28

Similar Documents

Publication Publication Date Title
CN111458733B (zh) 一种结合gps定位与车身信息的位姿校正方法及装置
CN110271534B (zh) 自动驾驶车辆的控制方法、装置、计算机设备和存储介质
Xu et al. Enhancing localization accuracy of MEMS-INS/GPS/in-vehicle sensors integration during GPS outages
CN106840179B (zh) 一种基于多传感器信息融合的智能车定位方法
CN112097763B (zh) 一种基于mems imu/磁力计/dvl组合的水下运载体组合导航方法
US7983842B2 (en) Vehicular present position detection apparatus and program storage medium
CN111912413B (zh) 一种定位方法及装置
CN109855617A (zh) 一种车辆定位方法、车辆定位装置及终端设备
CN106980133A (zh) 利用神经网络算法补偿和修正的gps ins组合导航方法及系统
US11529838B2 (en) Method and system for estimating vehicle body tilt
CN107132563B (zh) 一种里程计结合双天线差分gnss的组合导航方法
CN111721290B (zh) 一种多源传感器信息融合定位切换方法
CN110133694B (zh) 基于双天线gnss航向和轮速辅助的车辆定位方法及系统
CN113252048A (zh) 一种导航定位方法、导航定位系统及计算机可读存储介质
CN111536972A (zh) 一种基于里程计刻度系数修正的车载dr导航方法
CN115042816B (zh) 路径跟踪方法、装置、设备、介质及程序
CN112798021A (zh) 基于激光多普勒测速仪的惯导系统行进间初始对准方法
CN112433531A (zh) 一种自动驾驶车辆的轨迹跟踪方法、装置及计算机设备
CN114076610A (zh) Gnss/mems车载组合导航系统的误差标定、导航方法及其装置
JP7431511B2 (ja) 運動及び位置センサを用いて車両位置を衛星に依拠して算出する方法
CN115617051B (zh) 车辆控制方法、装置、设备和计算机可读介质
CN110133695A (zh) 一种双天线gnss位置延迟时间动态估计系统及方法
CN112965373A (zh) 一种农用和矿用铰接式车辆路径跟踪控制方法
CN111044053A (zh) 单舵轮无人车的导航方法、装置和单舵轮无人车
CN114935345A (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