一种基于碰撞预警算法的辅助驾驶系统

摘要

本发明涉及一种基于碰撞预警算法的辅助驾驶系统，属于计算机视觉和智能辅助驾驶技术领域。该系统包括：检测与测距模块，通过摄像头采集汽车行驶过程中的路况信息，使用YOLOv3模型对障碍物进行检测、识别和距离测量；碰撞预警模块，进行碰撞预测分类，并计算出发生碰撞需要的时间及时给出预警判断并对驾驶员进行预警播报；定位模块：利用GPS/IMU组合导航对车辆的行驶位置信息进行采集，当GPS信号缺失时系统自动转换为IMU进行定位，当GPS信号正常时重新转为GPS定位；GUI显示与云端视频备份模块，对识别视频流、行驶状态及地图软件标注信息进行实时显示并进行云端备份。本发明能够提高辅助驾驶系统的预测精度及实时性。

权利要求

1.一种基于碰撞预警算法的辅助驾驶系统，其特征在于，该系统包括：

检测与测距模块：使用YOLOv3模型实现车辆前方行人和车的检测，通过基于单目视觉的车辆车距测量方法实现前方所有障碍物的距离测量；

碰撞预警模块：利用GPS检测的车辆位置坐标、车辆速度和YOLOv3模型检测的前方障碍物及车辆距离，分类预测发生碰撞需要的时间；通过考虑驾驶员反应时间及制动车辆所需时间得出安全防撞时间，当预测碰撞时间小于安全防撞时间，语音播报及时预警；

定位模块：利用GPS/IMU组合导航方式对车辆的行驶位置信息进行采集；通过对GPS信号的提取显示出用户在地图上的位置信息，当GPS信号缺失时，系统将自动转换为IMU对车辆进行定位，当GPS信号正常时重新转为GPS进行定位；

GUI显示与云端视频备份模块：利用摄像头采集视频显示区域对当前车辆前方的路况信息进行实时显示并结合导航系统的路况信息数据，对车辆实时位置在地图软件上进行标注，进行可视化展示。

2.根据权利要求1所述的一种基于碰撞预警算法的辅助驾驶系统，其特征在于，所述检测与测距模块中，车辆前方行人与车辆的检测与测距具体包括以下步骤：S11：将单目测距算法编写入YOLOv3模型中，训练好模型的权重及其他参数并将单目测距算法编写入模型中；

S12：在车载端搭载高清单目摄像头，采用单目测距的方法实现前方车辆和行人的检测，摄像头实时地采集车辆前方视频流信息；

S13：将采集的实时视频流上传到YOLOv3模型中，输出实时的障碍物检测与测距信息。

3.根据权利要求1所述的一种基于碰撞预警算法的辅助驾驶系统，其特征在于，所述碰撞预警模块中，利用YOLOv3检测的前方障碍物及车辆距离和GPS检测的车辆位置坐标、车辆速度，分类预测发生碰撞需要的时间，并及时给出预警提示，具体包括以下步骤：S21：通过基于单目摄像头的YOLOv3目标及距离检测，检测出车辆前方的行人及车辆的实时距离，计算出目标车辆的速度v1，v2；

S22：根据两车之间的角度关系进行碰撞情况的分类：设 分别为两辆车各自行驶方向与两车连线的夹角；

若 表示两车为平行且相向行驶，可能存在正面碰撞；

若 表示两车为平行且同相行驶，可能存在追尾碰撞；

若 且 与 为异号，表示两车为同侧且相向行驶，可能存在侧面碰撞；

S23：车辆碰撞分类完成后分别对不同的分类情况进行相对应的计算处理，进而预测碰撞时间；设车身宽为L，两车之间的距离为l；

(1)正面碰撞：当检测到 时，进行预测碰撞时间和安全防撞时间比较；

(2)追尾碰撞：当检测到 时，进行预测碰撞时间和安全防撞时间比较；

(3)侧面碰撞：在两车维持当前车速及行驶方向不变情况下，依据当前运动状态预测车辆未来运行轨迹，确定碰撞点及车辆碰撞时间，计算两车距离碰撞点的时间差进行判断是否存在碰撞危险；

S24：计算安全防撞时间及预警判断，系统通过车载设备实时接收车辆状态信息，当满足直线碰撞和侧面碰撞的前提条件时，实时计算车辆发生碰撞需要的时间与安全防撞时间进行比较，存在碰撞危险则上进行预警语言提醒。

4.根据权利要求1所述的一种基于碰撞预警算法的辅助驾驶系统，其特征在于，所述定位模块中，将系统状态方程和GPS/IMU组合导航的量测方程相结合得到高精度的实时定位导航系统，采集目标车辆的GPS信号，具体包括以下步骤：S31：采用导航坐标系为东北天地理坐标系，车载GPS/IMU组合导航系统的导航参数的误差模型参数矩阵X(t)为：其中，δL,δλ,δh分别为纬度误差、经度误差和高度误差；δvE,δvN,δvU分别为载体的东向、北向和天向速度误差；θE,θN,θU为数学平台矢准角；ψx,ψy,ψz, 分别为陀螺随机常值漂移和加速度计随机常值零偏；

系统状态方程为：

其中，F(t)为系统的状态转移矩阵，G(t)为系统的噪声方差矩阵；

S32：选取IMU与GPS输出的位置之差和速度之差作为量测量，得出速度量测矢量Zυ(t)和位置量测矢量ZS(t)分别为：上述量测矢量中VS(t)＝[PGE PGN PGU]，Vv(t)＝[VGE VGN VGU]，HS＝[03×6 diag(RM RNcosL 1) 03×6]

Hv＝[03×6 diag(1 1 1) 03×6]；

其中，下标I表示IMU，下标G表示GPS；LGE,λGN,hGU分别为GPS沿东北天方向的纬度、经度、高度，PGE,PGN,PGU为GPS沿东北天方向的位置误差，VGE,VGN,VGU为GPS沿东北天方向的速度误差，RM、RN分别为载体所在子午圈和卯酉圈曲率半径；

S33：联合位置量测矢量方程和速度测量方程得出GPS/IMU组合导航的量测方程Z(t)为：

5.根据权利要求1所述的一种基于碰撞预警算法的辅助驾驶系统，其特征在于，所述GUI显示与云端视频备份模块中，GUI界面显示与联网环境下视频流的云备份具体包括以下步骤：S41：利用车载端搭载的摄像头对车辆行驶期间正前方的路况信息进行采集，对采集得到的视频经过YOLOv3深度学习算法预处理，得到包含检测信息和测距信息的视频流；

S42：通过车载微型计算机对处理好的视频流进行云备份，检测与测距模块对汽车行驶前方的障碍物进行识别检测，并将结果传输到GUI显示终端，结合导航系统的路况信息数据，对车辆实时位置在地图软件上进行标注，进行可视化展示。

说明书

一种基于碰撞预警算法的辅助驾驶系统

技术领域

[0001] 本发明属于计算机视觉和智能辅助驾驶技术领域，涉及一种基于碰撞预警算法的辅助驾驶系统。

背景技术

[0002] 近年来随着我国经济的飞速发展，公路和高速公路里程数不断刷新记录，人们在享受汽车带来便捷的同时，交通事故的发生数量也在显著增加，为了能减少交通事故的发生，开发更先进的汽车安全辅助驾驶系统，及时提醒驾驶人或主动接管汽车的部分功能，能够有效避免交通事故的发生。

[0003] 在辅助驾驶系统的应用研究中，如何提高碰撞预警算法的准确率一直是其中难点。当前文献中已提出许多行之有效的基于深度学习的自然语句理解模型来解决该问题，其中就有基于眼电图的安全辅助驾驶系统、基于车路协同的特种车辆辅助驾驶系统、基于图像识别的信号灯路口辅助驾驶方法、一种雾天高速公路辅助驾驶系统等等，这些模型都在不同的应用场景下取得了良好的实验效果。然而这些辅助驾驶系统目前都存在着相应的缺陷，主要体现在以下方面：其一是现有的基于不同场景下的辅助驾驶系统欠缺具体的碰撞预警算法；其二是多数辅助驾驶系统都将重心放在了不同的场景应用上面，忽视了其中GPS信号实时获取与信号丢失该如何处理的问题；其三是现有的大多数辅助驾驶系统忽略了在人机交互中GUI显示的流畅性应用。

[0004] 综上所述，虽然基于计算机视觉技术的辅助驾驶系统在智能辅助驾驶领域取得了一定程度的研究进展，但如何降低外界环境因素对检测效果的干扰，提升系统对目标的检测精度；如何更高的检测率和实时性能极大地提升辅助驾驶性能，提升驾驶安全性能，降低行车事故发生；如何解决车辆行驶过程中进入隧道时丢失GPS信号的问题；如何拓展辅助驾驶系统的应用范围等都是需要继续深究和实践的课题。

发明内容

[0005] 有鉴于此，本发明的目的在于提供一种基于碰撞预警算法的辅助驾驶系统，用于提高辅助驾驶系统的的检测精度，并解决车辆行驶过程中进入隧道时丢失GPS信号的问题。

[0006] 为达到上述目的，本发明提供如下技术方案：

[0007] 一种基于碰撞预警算法的辅助驾驶系统，包括：

[0008] 检测与测距模块：使用YOLOv3模型实现车辆前方行人和车等物体的检测，通过基于单目视觉的车辆车距测量方法实现前方所有障碍物的距离测量；

[0009] 碰撞预警模块：利用GPS检测的车辆位置坐标、车辆速度和YOLOv3模型检测的前方障碍物及车辆距离，分类预测发生碰撞需要的时间；通过考虑驾驶员反应时间及制动车辆所需时间得出安全防撞时间，当预测碰撞时间小于安全防撞时间，语音播报及时预警；

[0010] 定位模块：利用GPS/IMU组合导航方式对车辆的行驶位置信息进行采集；通过对GPS信号的提取显示出用户在地图上的位置信息，当GPS信号缺失时，系统将自动转换为惯性导航(IMU)对车辆进行定位，当GPS信号正常时重新转为GPS进行定位；

[0011] GUI显示与云端视频备份模块：利用摄像头采集视频显示区域对当前车辆前方的路况信息进行实时显示并结合导航系统的路况信息数据，对车辆实时位置在地图软件上进行标注，进行可视化展示。

[0012] 进一步，所述检测与测距模块中，车辆前方行人与车辆的检测与测距具体包括以下步骤：

[0013] S11：将单目测距算法编写入YOLOv3模型中，训练好模型的权重及其他参数并将单目测距算法编写入模型中；

[0014] S12：在车载端搭载高清单目摄像头，采用单目测距的方法实现前方车辆和行人等的检测，摄像头实时地采集车辆前方视频流信息；

[0015] S13：将采集的实时视频流上传到YOLOv3模型中，输出实时的障碍物检测与测距信息，为后续碰撞预警及GUI显示提供支撑。

[0016] 进一步，所述碰撞预警模块中，利用YOLOv3检测的前方障碍物及车辆距离和GPS检测的车辆位置坐标、车辆速度，分类预测发生碰撞需要的时间，并及时给出预警提示，具体包括以下步骤：

[0017] S21：通过基于单目摄像头的YOLOv3目标及距离检测，检测出车辆前方的行人及车辆的实时距离，计算出目标车辆的速度v1，v2；

[0018] S22：根据两车之间的角度关系进行碰撞情况的分类：设 分别为两辆车各自行驶方向与两车连线的夹角；

[0019] 若 表示两车为平行且相向行驶，可能存在正面碰撞；

[0020] 若 表示两车为平行且同相行驶，可能存在追尾碰撞；

[0021] 若 且 与 为异号，表示两车为同侧且相向行驶，可能存在侧面碰撞；

[0022] S23：车辆碰撞分类完成后分别对不同的分类情况进行相对应的计算处理，进而预测碰撞时间；设车身宽为L，两车之间的距离为l。

[0023] (1)正面碰撞：当检测到 时，进行预测碰撞时间和安全防撞时间比较；

[0024] (2)追尾碰撞：当检测到 时，进行预测碰撞时间和安全防撞时间比较；

[0025] (3)侧面碰撞：在两车维持当前车速及行驶方向不变情况下，依据当前运动状态预测车辆未来运行轨迹，确定碰撞点及车辆碰撞时间，计算两车距离碰撞点的时间差进行判断是否存在碰撞危险；

[0026] S24：计算安全防撞时间及预警判断，系统通过车载设备实时接收车辆状态信息，当满足直线碰撞和侧面碰撞的前提条件时，实时计算车辆发生碰撞需要的时间与安全防撞时间进行比较，存在碰撞危险则上进行预警语言提醒。

[0027] 进一步，所述定位模块中，将系统状态方程和GPS/IMU组合导航的量测方程相结合得到高精度的实时定位导航系统，采集目标车辆的GPS信号，具体包括以下步骤：

[0028] S31：采用导航坐标系为东北天地理坐标系，车载GPS/IMU组合导航系统的导航参数的误差模型参数矩阵X(t)为：

[0029] X(t)＝[θE,θN,θU,δL,δλ,δh,δυE,δυN,δυU,ψx,ψy,ψz,▽x,▽y,▽z]T[0030] 其中，δL,δλ,δh分别为纬度误差、经度误差和高度误差；δvE,δvN,δvU分别为载体的东向、北向和天向速度误差；θE,θN,θU为数学平台矢准角；ψx,ψy,ψz,▽x,▽y,▽z分别为陀螺随机常值漂移和加速度计随机常值零偏；

[0031] 系统状态方程为：

[0032]

[0033] 其中，F(t)为系统的状态转移矩阵，G(t)为系统的噪声方差矩阵。

[0034] S32：选取IMU与GPS输出的位置之差和速度之差作为量测量，得出速度量测矢量Zυ(t)和位置量测矢量ZS(t)分别为：

[0035]

[0036]

[0037] 上述量测矢量中VS(t)＝[PGE PGN PGU]，Vv(t)＝[VGE VGN VGU]，

[0038] HS＝[03×6 diag(RM RNcos L 1) 03×6]

[0039] Hv＝[03×6 diag(1 1 1) 03×6]；

[0040] 其中，下标I表示IMU，下标G表示GPS；LGE,λGN,hGU分别为GPS沿东北天方向的纬度、经度、高度，PGE,PGN,PGU为GPS沿东北天方向的位置误差，VGE,VGN,VGU为GPS沿东北天方向的速度误差，RM、RN分别为载体所在子午圈和卯酉圈曲率半径；

[0041] S33：联合位置量测矢量方程和速度测量方程得出GPS/IMU组合导航的量测方程Z(t)为：

[0042]

[0043] 进一步，所述GUI显示与云端视频备份模块中，GUI界面显示与联网环境下视频流的云备份具体包括以下步骤：

[0044] S41：利用车载端搭载的摄像头对车辆行驶期间正前方的路况信息进行采集，对采集得到的视频经过YOLOv3深度学习算法预处理，得到包含检测信息和测距信息的视频流；

[0045] S42：通过车载微型计算机TX2可联网的特性对处理好的视频流进行云备份，检测与测距模块对汽车行驶前方的车辆、行人等障碍物进行识别检测，并将结果传输到GUI显示终端，结合导航系统的路况信息数据，对车辆实时位置在地图软件上进行标注，进行可视化展示。

[0046] 本发明的有益效果在于：

[0047] (1)本发明采用YOLOv3检测模型的精准性高，使目标检测更准确，平台运用更广泛。本发明选取精准性高的YOLOv3模型对车辆、行人、自行车等进行实时检测，依托深度学习技术，选取合适的卷积神经网络模型，降低外界环境因素对检测效果的干扰，提升系统对目标的检测精度。检测算法实时性高，少量分类的检测精度更高，标注更加容易，通过模型修剪、压缩等技术，能实现更高的实时性，能在更广泛的平台上搭建模型与使用。

[0048] (2)本发明利用了碰撞预警算法的合理性，使预警率更高，系统性能更稳定。本发明使用的基于行驶车速的车辆防撞时间预警算法误报率较低，预警时机适宜率高，基本能够达到工业级的应用标准。同时防碰撞预警作为辅助驾驶系统的一大分支，更高的检测率和实时性能极大地提升辅助驾驶性能，提升驾驶安全性能，降低行车事故发生。另外可依托此技术，开发诸如盲区变道、弯道超速预警、红绿灯路口预警、交通路口调度规划、行人预警等技术。

[0049] (3)本发明利用了隧道GPS丢失预测算法的前沿性，使车辆定位更完整准确。本发明针对车辆行驶进隧道时GPS会发生丢失的问题，算法利用IMU模块从GPS丢失时刻开始采集车辆速度、加速度和姿态角，算出车辆在隧道中行驶时的相对位置，预测车辆GPS数据。把GPS技术和惯性技术结合起来可以得到高精度的导航系统，从而实现较为准确的实时定位。

[0050] (4)本发明采用了GUI显示的便利性，使人机交互更流畅，后续开发更便捷。本发明采用GUI实时显示，为后续的开发及在各行业的应用提供便利。GUI是一种用户交互界面，通过设置GUI显示，可为以后无人驾驶远程操控技术以及其他可扩展功能技术提供支持，同时该项技术仍然可以广泛应用在全地形车自动驾驶、军用靶车、沙滩车以及无人码头等众多场合的远程操控上，带来更可观的经济效益。

[0051] (5)本发明具有云端视频流备份功能，可在4G通信良好的状态下，将本地保存的视频流数据上传到云端。当车辆发生事故时，云端备份的视频流作为保险赔偿相关依据。

[0052] 本发明的其他优点、目标和特征在某种程度上将在随后的说明书中进行阐述，并且在某种程度上，基于对下文的考察研究对本领域技术人员而言将是显而易见的，或者可以从本发明的实践中得到教导。本发明的目标和其他优点可以通过下面的说明书来实现和获得。

附图说明

[0053] 为了使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明作优选的详细描述，其中：

[0054] 图1为本发明提供的辅助驾驶系统的框架图；

[0055] 图2为本发明提供的碰撞预警算法的碰撞分类的模型示意图；

[0056] 图3为本发明提供的GPS/IMU组合导航方法实施流程图；

[0057] 图4为本实施例提供的GUI显示终端效果图。

具体实施方式

[0058] 以下通过特定的具体实例说明本发明的实施方式，本领域技术人员可由本说明书所揭露的内容轻易地了解本发明的其他优点与功效。本发明还可以通过另外不同的具体实施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离本发明的精神下进行各种修饰或改变。需要说明的是，以下实施例中所提供的图示仅以示意方式说明本发明的基本构想，在不冲突的情况下，以下实施例及实施例中的特征可以相互组合。

[0059] 如图1所示，本发明所述的基于碰撞预警算法的辅助驾驶系统的控制方法为：检测模块通过单目摄像头采集汽车行驶过程中的路况信息，在YOLOv3模型中编写测距代码实现前方障碍物的距离测量；检测车与车(人)之间的相对坐标及速度，按照两车之间的角度关系进行碰撞分类，从而预测碰撞时间，计算出安全防撞时间及给出预警提示；将系统状态方程和GPS/IMU组合导航的量测方程与导航参数误差模型相结合，得到高精度的导航系统，再根据位置采集信息在百度地图进行标注，从而实现较为准确的实时定位；最后在GUI显示模块上对识别视频流、行驶状态信息及百度地图标注信息进行实时显示并进行云端备份。

[0060] 具体实施例如下：

[0061] 步骤1：将YOLOv3模型结构cfg文件转换成caffe格式文件，然后导入到caffe官方网站可以得到模型架构，通过不断地训练，使得误差函数收敛，最终得到最优的训练权重。然后训练好模型的权重及其他参数，并将单目测距算法编写入模型中，通过车载端高清摄像头采集实时图像并上传到TX2工业板上的YOLOv3模型中，就可以输出实时的障碍物检测与测距信息；

[0062] 步骤2：在车载端搭载高清单目摄像头，采用单目测距的方法实现前方车辆和行人等的检测，摄像头实时地采集车辆前方视频流信息，并将视频流输入到模型中，最终得到前方障碍物类别信息、定位信息及距离信息。

[0063] 步骤201：在车载端搭载高清单目摄像头，采用单目测距的方法实现前方车辆和行人等的检测，摄像头实时地采集车辆前方视频流信息，并将视频流输入到模型中，最终得到前方障碍物类别信息、定位信息及距离信息。

[0064] 步骤202：在YOLOv3模型中编写测距代码，实现了前方障碍物的距离测量。试验结果表明，该方法的车距测量相对误差小于3％，具备了较高的检测精度。车距测量结果只与图像中的近视场点到摄像机的实际距离有关，无需对所有的摄像机参数进行标定，从而解决了单目视觉车距测量问题。

[0065] 步骤3：检测车与车(人)之间的相对坐标及速度，按照两车之间的角度关系进行碰撞分类，从而预测碰撞时间，计算出安全防撞时间及给出预警提示。具体步骤为：

[0066] 步骤301：通过基于单目摄像头的YOLOv3目标及距离检测，可以检测出车辆前方的行人及车辆的实时距离，计算出目标车辆的速度v1，v2；

[0067] 步骤302：根据两车之间的角度关系进行碰撞情况的分类。设 分别为两辆车各自行驶方向与两车连线的夹角；

[0068] 若 表示两车为平行且相向行驶，可能存在正面碰撞；

[0069] 若 表示两车为平行且同相行驶，可能存在追尾碰撞；

[0070] 若 且 与 为异号，表示两车为同侧且相向行驶，可能存在侧面碰撞；

[0071] 步骤303：车辆碰撞分类完成后分别对不同的分类情况进行相对应的计算处理，进而预测碰撞时间，如图2所示，设两车之间的距离为l(m)，两车速度分别为v1(km/h)，v2(km/h)。

[0072] (1)正面碰撞：设车身宽为L(m)，当检测到 时，进行预测碰撞时间和安全防撞时间比较。

[0073] 当满足 且 时，预测碰撞时间为

[0074]

[0075] (2)追尾碰撞：设车身宽为L(m)，当检测到 时，进行预测碰撞时间和安全防撞时间比较。

[0076] 当v1＜v2且 时，预测碰撞时间为 此时为追尾；

[0077] 当v1＞v2且 时，预测碰撞时间为 此时为被追尾；

[0078] (3)侧面碰撞：在两车维持当前车速及行驶方向不变情况下，依据当前运动状态预测车辆未来运行轨迹，确定碰撞点及车辆碰撞时间，计算两车距离碰撞点的时间差进行判断是否存在碰撞危险。

[0079] 若设定自车与碰撞点的距离为S1，他车与碰撞点的距离为S2，

[0080] 则当 时，

[0081] 当 时，

[0082] 当 时，

[0083]

[0084] 可得自车到达碰撞点的时间为t1＝3.6×(S1-2.5)/v1；

[0085] 他车到达碰撞点所需的时间为t2＝3.6×(S2-2.5)/v2；

[0086] 两车距离碰撞点的时间差Δt≥|t1-t2|，自车到达碰撞点的时间为t1，他车到达碰撞点所需的时间为t2。若设定Δt为2s，则当满足Δt≥|t1-t2|条件时，将自车与他车到达碰撞点的时间与各自车辆的安全防撞时间进行比较，判断是否需要进行预警显示；

[0087] 步骤304：计算安全防撞时间及预警判断，安全防撞时间是驾驶员采取措施恰好避免危险所需的最短时间，包括驾驶员反应时间及控制车辆(转向、制动)所需时间。

[0088] 安全防撞时间的计算公式如下：

[0089]

[0090]

[0091] 其中v为车辆的行驶速度，km/h；g为重力加速度，取9.8m/s2；μ为轮胎-路面附着系数；持续制动时间随车速变化而变化，g·μ为制动最大减速度，不同路面附着系数制动效果不同；t1为驾驶员反应时间是接到预警信号到做出反应的时间；t2为制动器协调时间，即制动踏板开始踩下到制动生效的时间t3为制动力增长时间，即制动生效到液压器产生制动的时间；t4为持续制动时间。

[0092] 通常情况下：TTC≤3s设定为危险警告；3s＜TTC≤5s设定为危险提示；TTC＞5s设定为无危险。

[0093] 步骤4：如图3所示，将系统状态方程和GPS/IMU组合导航的量测方程与导航参数误差模型相结合，对车辆的行驶位置信息进行采集，当GPS信号缺失时，系统自动转换为IMU定位，当GPS信号正常时重新转为GPS定位从而得到高精度的导航系统，实现较为准确的实时定位。具体步骤为：

[0094] 步骤401：本系统中采用导航坐标系为东北天地理坐标系，车载GPS/IMU组合导航系统的导航参数的误差模型参数矩阵X(t)为：

[0095] X(t)＝[θE,θN,θU,δL,δλ,δh,δυE,δυN,δυU,ψx,ψy,ψz,▽x,▽y,▽z]T[0096] 其中，δL,δλ,δh分别为纬度误差、经度误差和高度误差；δvE,δvN,δvU分别为载体的东向、北向和天向速度误差；θE,θN,θU为数学平台矢准角；ψx,ψy,ψz,▽x,▽y,▽z分别为陀螺随机常值漂移和加速度计随机常值零偏；

[0097] 系统状态方程为：

[0098]

[0099] 其中，F(t)为系统的状态转移矩阵，G(t)为系统的噪声方差矩阵。

[0100] 步骤402：选取IMU与GPS输出的位置之差和速度之差作为量测量，得出速度量测矢量Zυ(t)和位置量测矢量ZS(t)分别为：

[0101]

[0102]

[0103] 上述量测矢量中VS(t)＝[PGE PGN PGU]，Vv(t)＝[VGE VGN VGU]，

[0104] HS＝[03×6 diag(RM RNcos L 1) 03×6]

[0105] Hv＝[03×6 diag(1 1 1) 03×6]；

[0106] 其中，下标I表示IMU，下标G表示GPS；LGE,λGN,hGU分别为GPS沿东北天方向的纬度、经度、高度，PGE,PGN,PGU为GPS沿东北天方向的位置误差，VGE,VGN,VGU为GPS沿东北天方向的速度误差，RM、RN分别为载体所在子午圈和卯酉圈曲率半径。

[0107] 步骤403：联合位置量测矢量方程和速度测量方程得出GPS/IMU组合导航的量测方程Z(t)如下

[0108]

[0109] 根据上述系统状态方程和GPS/IMU组合导航的量测方程，本系统采用STM32F103作为主控制器，I2C串行总线连接IMU模块，串口接收GPS信息，实时采集GPS数据和IMU数据，通过数据预处理，姿态解算来达到组合导航的目的，并且可以得到当前车辆较为精确的实时位置、速度信息和姿态信息。

[0110] 步骤5：利用摄像头采集视频显示区域对当前车辆前方的路况信息进行实时显示并结合百度导航系统强大的路况信息数据，对车辆实时位置在百度地图上进行标注，进行可视化展示，如图4所示。具体包括以下步骤：

[0111] 步骤501：利用行车记录仪上的摄像头对车辆行驶期间正前方的路况信息进行采集，对采集得到的视频经过YOLOv3深度学习算法预处理，得到包含检测信息和测距信息的视频流。同时通过车载微型计算机TX2可联网的特性对处理好的视频流进行云备份，检测模块对汽车行驶前方的车辆、行人等障碍物进行识别检测，并将结果传输到GUI显示终端，在摄像头采集视频显示区域对当前车辆前方的路况信息进行实时显示。

[0112] 步骤502：通过GPS模块采集到的车辆位置信息，结合百度导航系统强大的路况信息数据，对车辆实际地理位置在百度地图上进行标注，进行可视化展示。

[0113] 步骤503：GUI显示功能按键区域包含三个按键分别为“状态信息”、“复位”和“退出”。“状态信息”按键主要打开车辆行驶过程中具体的路况信息(包含里程、时间、天气等)的子页面；“复位”按钮主要提供系统的复位功能；“退出”按钮主要提供退出系统的功能。

[0114] 最后说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本技术方案的宗旨和范围，其均应涵盖在本发明的权利要求范围当中。