一种测距、测向、测速相结合的智能车协同定位方法技术

本发明专利技术提供了一种测距、测向、测速相结合的智能车协同定位方法，包括：基于惯性测量单元测量车辆的线运动信息和角运动信息，获得离散时间的车辆名义位姿递推方程；基于惯性测量单元测量的误差状态变量，建立误差状态离散时间下的递推方程，获得车辆误差状态的先验估计；利用路侧单元对车辆进行实时测距、测向、测速，获得观测方程，基于卡尔曼滤波方程对车辆的状态进行估计，获得车辆误差状态的后验估计；将所述误差状态的后验估计注入到车辆名义位姿递推方程中，获得车辆定位估计。本发明专利技术在GNSS信号不佳区域，利用路侧单元辅助可以修正车辆的累计定位误差，提高车辆的定位精度。提高车辆的定位精度。提高车辆的定位精度。

【技术实现步骤摘要】
一种测距、测向、测速相结合的智能车协同定位方法


[0001]本专利技术涉及智能交通
，具体涉及一种测距、测向、测速相结合的智能车协同定位方法。

技术介绍

[0002]智能车被认为在改善道路安全、提高交通运行效率等方面有着巨大的潜力，近年来得到了广泛关注与研究。目前利用RTK
‑
GNSS、INS进行组合导航定位的无人车辆在理想工况下可以实现满足行驶要求的厘米级定位。但是在城市环境下，高大建筑物遮挡GNSS信号时，组合导航的定位精度变差，不足以保证车辆的安全行驶。
[0003]因此，如何提供一种基于路侧单元辅助，提高车辆的定位精度的测距、测向、测速相结合的智能车协同定位方法是本领域技术人员亟需解决的问题。

技术实现思路

[0004]有鉴于此，本专利技术提出一种测距、测向、测速相结合的智能车协同定位方法，不受道路环境限制，保证车辆的安全行驶。
[0005]为了实现上述目的，本专利技术采用如下技术方案：
[0006]一种测距、测向、测速相结合的智能车协同定位方法，包括如下步骤：
[0007]基于惯性测量单元测量车辆的线运动信息和角运动信息，获得离散时间的车辆名义位姿递推方程；
[0008]基于惯性测量单元测量的误差状态变量，建立误差状态离散时间下的递推方程，获得车辆误差状态的先验估计；
[0009]利用路侧单元对车辆进行实时测距、测向、测速，获得观测方程，基于卡尔曼滤波方程对车辆的状态进行估计，获得车辆误差状态的后验估计；
[0010]将所述误差状态的后验估计注入到车辆名义位姿递推方程中，获得车辆定位估计。
[0011]优选的，利用IMU测量车辆线运动信息和角运动信息，所述线运动信息包括加速度测量值及其偏置，所述角运动信息包括角速率测量值及其偏置；
[0012]根据车辆初始位姿和位姿递推方程递推车辆的位姿，当加速度测量值和角速度测量值具有固定的偏置，获得离散时间的车辆名义位姿递推方程为:
[0013][0014]v
k+1
＝v
k
+[R
k
(a
mk
‑
a
bk
)+g
k
]Δt
[0015][0016]a
b(k+1)
＝a
bk
[0017]ω
b(k+1)
＝ω
bk
[0018]式中，下标k和k+1表示相邻的两个时刻，对应的时间间隔为Δt；p、v分别表示车辆
在导航坐标系下的位置和速度；R表示从机体坐标系到导航坐标系的旋转矩阵；a
m
、a
b
分别表示加速度测量值及其偏置；g表示车辆所在位置的重力；q表示从机体坐标系到导航坐标系的旋转四元数；q
k
{(ω
mk
‑
ω
bk
)Δt}表示轴角向量(ω
mk
‑
ω
bk
)Δt对应的四元数；ω
m
、ω
b
分别表示角速率测量值及其偏置。
[0019]优选的，待估计的所述误差状态变量为：
[0020]δX＝[(δp)
T (δv)
T (δθ)
T (δa
b
)
T (δω
b
)
T
]T
[0021]式中，δp、δv和δθ分别表示车辆的位置误差、速度误差和姿态误差；δa
b
和δω
b
分别表示加速度偏置估计和角速度偏置估计的误差；
[0022]建立误差状态离散时间下的递推方程为：
[0023]δp
k+1
＝δp
k
+δv
k
Δt
[0024][0025][0026][0027][0028]式中，下标k和k+1表示相邻的两个时刻，对应的时间间隔为Δt；R、R
T
分别表示从机体坐标系到导航坐标系的旋转矩阵及其转置；a
m
、a
b
分别表示加速度测量值及其偏置；δθ表示车辆机体坐标系下的姿态误差；表示速度误差估计的扰动向量；ω
m
、ω
b
分别表示角速率测量值及其偏置；表示姿态误差估计的扰动脉冲向量；分别表示加速度偏置估计和角速度偏置估计的扰动脉冲向量。
[0029]优选的，根据误差状态离散时间下的递推方程，获得车辆误差状态的先验估计及其对应的协方差
[0030][0031][0032][0033][0034][0035]式中，I为3
×
3的单位矩阵；0为3
×
3的零矩阵；Q
k
表示扰动脉冲向量n的协方差矩阵，分别表示速度随机游走和角度随机游走；分别表示加速度计和陀螺仪的动态零偏的功率谱密度。
[0036]优选的，所述观测方程为：
[0037][0038][0039]式中，Z为观测矩阵；分别为路侧单元对车辆的实际距离测量、角度测量、速度测量；分别为车辆到路侧单元之间的计算距离、计算角度和计算速度；w为路测单元观测噪声；
[0040]车辆误差状态的后验估计及其对应的协方差为：
[0041][0042][0043][0044][0045]式中，I为3
×
3的单位矩阵；分别为路侧单元与车辆之间的测距误差、测向误差、测速误差服从的概率分布的方差。
[0046]优选的，将所述误差状态的后验估计注入到车辆名义位姿递推方程中，获得车辆定位估计为：
[0047][0048][0049][0050]式中，p
k+1
、v
k+1
分别为车辆名义位置估计和名义速度估计；分别为车辆位置误差后验估计和速度误差后验估计；q
k+1
为车辆名义姿态对应的四元数；算子表示姿态误差后验估计对应的四元数。
[0051]优选的，获得当前时刻车辆定位估计后，误差状态进行重置以用于下一个滤波周期的估计：
[0052][0053]其中，每收到一次路侧单元对车辆的测距、测向、测速信息对应开启一个滤波周期。
[0054]经由上述的技术方案可知，与现有技术相比，本专利技术的有益效果包括：
[0055]本专利技术车辆基于惯性测量单元IMU测量进行位姿递推解算；同时在GNSS信号不佳区域，利用路侧单元辅助可以与车辆进行通讯以及测距、测向、测速，利用误差状态卡尔曼滤波算法对车辆位姿进行滤波，修正惯性测量单元对车辆定位的累计定位误差，提高车辆的定位精度提高了车辆的定位精度，保证了车辆的安全行驶。
附图说明
[0056]为了更清楚地说明本专利技术实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本专利技术的实施例，对于本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种测距、测向、测速相结合的智能车协同定位方法，其特征在于，包括如下步骤：基于惯性测量单元测量车辆的线运动信息和角运动信息，获得离散时间的车辆名义位姿递推方程；基于惯性测量单元测量的误差状态变量，建立误差状态离散时间下的递推方程，获得车辆误差状态的先验估计；利用路侧单元对车辆进行实时测距、测向、测速，获得观测方程，基于卡尔曼滤波方程对车辆的状态进行估计，获得车辆误差状态的后验估计；将所述误差状态的后验估计注入到车辆名义位姿递推方程中，获得车辆定位估计。2.根据权利要求1所述的一种测距、测向、测速相结合的智能车协同定位方法，其特征在于，利用IMU测量车辆线运动信息和角运动信息，所述线运动信息包括加速度测量值及其偏置，所述角运动信息包括角速率测量值及其偏置；根据车辆初始位姿和位姿递推方程递推车辆的位姿，当加速度测量值和角速度测量值具有固定的偏置，获得离散时间的车辆名义位姿递推方程为:v
k+1
＝v
k
+[R
k
(a
mk
‑
a
bk
)+g
k
]Δta
b(k+1)
＝a
bk
ω
b(k+1)
＝ω
bk
式中，下标k和k+1表示相邻的两个时刻，对应的时间间隔为Δt；p、v分别表示车辆在导航坐标系下的位置和速度；R表示从机体坐标系到导航坐标系的旋转矩阵；a
m
、a
b
分别表示加速度测量值及其偏置；g表示车辆所在位置的重力；q表示从机体坐标系到导航坐标系的旋转四元数；q
k
{(ω
mk
‑
ω
bk
)Δt}表示轴角向量(ω
mk
‑
ω
bk
)Δt对应的四元数；ω
m
、ω
b
分别表示角速率测量值及其偏置。3.根据权利要求2所述的一种测距、测向、测速相结合的智能车协同定位方法，其特征在于，待估计的所述误差状态变量为：δX＝[(δp)
T (δv)
T (δθ)
T (δa
b
)
T (δω
b
)
T
]
T
式中，δp、δv和δθ分别表示车辆的位置误差、速度误差和姿态误差；δa
b
和δω
b
分别表示加速度偏置估计和角速度偏置估计的误差；建立误差状态离散时...

【专利技术属性】
技术研发人员：蒋朝阳，盛树轩，荆崇波，王星琦，高九龙，
申请(专利权)人：北京理工大学，
类型：发明
国别省市：