CN116499472A - 一种兼顾车辆动力学和非完整约束的融合定位方法 - Google Patents

一种兼顾车辆动力学和非完整约束的融合定位方法 Download PDF

Info

Publication number
CN116499472A
CN116499472A CN202310415749.1A CN202310415749A CN116499472A CN 116499472 A CN116499472 A CN 116499472A CN 202310415749 A CN202310415749 A CN 202310415749A CN 116499472 A CN116499472 A CN 116499472A
Authority
CN
China
Prior art keywords
vehicle
speed
equation
coordinate system
establishing
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
CN202310415749.1A
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.)
Jilin University
Original Assignee
Jilin University
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 Jilin University filed Critical Jilin University
Priority to CN202310415749.1A priority Critical patent/CN116499472A/zh
Publication of CN116499472A publication Critical patent/CN116499472A/zh
Pending legal-status Critical Current

Links

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/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
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

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

Abstract

本发明适用于车辆定位导航技术领域，提供了一种兼顾车辆动力学和非完整约束的融合定位方法，包括以下步骤：读取GPS接收机、IMU传感器、车辆CAN等提供的信息；根据读取的信息建立坐标系；根据IMU传感器与GPS接收机（或UWB）提供的信息，通过INS运动学模型解算出位置、速度和姿态信息；建立状态方程和观测方程，利用误差状态卡尔曼滤波估计导航误差值和IMU传感器偏差值，结果用来对INS运动学模型进行修正；基于车辆动力学建立航迹推算模型，建立状态方程和观测方程，利用扩展卡尔曼滤波估计位置和航向角信息。该方法可提高GNSS信号受干扰或中断时导航定位的精度，有效提高定位精度并延长定位时间。

Description

一种兼顾车辆动力学和非完整约束的融合定位方法
技术领域
本发明属于车辆定位导航技术领域，尤其涉及一种兼顾车辆动力学和非完整约束的融合定位方法。
背景技术
智能车辆的决策、轨迹规划和运动控制的实现，高度依赖于精确和连续的位置信息，GNSS是一种公认的获取车辆位置的方法，可以提供与实时运动学(RTK)相结合的亚分米定位精度。然而，由于复杂的城市环境，其高精度定位可能会因信号中断而退化。INS可以在短时间内确定位置、速度和姿态信息，但加速度计和陀螺仪的偏差和噪声会导致误差随时间累积。通过集成GNSS和INS，可以实现更准确和健壮的导航解决方案。
然而，因为在GNSS中断期间只有INS运行，所以GNSS/INS导航系统不能在GNSS易受干扰的环境中提供准确的定位信息。航迹推算系统可以根据传感器信息独立推算位置而不依赖GNSS信息，可以在GNSS信号短时间断开的情况下提供定位信息，但是单独的DR算法不能满足智能车辆对定位精度的需求，这是由于实际行驶过程中，路面颠簸导致的二维平面假设不成立会引起较大的误差。UWB可以提供高精度的定位信息，但UWB设备需要预先安装，难以实现室外道路全覆盖，一般用于室内定位。
发明内容
本发明实施例的目的在于提供一种兼顾车辆动力学和非完整约束的融合定位方法，旨在解决上述背景技术中提出的问题。
本发明实施例是这样实现的，一种兼顾车辆动力学和非完整约束的融合定位方法，包括以下步骤：
步骤1、读取GPS接收机提供的位置、速度和姿态信息，IMU传感器提供的加速度和角速度信息车辆CAN总线提供的车速、轮速和前轮转角信息，以及在室内预先安装的UWB设备提供的位置信息；
步骤2、对读取的信息进行预处理，建立相应的坐标系；
步骤3、根据水平几何精度因子，即HDOP值判断GNSS信号是否可用，若不可用，则读取UWB提供的位置信息；
步骤4、根据IMU传感器与GPS接收机，或IMU传感器与UWB提供的信息，通过INS运动学模型解算出位置、速度和姿态信息；
步骤5、建立状态方程和观测方程，利用误差状态卡尔曼滤波(ESKF)估计导航误差值和IMU传感器偏差值，所得的导航误差估计值和IMU传感器偏差估计值用来对INS运动学模型进行修正；
步骤6、基于车辆动力学建立航迹推算模型(DR)，建立状态方程和观测方程，利用扩展卡尔曼滤波(EKF)估计位置和航向角信息；
步骤S7，输出最终估计的位置信息和航向角信息。
进一步的技术方案，在所述步骤4中，INS运动学模型如下：
其中，pk、vk、εk表示GPS接收机提供的(或UWB提供的)位置、速度和姿态(用单位四元数表示)，sk、ωk表示惯性测量单元(IMU)提供的三维比例和角速度信息，是本体坐标系到导航平台坐标系的旋转矩阵。
进一步的技术方案，在所述步骤3中，当HDOP小于阈值，则依赖GPS接收机信号进行位置估计；当HDOP大于阈值，则依赖UWB信号进行位置估计。
进一步的技术方案，在所述步骤5中，误差状态卡尔曼滤波系统的状态空间模型的搭建包括以下步骤：
步骤5.1、建立系统状态方程如下：
其中，δpk、δvk、εk分别表示导航平台位置、速度、姿态的实际值与名义值之间的误差；δsk、δωk分别表示缓慢变化的IMU测得的比例和角速度偏差；表示测量噪声，设为高斯白噪声，协方差矩阵为Qm；/>表示随机游走的过程噪声，设为高斯白噪声，协方差矩阵为Qb，假设随机游走过程噪声与测量噪声不相关；εk为一个欧拉角向量，可根据式(3)求得代表导航平台姿态实际值的单位四元数q：
步骤5.2、建立系统观测方程如下：
其中，是x，y方向上速度的虚拟测量值，根据非完整约束确定，/>和/>分别表示GPS接收机给出的位置和速度的测量噪声，假设为不相关的高斯白噪声，协方差矩阵为和/> 为CAN总线上车速的测量噪声，设其为高斯白噪声，用协方差矩阵为 是从导航平台坐标系到车体坐标系的旋转矩阵，可通过标定获得，/>是当地水平坐标系到导航平台坐标系的旋转矩阵，/>表示将当地水平坐标系下的导航平台速度转换为车体坐标系下的车辆质心速度。
进一步的技术方案，在所述步骤5.2中，非完整约束假设车辆在平面运动过程中不会侧滑并且在与路面垂直的方向上没有运动，即车辆质心处沿车体坐标系下的y轴和z轴速度为零，但实际情况由于转向时存在侧滑、道路颠簸等原因使得这两个方向的速度不完全为零，因此根据式(5)放宽这些约束：
其中，enhlolo为高斯白噪声，协方差矩阵为Rnhlolo，用以反映相应约束条件的放松程度。
进一步的技术方案，在所述步骤6中，基于车辆动力学的航迹推算模型如下：
其中，X、Y表示车辆在当地水平坐标系中的坐标，vx、vy表示其速度在车体坐标系中的表达，ψ表示车辆的横摆角速度。
进一步的技术方案，在所述步骤6中，扩展卡尔曼滤波的状态空间模型的搭建包括以下步骤：
步骤6.1、建立系统非线性状态方程如下：
xk＝f(xk-1,uk)+wk,cov(wk)＝Qk (7)
xk＝f(xk-1,uk)具体为下列方程：
ψk＝ψk-1+Tsrk-1
其中，vx、vy分别是车速沿车体坐标系x轴和y轴方向的分量，r是车辆的横摆角速度，δ是前轮转角，lf、lr分别是从车辆的质心到前后轴的距离，Cαf、Cαr分别为单个前轮轮胎和单个后轮轮胎的侧偏刚度；
步骤6.2、建立系统观测方程如下：
其中，vrL和vrR分别代表左右后轮中心的速度，dr代表后轮轮距。利用后轮轮速与车辆的纵向速度和横摆角速度之间的关系，将ABS轮速传感器的测量值也用于观测方程的建立。
进一步的技术方案，在所述步骤6中，在GPS信号中断时INS定位信息会产生较大误差，建立观测方程时排除观测值中第三、第四行的导航位置信息，使航迹推算模型的结果不易受INS较大误差的影响，提高定位精度。
本发明实施例提供的一种兼顾车辆动力学和非完整约束的融合定位方法，同时考虑车辆动力学和非完整约束，将GNSS、INS、DR和UWB融合进行定位，可以提高GNSS信号受干扰或中断时导航定位的精度，尤其在GNSS信号被完全遮盖的场景下(如地下停车场)，可以有效提高定位精度并延长定位时间。
附图说明
图1为本发明实施例提供的一种兼顾车辆动力学和非完整约束的融合定位方法的流程示意图；
图2为本发明实施例提供的一种兼顾车辆动力学和非完整约束的融合定位方法的结构框图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
以下结合具体实施例对本发明的具体实现进行详细描述。
本发明一个实施例提供的一种兼顾车辆动力学和非完整约束的融合定位方法，具体的，为兼顾车辆动力学和非完整约束的GNSS/INS/DR/UWB融合定位方法，包括以下步骤：
步骤1、读取GPS接收机和IMU传感器提供的信息(其中，GPS接收机提供的位置、速度和姿态信息，IMU提供的加速度和角速度信息)，车辆CAN总线提供的车速、轮速和前轮转角信息，以及在室内(如地下停车场等)预先安装的UWB设备提供的位置信息；
步骤2、对读取的信息进行预处理，建立相应的坐标系；
步骤3、根据HDOP值判断GNSS信号是否可用，若不可用，则读取UWB提供的位置信息，具体的，HDOP小于阈值，则依赖GPS接收机信号进行位置估计，HDOP大于阈值，则依赖UWB信号进行位置估计；
步骤4、根据GPS接收机(或UWB)和IMU提供的信息，通过INS运动学模型解算出位置、速度和姿态信息。INS运动学模型如下：
其中，pk、vk、εk表示GPS接收机提供的(或UWB提供的)位置、速度和姿态(用单位四元数表示)，sk、ωk表示惯性测量单元(IMU)提供的三维比例和角速度信息，是本体坐标系到导航平台坐标系的旋转矩阵。
步骤5、建立状态方程和观测方程，利用误差状态卡尔曼滤波(ESKF)估计导航误差值和IMU传感器偏差值。所得的导航误差估计值和IMU传感器偏差估计值用来对INS运动学模型进行修正。
系统状态方程如下：
其中，δpk、δvk、εk分别表示导航平台位置、速度、姿态的实际值与名义值之间的误差；δsk、δωk分别表示缓慢变化的IMU测得的比例和角速度偏差；表示测量噪声，设为高斯白噪声，协方差矩阵为Qm；/>表示随机游走的过程噪声，设为高斯白噪声，协方差矩阵为Qb，假设随机游走过程噪声与测量噪声不相关。εk为一个欧拉角向量，可根据式(3)求得代表导航平台姿态实际值的单位四元数q：
系统观测方程如下：
其中，是x，y方向上速度的虚拟测量值，根据非完整约束确定，/>和eGPSr分别表示GPS接收机给出的位置和速度的测量噪声，假设为不相关的高斯白噪声，协方差矩阵为和/> 为CAN总线上车速的测量噪声，设其为高斯白噪声，用协方差矩阵为 是从导航平台坐标系到车体坐标系的旋转矩阵，可通过标定获得，/>是当地水平坐标系到导航平台坐标系的旋转矩阵，/>表示将当地水平坐标系下的导航平台速度转换为车体坐标系下的车辆质心速度。
其中，非完整约束假设车辆在平面运动过程中不会侧滑并且在与路面垂直的方向上没有运动，即车辆质心处沿车体坐标系下的y轴和z轴速度为零。但实际情况由于转向时存在侧滑、道路颠簸等原因使得这两个方向的速度不完全为零，因此根据式(5)放宽这些约束：
其中，enhlolo为高斯白噪声，协方差矩阵为Rnhlolo，用以反映相应约束条件的放松程度。
使用误差卡尔曼滤波得到导航误差估计值和IMU传感器偏差估计值，对INS运动学模型得到的导航信息进行修正，即可完成对导航信息的估计。在下一个采样周期开始时，误差值被重置为零。
步骤6、基于车辆动力学建立航迹推算模型(DR)，建立状态方程和观测方程，利用扩展卡尔曼滤波(EKF)估计位置和航向角信息。
系统非线性状态方程如下：
xk＝f(xk-1,uk)+wk,cov(wk)＝Qk (7)
xk＝f(xk-1,uk)具体为下列方程：
ψk＝ψk-1+Tsrk-1
其中，vx、vy分别是车速沿车体坐标系x轴和y轴方向的分量，r是车辆的横摆角速度，δ是前轮转角，lf、lr分别是从车辆的质心到前后轴的距离，Cαf、Cαr分别为单个前轮轮胎和单个后轮轮胎的侧偏刚度。
系统观测方程如下：
其中，vrL和vrR分别代表左右后轮中心的速度，dr代表后轮轮距。利用后轮轮速与车辆的纵向速度和横摆角速度之间的关系，将ABS轮速传感器的测量值也用于观测方程的建立。
在步骤6中，基于车辆动力学的航迹推算模型如下：
其中，X、Y表示车辆在当地水平坐标系中的坐标，vx、vy表示其速度在车体坐标系中的表达，ψ表示车辆的横摆角速度。
步骤S7，输出最终估计的位置信息和航向角信息。
以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (5)

1.一种兼顾车辆动力学和非完整约束的融合定位方法，其特征在于，包括以下步骤：
步骤1、读取GPS接收机提供的位置、速度和姿态信息，IMU传感器提供的加速度和角速度信息车辆CAN总线提供的车速、轮速和前轮转角信息，以及在室内预先安装的UWB设备提供的位置信息；
步骤2、对读取的信息进行预处理，建立相应的坐标系；
步骤3、根据水平几何精度因子，即HDOP值判断GNSS信号是否可用，若不可用，则读取UWB提供的位置信息；
步骤4、根据IMU传感器与GPS接收机，或IMU传感器与UWB提供的信息，通过INS运动学模型解算出位置、速度和姿态信息；
步骤5、建立状态方程和观测方程，利用误差状态卡尔曼滤波估计导航误差值和IMU传感器偏差值，所得的导航误差估计值和IMU传感器偏差估计值用来对INS运动学模型进行修正；
步骤6、基于车辆动力学建立航迹推算模型，建立状态方程和观测方程，利用扩展卡尔曼滤波估计位置和航向角信息；
步骤S7，输出最终估计的位置信息和航向角信息。
2.根据权利要求1所述的兼顾车辆动力学和非完整约束的融合定位方法，其特征在于，在所述步骤3中，当HDOP小于阈值，依赖GPS接收机信号进行位置估计；当HDOP大于阈值，则依赖UWB信号进行位置估计。
3.根据权利要求1所述的兼顾车辆动力学和非完整约束的融合定位方法，其特征在于，非完整约束假设车辆在平面运动过程中不会侧滑并且在与路面垂直的方向上没有运动，即车辆质心处沿车体坐标系下的y轴和z轴速度为零，但实际情况由于转向时存在侧滑、道路颠簸等原因使得这两个方向的速度不完全为零，因此根据式(5)放宽这些约束：
其中，enhlolo为高斯白噪声，协方差矩阵为Rnhlolo，用以反映相应约束条件的放松程度。
4.根据权利要求1所述的兼顾车辆动力学和非完整约束的融合定位方法，其特征在于，在所述步骤6中，基于车辆动力学的航迹推算模型如下：
其中，X、Y表示车辆在当地水平坐标系中的坐标，vx、vy表示其速度在车体坐标系中的表达，ψ表示车辆的横摆角速度。
5.根据权利要求4所述的兼顾车辆动力学和非完整约束的融合定位方法，其特征在于，在所述步骤6中，扩展卡尔曼滤波的状态空间模型的搭建包括以下步骤：
步骤6.1、建立系统非线性状态方程如下：
xk＝f(xk-1,uk)+wk,cov(wk)＝Qk (7)
xk＝f(xk-1,uk)具体为下列方程：
ψk＝ψk-1+Tsrk-1
其中，vx、vy分别是车速沿车体坐标系x轴和y轴方向的分量，r是车辆的横摆角速度，δ是前轮转角，lf、lr分别是从车辆的质心到前后轴的距离，Cαf、Cαr分别为单个前轮轮胎和单个后轮轮胎的侧偏刚度；
步骤6.2、建立系统观测方程如下：
其中，vrL和vrR分别代表左右后轮中心的速度，dr代表后轮轮距；利用后轮轮速与车辆的纵向速度和横摆角速度之间的关系，将ABS轮速传感器的测量值也用于观测方程的建立。
CN202310415749.1A 2023-04-18 2023-04-18 一种兼顾车辆动力学和非完整约束的融合定位方法 Pending CN116499472A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310415749.1A CN116499472A (zh) 2023-04-18 2023-04-18 一种兼顾车辆动力学和非完整约束的融合定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310415749.1A CN116499472A (zh) 2023-04-18 2023-04-18 一种兼顾车辆动力学和非完整约束的融合定位方法

Publications (1)

Publication Number Publication Date
CN116499472A true CN116499472A (zh) 2023-07-28

Family

ID=87322357

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310415749.1A Pending CN116499472A (zh) 2023-04-18 2023-04-18 一种兼顾车辆动力学和非完整约束的融合定位方法

Country Status (1)

Country Link
CN (1) CN116499472A (zh)

Similar Documents

Publication Publication Date Title
CN109946732B (zh) 一种基于多传感器数据融合的无人车定位方法
Ahmed et al. Accurate attitude estimation of a moving land vehicle using low-cost MEMS IMU sensors
EP3109589B1 (en) A unit and method for improving positioning accuracy
Bevly et al. Integrating INS sensors with GPS measurements for continuous estimation of vehicle sideslip, roll, and tire cornering stiffness
CN104061899B (zh) 一种基于卡尔曼滤波的车辆侧倾角与俯仰角估计方法
Yi et al. IMU-based localization and slip estimation for skid-steered mobile robots
CN106568449B (zh) 一种基于mems的车辆模型辅助和约束的gnss/ins组合导航方法
WO2007143806A2 (en) Vehicular navigation and positioning system
CN109186597B (zh) 一种基于双mems-imu的室内轮式机器人的定位方法
CN103759733B (zh) 基于联邦滤波的ins/vkm/vdm车载导航系统
Yu et al. GPS/INS/Odometer/DR integrated navigation system aided with vehicular dynamic characteristics for autonomous vehicle application
Gao et al. Development of precise GPS/INS/wheel speed sensor/yaw rate sensor integrated vehicular positioning system
CN115060257B (zh) 一种基于民用级惯性测量单元的车辆变道检测方法
CN110940344A (zh) 一种用于自动驾驶的低成本传感器组合定位方法
Gao et al. An integrated land vehicle navigation system based on context awareness
CN113008229B (zh) 一种基于低成本车载传感器的分布式自主组合导航方法
Moussa et al. Wheel-based aiding of low-cost imu for land vehicle navigation in gnss challenging environment
Niu et al. Camera-based lane-aided multi-information integration for land vehicle navigation
CN110926483B (zh) 一种用于自动驾驶的低成本传感器组合定位系统及方法
CN116576849A (zh) 一种基于gmm辅助的车辆融合定位方法及系统
Han et al. Performance analysis of two-dimensional dead reckoning based on vehicle dynamic sensors during GNSS outages
CN116337053A (zh) 车辆导航方法、装置、电子设备及存储介质
CN108718533A (zh) 具有高程数据输入的无线惯性车辆导航
Liu et al. IMU/vehicle calibration and integrated localization for autonomous driving
CN116499472A (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