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

1.本发明涉及智能交通技术领域，具体涉及一种测距、测向、测速相结合的智能车协同定位方法。


背景技术：

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


技术实现要素：

4.有鉴于此，本发明提出一种测距、测向、测速相结合的智能车协同定位方法，不受道路环境限制，保证车辆的安全行驶。
5.为了实现上述目的，本发明采用如下技术方案：
6.一种测距、测向、测速相结合的智能车协同定位方法，包括如下步骤：
7.基于惯性测量单元测量车辆的线运动信息和角运动信息，获得离散时间的车辆名义位姿递推方程；
8.基于惯性测量单元测量的误差状态变量，建立误差状态离散时间下的递推方程，获得车辆误差状态的先验估计；
9.利用路侧单元对车辆进行实时测距、测向、测速，获得观测方程，基于卡尔曼滤波方程对车辆的状态进行估计，获得车辆误差状态的后验估计；
10.将所述误差状态的后验估计注入到车辆名义位姿递推方程中，获得车辆定位估计。
11.优选的，利用imu测量车辆线运动信息和角运动信息，所述线运动信息包括加速度测量值及其偏置，所述角运动信息包括角速率测量值及其偏置；
12.根据车辆初始位姿和位姿递推方程递推车辆的位姿，当加速度测量值和角速度测量值具有固定的偏置，获得离散时间的车辆名义位姿递推方程为:
[0013][0014]vk+1
＝vk+[rk(a
mk-a
bk
)+gk]δt
[0015][0016]ab(k+1)
＝a
bk
[0017]
ω
b(k+1)
＝ω
bk
[0018]
式中，下标k和k+1表示相邻的两个时刻，对应的时间间隔为δt；p、v分别表示车辆
在导航坐标系下的位置和速度；r表示从机体坐标系到导航坐标系的旋转矩阵；am、ab分别表示加速度测量值及其偏置；g表示车辆所在位置的重力；q表示从机体坐标系到导航坐标系的旋转四元数；qk{(ω
mk-ω
bk
)δt}表示轴角向量(ω
mk-ω
bk
)δt对应的四元数；ωm、ωb分别表示角速率测量值及其偏置。
[0019]
优选的，待估计的所述误差状态变量为：
[0020]
δx＝[(δp)
t (δv)
t (δθ)
t (δab)
t (δωb)
t
]
t
[0021]
式中，δp、δv和δθ分别表示车辆的位置误差、速度误差和姿态误差；δab和δωb分别表示加速度偏置估计和角速度偏置估计的误差；
[0022]
建立误差状态离散时间下的递推方程为：
[0023]
δp
k+1
＝δpk+δvkδt
[0024][0025][0026][0027][0028]
式中，下标k和k+1表示相邻的两个时刻，对应的时间间隔为δt；r、r
t
分别表示从机体坐标系到导航坐标系的旋转矩阵及其转置；am、ab分别表示加速度测量值及其偏置；δθ表示车辆机体坐标系下的姿态误差；表示速度误差估计的扰动向量；ωm、ωb分别表示角速率测量值及其偏置；表示姿态误差估计的扰动脉冲向量；分别表示加速度偏置估计和角速度偏置估计的扰动脉冲向量。
[0029]
优选的，根据误差状态离散时间下的递推方程，获得车辆误差状态的先验估计及其对应的协方差
[0030][0031][0032][0033]
[0034][0035]
式中，i为3
×
3的单位矩阵；0为3
×
3的零矩阵；qk表示扰动脉冲向量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]
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据提供的附图获得其他的附图；
[0057]
图1为本发明实施例提供的测距、测向、测速相结合的智能车协同定位方法的流程图；
[0058]
图2为本发明实施例提供的路侧单元对车辆进行测距、测向、测速的状态示意图。
具体实施方式
[0059]
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0060]
参见附图1，本发明提供了一种测距、测向、测速相结合的智能车协同定位方法，按照以下过程实现的：
[0061]
基于惯性测量单元测量车辆的线运动信息和角运动信息，获得离散时间的车辆名义位姿递推方程；
[0062]
基于惯性测量单元测量的误差状态变量，建立误差状态离散时间下的递推方程，获得车辆误差状态的先验估计；
[0063]
利用路侧单元对车辆进行实时测距、测向、测速，获得观测方程，基于卡尔曼滤波方程对车辆的状态进行估计，获得车辆误差状态的后验估计；
[0064]
将所述误差状态的后验估计注入到车辆名义位姿递推方程中，获得车辆定位估计。
[0065]
在一个实施例中，基于惯导的航位推算定位步骤，具体包括：
[0066]
惯性测量单元imu安装在车辆上，利用imu测量车辆线运动信息和角运动信息，根据车辆初始位姿和位姿递推方程递推车辆的位姿，如果不考虑imu测量噪声，假设加速度计和陀螺仪测量只有一个固定不变的偏置，此时车辆的状态称为名义状态，离散时间的车辆名义位姿递推方程为:
[0067][0068]vk+1
＝vk+[rk(a
mk-a
bk
)+gk]δt
[0069][0070]ab(k+1)
＝a
bk
[0071]
ω
b(k+1)
＝ω
bk
[0072]
式中，下标k和k+1表示相邻的两个时刻，对应的时间间隔为δt；p、v分别表示车辆在导航坐标系下的位置和速度；r表示从机体坐标系到导航坐标系的旋转矩阵；am、ab分别表示加速度测量值及其偏置，即线运动信息；g表示车辆所在位置的重力；q表示从机体坐标系到导航坐标系的旋转四元数；qk{(ω
mk-ω
bk
)δt}表示轴角向量(ω
mk-ω
bk
)δt对应的四元数；ωm、ωb分别表示角速率测量值及其偏置，即角运动信息。
[0073]
本实施例中，车辆实际运动过程中，由于存在测量噪声，加速度测量偏置ab和角速率测量偏置ωb是时变的，需要结合其他传感器观测进行估计，否则会产生累计误差。选取待估计的误差状态变量为：
[0074]
δx＝[(δp)
t (δv)
t (δθ)
t (δab)
t (δωb)
t
]
t
[0075]
式中，δp、δv和δθ分别分别表示车辆的位置误差、速度误差和姿态误差；和分别表示加速度偏置估计和角速度偏置估计的误差。
[0076]
建立误差状态离散时间下的递推方程为：
[0077]
δp
k+1
＝δpk+δvkδt
[0078][0079][0080][0081][0082]
式中，下标k和k+1表示相邻的两个时刻，对应的时间间隔为δt；r、r
t
分别表示从机体坐标系到导航坐标系的旋转矩阵及其转置；am、ab分别表示加速度测量值及其偏置；δθ表示车辆机体坐标系下的姿态误差；表示速度误差估计的扰动向量；ωm、ωb分别表示角速率测量值及其偏置；表示姿态误差估计的扰动脉冲向量；分别表示加速度偏置估计和角速度偏置估计的扰动脉冲向量。
[0083]
在一个实施例中，利用路侧单元进行测距、测向、测速相结合的车辆协同定位，具体包括：
[0084]
车辆与路侧单元可以进行通讯以及测距、测向、测速，从而对车辆的累计定位误差进行修正。
[0085]
路侧单元对车辆进行测距、测向、测速观测，借助卡尔曼滤波方程对车辆的状态进行估计。
[0086]
本实施例中，根据已建立的误差状态离散时间下的递推方程，获得车辆误差状态的先验估计及其对应的协方差
[0087][0088][0089][0090][0091][0092]
式中，i为3
×
3的单位矩阵；0为3
×
3的零矩阵；qk表示扰动脉冲向量n的协方差矩阵，分别表示速度随机游走和角度随机游走；分别表示加速度计和陀螺仪的动态零偏的功率谱密度。
[0093]
本实施例中，路侧单元与车辆之间的测距、测向、测速作为观测量，观测方程为：
[0094][0095][0096]
式中，z为观测矩阵；分别为路侧单元对车辆的实际距离测量、角度测量、速度测量；分别为车辆到路侧单元之间的计算距离、计算角度和计算速度；w为路测单元观测噪声。
[0097]
根据卡尔曼滤波方程，车辆误差状态的后验估计及其对应的协方差为：
[0098][0099][0100][0101][0102]
式中，i为3
×
3的单位矩阵；分别为路侧单元与车辆之间的测距误
差、测向误差、测速误差服从的概率分布的方差。
[0103]
本实施例中，将滤波后的误差状态估计注入到名义状态估计中，以获得更加准确的车辆定位估计：
[0104][0105][0106][0107]
式中，p
k+1
、v
k+1
分别为车辆名义位置估计和名义速度估计；分别为车辆位置误差后验估计和速度误差后验估计；q
k+1
为车辆名义姿态对应的四元数；算子表示姿态误差后验估计对应的四元数。
[0108]
误差状态注入后进行重置以用于下一个滤波周期的估计：
[0109]
其中，每收到一次路侧单元对车辆的测距、测向、测速信息对应开启一个滤波周期，即当下一次收到路侧单元对车辆的测距、测向、测速信息时，将k+1周期的误差状态重置，计算k+2周期的车辆定位估计。
[0110]
经过多周期的重复计算，获得车辆在离散时间下的实时定位结果。
[0111]
以上对本发明所提供的测距、测向、测速相结合的智能车协同定位方法进行了详细介绍，本实施例中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处，综上，本说明书内容不应理解为对本发明的限制。
[0112]
对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本实施例中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本实施例所示的这些实施例，而是要符合与本实施例所公开的原理和新颖特点相一致的最宽的范围。