CN103033836B - 一种车载导航指向装置的导航指向方法 - Google Patents

Abstract

本发明涉及一种车载导航指向装置，由GNSS模块、加速度传感器模块、图像采集模块、处理模块、存储模块和输出模块组成，GNSS模块用于接收GNSS系统的卫星信号；加速度传感器模块采集MEMS三维加速度传感器的值；图像采集模块通过摄像头采集的当前场景图像帧；处理模块接收GNSS模块输出的导航指向信息、加速度传感器模块输出的MEMS三维加速度传感器的值和图像采集模块输出的当前场景图像帧，并将导航指向信息输入到输出模块。本发明还涉及一种上述车载导航指向装置的导航指向方法。本发明利用车辆前进方向场景图像帧，与历史导航指向信息融合计算，能在遮蔽严重的复杂路况情况下提供持续的定位导航能力。

Description

一种车载导航指向装置的导航指向方法

技术领域

本发明属于导航指向技术领域，涉及一种组合导航指向系统及方法，尤其是一种采用GNSS数据与MEMS三维加速度传感器、摄像头采集的图像场景数据进行数据融合处理得到导航指向信息的车载导航指向装置及方法。 

背景技术

GNSS（全球导航卫星系统）是利用分布于地球附近轨道的卫星星座、地面监测站和用户接收机实现全球导航的系统。目前在用和正在建设的卫星导航系统包括美国的GPS（全球定位系统）、俄罗斯的GLONASS（格洛纳斯）、中国的COMPASS（北斗二代）和欧洲的Galileo(伽利略)系统。 

随着全球卫星定位系统GNSS技术的发展，各种导航终端产品走进千家万户的生活，为人们的生活工作提供了极大的便利。GNSS系统通过接收卫星定位信号，来确定当前设备位置，当GNSS不断更新当前地点信息，每一点的坐标和上一点的坐标进行比较，就可以得到前进的方向。如果卫星定位信号弱或者没有时，如进入隧道或进入小巷等卫星定位信号接收较差的地区，设备将无法定位。而且GNSS没有指北针的功能，当装置原地或GNSS导航误差范围内行进、转向（GNSS判断静止状态）时是不能提供方向、导航信息的。 

现有技术大多利用陀螺仪、加速度传感器来解决辅助定位导航，其中陀螺仪用来测量载体运动过程中的旋转角，从而测量载体的航向，但一般使用的电子陀螺仪有易受电磁干扰的缺点，而机械陀螺仪则体积大、价格贵，不易于实用。 

发明内容

本发明的目的是为了克服现有技术中存在的问题和不足，提供一种能在GNSS信号无效或车辆在GNSS导航误差范围内行进、转向时能有效导航指向的装置及导航指向方法，其系统结构成熟、稳定，在隧道等无卫星信号或车辆在GNSS导航误差范围内行进、转向时，能提供持续的定位航向信息。 

为达到上述目的，解决上述技术问题，本发明所采取的技术方案是：一种车载导航指向装置，由GNSS模块、加速度传感器模块、图像采集模块、处理模块、存储模块和输出模块组成；其中： 

GNSS模块用于接收GNSS系统的卫星信号，处理后输出：位置坐标、时间、速度和航向导航指向信息到处理模块； 

加速度传感器模块用于采集MEMS三维加速度传感器的值，并将所采集的MEMS三维 加速度传感器的值输出到处理模块； 

图像采集模块用于通过摄像头采集的当前场景图像帧输出到处理模块； 

处理模块用于接收GNSS模块输出的导航指向信息、加速度传感器模块输出的MEMS三维加速度传感器的值和图像采集模块输出的当前场景图像帧，将GNSS模块输出的导航指向信息输入到存储模块，并将导航指向信息输入到输出模块； 

存储模块用于储存通过处理模块接收的GNSS模块输出的导航指向信息，包括位置、速度、时间和航向； 

输出模块用于接收处理模块输出的导航指向信息，并在可视化界面显示。 

为解决所述技术问题，本发明提供的另一个技术方案是：一种上述车载导航指向装置的导航指向方法，该导航指向方法的具体步骤为： 

（1）处理模块判断GNSS模块输出导航指向信息是否有效，若导航指向信息有效，则执行步骤（2），反之，则执行步骤（4）； 

（2）处理模块根据GNSS模块输出的导航指向信息获取车辆的位置坐标，如果连续的位置坐标之间的距离小于预设的定位误差容限，则判定车辆在静止状态，执行步骤（4），如果连续的位置坐标之间的距离大于预设的定位误差容限，则判定车辆在运动中，执行步骤（3）； 

（3）处理模块输出GNSS模块的导航指向信息到输出模块，执行步骤（6）； 

（4）处理模块根据图像采集模块采集的当前场景图像帧，对连续的当前场景图像帧进行多区域的块匹配，得到不同区域的局部平移运动矢量；利用刚性运动模型，获得连续的场景图像间的旋转角度；再与存储在存储模块中的历史导航指向信息中最后有效的航向相累加得出当前航向； 

（5）处理模块根据存储模块中历史导航指向信息、推算出的当前航向及前进方向加速度的值，融合处理得出车辆导航指向信息，并输出到输出模块； 

（6）输出模块接收导航指向信息，在可视化界面上显示。 

上述步骤（2）中预设的定位误差容限为5m。 

进一步，上述步骤（5）中的融合处理包括如下步骤： 

（51）处理模块从存储模块中提取历史导航指向信息中最后有效的车辆位置P₀(x₀,y₀)，速度v₀，航向θ₀，从加速度传感器模块输出的MEMS三维加速度传感器的值中提取车辆前进方向加速度； 

（52）车辆在t时刻的更新速度为： 

$v_t=v_{t-1}\cos ({\mathrm{\θ}}_t-{\mathrm{\θ}}_{t-1})+\sqrt{a_{t-1}^2\×{\mathrm{\Δt}}^2-v_{t-1}^2{\sin }^2({\mathrm{\θ}}_t-{\mathrm{\θ}}_{t-1})}$

式中v_t-1是t-1时刻的速度，v_t是t时刻的更新速度；a_t-1是t-1时刻的车辆的前进方向加 速度，θ_t-1和θ_t分别是车辆在t-1时刻和t时刻车辆的航向，Δt是加速度传感器模块测量加速度的时间间隔； 

(53)确定车辆在t时刻的位置P_t： 

P_t＝(x_t，y_t)＝(|OP_t|COSθ_t，OP_t|sinθ_t) 

上式中，|OP_t|为更新位置P_t到初始位置P₀的距离： 

$\left|{\mathrm{OP}}_t\right|=\left|{\mathrm{OP}}_{t-1}\right|\cos ({\mathrm{\θ}}_t-{\mathrm{\θ}}_{t-1})+\sqrt{v_{t-1}^2\×{\mathrm{\Δt}}^2-{\left|{\mathrm{OP}}_{t-1}^2\right|}^2{\sin }^2({\mathrm{\θ}}_t-{\mathrm{\θ}}_{t-1})}$

其中P_t-1是t-1时刻的位置。 

所述加速度传感器模块测量加速度的时间间隔为50ms。 

综上所述，本发明与现有技术相比具有如下特点和有益效果： 

(1)利用图像采集模块采集车辆前进方向场景图像，对连续的场景图像帧进行多区域的块匹配，计算出车辆在丢失GNSS信号或导航误差范围内转向、行进时的航向转角，能在复杂路况情况下提供持续的定位导航能力； 

(2)由于利用目前成熟的图像采集系统，结构成熟、稳定，有效克服现有技术中用陀螺仪来测量载体运动过程中的旋转角而产生的体积大、价格贵，不易于实用的问题； 

(3)将图像采集模块、加速度传感器模块与GNSS模块相结合，克服了GNSS单独使用定位、航向不连续的问题，同时满足了车辆实时定位的需要； 

(4)提高复杂环境如城市高楼峡谷、树叶茂密等信号遮挡严重区域的定位精度问题和持续准确导航问题； 

(5)系统结构简单，成本低。 

附图说明

图1是本发明的一种车载导航指向装置构成的模块图； 

图2是本发明车载导航指向装置采集场景图像多区域块匹配示意图； 

图3是为车辆行驶路径的俯瞰图； 

图4是本发明的一种车载导航指向装置的导航指向方法流程示意图。。 

具体实施方式

下面结合附图和实施方式对本发明作进一步说明。 

如图1所示，为本发明的一种车载导航指向装置构成的模块图，由GNSS模块1、加速度传感器模块2、图像采集模块3、处理模块4、存储模块5和输出模块6组成；其中： 

GNSS模块1接收GNSS系统的卫星信号，处理后输出导航指向信息包括：位置坐标、时间、速度和航向到处理模块； 

加速度传感器模块采集MEMS三维加速度传感器的值，并将所采集的三个坐标轴方向上的加速度的值输出到处理模块，在本例中，加速度传感器使用了标准的传感器坐标系，该装置竖直安装在车辆上，z轴方向为车辆的前进方向，y轴方向竖直。当车辆前进时，z轴加速度的值为车辆的前进加速度的值； 

图像采集模块通过摄像头每隔100ms采集一帧当前场景图像，然后将所采集的图像输出到处理模块，在本例中摄像头安装在装置的背面能够拍摄到车辆前进方向的场景； 

存储模块通过处理模块储存GNSS模块输出的导航指向信息，包括位置、速度、时间和航向信息； 

输出模块根据通讯协议接收处理模块输出的导航指向信息，在可视化界面显示当前前进方向、位置及速度； 

处理模块接收GNSS模块输出的导航指向信息、加速度传感器模块输出的MEMS三维加速度传感器所采集的三个坐标轴方向上的加速度值和图像采集模块输出的场景图像，同时将GNSS模块输出的导航指向数据存储到存储模块，计算连续3个位置坐标之间的距离，并设定定位误差容限为5m。处理模块根据GNSS模块输出的导航指向信息和连续位置坐标之间距离信息，决策选择GNSS模块的输出的导航指向数据或者将多数据融合处理的信息作为导航指向信息，具体如下： 

(1)当GNSS模块输出的导航指向信息无效或连续3个位置坐标之间的距离小于预设的定位误差容限情况下：处理模块根据图像采集模块采集的连续场景图像帧，对连续的场景图像帧进行多区域的块匹配，图2所示为本发明车载导航指向装置采集场景图像多区域块匹配示意图，在当前场景图像上设定9个匹配区域，分别在上一帧场景图像同样位置搜索匹配区域，以确定9个待匹配的局部平移运动矢量，即ΔX_i，ΔY_i(i=1,2，…，9)；两帧图像之间的运动满足刚性运动模型，即: 

$\left[\begin{array}{c}{X}_c\\ Y_c\end{array}\right]=S\left[\begin{array}{cc}\cos \mathrm{\θ}& -\sin \mathrm{\θ}\\ \sin \mathrm{\θ}& \cos \mathrm{\θ}\end{array}\right]\left[\begin{array}{c}{X}_P\\ Y_P\end{array}\right]+\left[\begin{array}{c}\mathrm{\ΔX}\\ \mathrm{\ΔY}\end{array}\right]$ ① 

其中X_c、X_p分别为当前场景图像帧和上一场景图像帧的横坐标，Y_c、Y_p分别为当前场景图像帧和上一场景图像帧的纵坐标，ΔX、ΔY为两帧间的平移量，θ为两帧间旋转角度，S为图像的缩放因子； 

选择在同一水平线上的两个匹配区域1区和3区，对于两个区域的中心点有 

$\left[\begin{array}{c}{X}_{1^{\′}}\\ Y_{1^{\′}}\end{array}\right]=S\left[\begin{array}{cc}\cos \mathrm{\θ}& -\sin \mathrm{\θ}\\ \sin \mathrm{\θ}& \cos \mathrm{\θ}\end{array}\right]\left[\begin{array}{c}{X}_1\\ Y_1\end{array}\right]+\left[\begin{array}{c}\mathrm{\ΔX}\\ \mathrm{\ΔY}\end{array}\right]$ ② 

和 

$\left[\begin{array}{c}{X}_{3^{\′}}\\ Y_{3^{\′}}\end{array}\right]=S\left[\begin{array}{cc}\cos \mathrm{\θ}& -\sin \mathrm{\θ}\\ \sin \mathrm{\θ}& \cos \mathrm{\θ}\end{array}\right]\left[\begin{array}{c}{X}_3\\ Y_3\end{array}\right]+\left[\begin{array}{c}\mathrm{\ΔX}\\ \mathrm{\ΔY}\end{array}\right]$ ③ 

由于两帧之间时间间隔很小，可以忽略缩放的影响，即认为S＝1；同时由于两帧之间的旋转角度较小，则cosθ≈1，sinθ≈0。将②和⑧相减，得到 

$\left[\begin{array}{c}{X}_{1^{\′}}-X_{3^{\′}}\\ Y_{1^{\′}}-Y_{3^{\′}}\end{array}\right]=\left[\begin{array}{cc}1& \mathrm{\θ}\\ \mathrm{\θ}& 1\end{array}\right]\left[\begin{array}{c}{X}_1-X_3\\ Y_1-Y_3\end{array}\right]$ ④ 

由于Y₁＝Y₃，故两帧之间的旋转角度为 

${\mathrm{\θ}}_1=\frac{Y_{1^{\′}}-Y_{3^{\′}}}{x_1-x_3}=\frac{{\mathrm{\ΔY}}_1-{\mathrm{\ΔY}}_3}{X_1-X_3}$ ⑤ 

同样，选择处于同一垂直线上的两个区域2区和8区，也可得到两帧间的旋转角度为 

${\mathrm{\θ}}_2=-\frac{{\mathrm{\ΔX}}_2-{\mathrm{\ΔX}}_8}{Y_2-Y_8}$ ⑥ 

选择不同区域，得到12组旋转角度，即θ_i(i＝1，2，…，12)；最终的旋转角度为 

$\mathrm{\θ}=[{\mathrm{\Σ}}_{i=1}^{12}{\mathrm{\θ}}_i-\max \left({\mathrm{\θ}}_i\right)-\min \left({\mathrm{\θ}}_i\right)]/10$

从历史导航指向信息中提取最后有效的航向θ₀，则车辆当前的航向为θ₀+θ。 

图3所示为车辆行驶路径的俯瞰图，处理模块从历史导航指向信息中提取最后有效的导航指向位置即GNSS信号最后一次正常工作时车辆位于位置P₀(x₀，y₀)，速度y₀，航向θ₀；处理模块接收加速度传感器输出的车辆在三个坐标轴方向上的加速度，获取车辆在前进方向的加速度，在t-1时刻车辆位于位置P_t-1，此时运行方向为θ_t-1，而在t时刻车辆处于位置P_t，运行方向为θ_t。 

车辆在t时刻的更新速度为： 

$v_t=v_{t-1}\cos ({\mathrm{\θ}}_t-{\mathrm{\θ}}_{t-1})+\sqrt{a_{t-1}^2\×{\mathrm{\Δt}}^2-v_{t-1}^2{\sin }^2({\mathrm{\θ}}_t-{\mathrm{\θ}}_{t-1})}$

其中v_t-1是t-1时刻的速度，v_t是t时刻的更新速度；a_t-1是t-1时刻的车辆的前进加速度，θ_t-1和θ_t分别是车辆在t-1时刻和t时刻车辆的航向，Δt是加速度传感器模块测量加速度的时间间隔，本例设为50ms； 

确定车辆的位置： 

$\left|{\mathrm{OP}}_t\right|=\left|{\mathrm{OP}}_{t-1}\right|\cos ({\mathrm{\θ}}_t-{\mathrm{\θ}}_{t-1})+\sqrt{v_{t-1}^2\×{\mathrm{\Δt}}^2-{\left|{\mathrm{OP}}_{t-1}^2\right|}^2{\sin }^2({\mathrm{\θ}}_t-{\mathrm{\θ}}_{t-1})}$

其中P_t-1是t-1时刻的位置，|OP_t|是更新位置P_t到初始位置P₀的距离； 

P_t＝(x_t，y_t)＝(|OP_t|cosθ_t，OP_t|sinθ_t) 

其中P_t是t时刻的位置； 

将t时刻的更新速度v_t和更新位置P_t(x_t，y_t)和更新航向θ_t＝θ₀+θ作为导航指向信息输出到输出模块；输出模块根据通讯协议接收导航指向信息，并在可视化界面上显示。 

（2）当GNSS模块输出的导航指向信息有效且连续位置坐标之间距离大于定位误差容限的情况下：处理模块直接输出GNSS模块的导航指向数据到输出模块；输出模块根据通讯协议接收导航指向信息，并在可视化界面上显示。 

以下给出一种上述车载导航指向装置的导航指向方法的具体步骤： 

（1）处理模块判断GNSS模块输出导航指向信息是否有效，若导航指向信息有效，则执行步骤（2），反之，则执行步骤（4）； 

（2）处理模块根据GNSS模块输出的导航指向信息获取车辆的位置坐标，如果连续的位置坐标之间的距离小于预设的定位误差容限，则判定车辆在静止状态，执行步骤（4），如果连续的位置坐标之间的距离大于预设的定位误差容限，则判定车辆在运动中，执行步骤（3）； 

（3）处理模块输出GNSS模块的导航指向信息到输出模块，执行步骤（6）； 

（4）处理模块根据图像采集模块采集的当前场景图像帧，对连续的当前场景图像帧进行多区域的块匹配，得到不同区域的局部平移运动矢量；利用刚性运动模型，获得连续的场景图像间的旋转角度；再与存储在存储模块中的历史导航指向信息中最后有效的航向相累加得出当前航向； 

（5）处理模块根据存储模块中历史导航指向信息、推算出的当前航向及前进方向加速度的值，融合处理得出车辆导航指向信息，并输出到输出模块； 

（6）输出模块接收导航指向信息，在可视化界面上显示。 

上述步骤（2）中预设的定位误差容限为5m。 

上述步骤（5）中的融合处理包括如下步骤： 

（51）处理模块从存储模块中提取历史导航指向信息中最后有效的车辆位置P₀(x₀,y₀)，速度v₀，航向θ₀，从加速度传感器模块输出的MEMS三维加速度传感器的值中提取车辆前进方向加速度； 

（52）车辆在t时刻的更新速度为： 

$v_t=v_{t-1}\cos ({\mathrm{\θ}}_t-{\mathrm{\θ}}_{t-1})+\sqrt{a_{t-1}^2\×{\mathrm{\Δt}}^2-v_{t-1}^2{\sin }^2({\mathrm{\θ}}_t-{\mathrm{\θ}}_{t-1})}$

式中v_t-1是t-1时刻的速度，v_t是t时刻的更新速度；a_t-1是t-1时刻的车辆的前进方向加速度，θ_t-1和θ_t分别是车辆在t-1时刻和t时刻车辆的航向，Δt是加速度传感器模块测量加速度的时间间隔； 

（53）确定车辆在t时刻的位置P_t： 

P_t=(x_t，y_t)=(/OP_t/cosθ_t,OP_t/sinθ_t) 

上式中，|OP_t|为更新位置P_t到初始位置P₀的距离： 

$\left|{\mathrm{OP}}_t\right|=\left|{\mathrm{OP}}_{t-1}\right|\cos ({\mathrm{\θ}}_t-{\mathrm{\θ}}_{t-1})+\sqrt{v_{t-1}^2\×{\mathrm{\Δt}}^2-{\left|{\mathrm{OP}}_{t-1}^2\right|}^2{\sin }^2({\mathrm{\θ}}_t-{\mathrm{\θ}}_{t-1})}$

其中P_t-1是t-1时刻的位置。 

所述加速度传感器模块测量加速度的时间间隔为50ms。 

Claims (4)

1.一种车载导航指向装置的导航指向方法，该方法基于的装置由GNSS模块、加速度传感器模块、图像采集模块、处理模块、存储模块和输出模块组成； 

其中，GNSS模块用于接收GNSS系统的卫星信号，处理后输出：位置坐标、时间、速度和航向导航指向信息到处理模块； 

加速度传感器模块用于采集MEMS三维加速度传感器的值，并将所采集的MEMS三维加速度传感器的值输出到处理模块； 

图像采集模块用于通过摄像头采集的当前场景图像帧输出到处理模块； 

处理模块用于接收GNSS模块输出的导航指向信息、加速度传感器模块输出的MEMS三维加速度传感器的值和图像采集模块输出的当前场景图像帧，将GNSS模块输出的导航指向信息输入到存储模块，并将导航指向信息输入到输出模块； 

存储模块用于储存通过处理模块接收的GNSS模块输出的导航指向信息，包括位置、速度、时间和航向； 

输出模块用于接收处理模块输出的导航指向信息，并在可视化界面显示； 

其特征在于，所述导航指向方法包括如下步骤： 

（1）处理模块判断GNSS模块输出导航指向信息是否有效，若导航指向信息有效，则执行步骤（2），反之，则执行步骤（4）； 

（2）处理模块根据GNSS模块输出的导航指向信息获取车辆的位置坐标，如果连续的位置坐标之间的距离小于预设的定位误差容限，则判定车辆在静止状态，执行步骤（4），如果连续的位置坐标之间的距离大于预设的定位误差容限，则判定车辆在运动中，执行步骤（3）； 

（3）处理模块输出GNSS模块的导航指向信息到输出模块，执行步骤（6）； 

（4）处理模块根据图像采集模块采集的当前场景图像帧，对连续的当前场景图像帧进行多区域的块匹配，得到不同区域的局部平移运动矢量；利用刚性运动模型，获得连续的场景图像间的旋转角度；再与存储在存储模块中的历史导航指向信息中最后有效的航向相累加得出当前航向； 

（5）处理模块根据存储模块中历史导航指向信息、推算出的当前航向及前进方向加速度的值，融合处理得出车辆导航指向信息，并输出到输出模块； 

（6）输出模块接收导航指向信息，在可视化界面上显示。 

2.根据权利要求1所述的导航指向方法，其特征在于所述预设的定位误差容限为5m。 

3.根据权利要求1所述的导航指向方法，其特征在于，步骤（5）中所述的融合处理包括如下步骤： 

（51）处理模块从存储模块中提取历史导航指向信息中最后有效的车辆位置P₀(x₀,y₀)，速度v₀，航向θ₀，从加速度传感器模块输出的MEMS三维加速度传感器的值中提取车辆前进方向加速度； 

（52）车辆在t时刻的更新速度为： 

式中v_t-1是t-1时刻的速度，v_t是t时刻的更新速度；a_t-1是t-1时刻的车辆的前进方向加速度，θ_t-1和θ_t分别是车辆在t-1时刻和t时刻车辆的航向，Δt是加速度传感器模块测量加速度的时间间隔； 

（53）确定车辆在t时刻的位置P_t： 

P_t=(xt,yt)=(|OP_t|cosθ_t,|OP_t|sinθ_t) 

上式中，|OP_t|为更新位置P_t到初始位置P_o的距离 

其中P_t-1是t-1时刻的位置。 

4.根据权利要求3所述的导航指向方法，其特征在于，所述加速度传感器模块测量加速度的时间间隔为50ms。 

Images