本发明属于辅助驾驶，具体为一种基于北斗与雷达定位的交通路况感知与路线优化系统。
背景技术：
1、随着科学技术的不断发展，交通领域也出现了许多创新和变革，如智能汽车、共享汽车等。这些创新为交通出行提供了更多选择和便利，但同时，这些创新也带来了新的交通安全问题，近几年的城市汽车保有量一直在逐步增长，道路容量不足，导致很多城市的交通常常出现阻塞，特别是上下班高峰期和市中心区域。
2、针对目前现有的导航系统，可以提供实时交通信息，但是交通情况可能会受到误报、延误或者无法即时更新的因素影响。国内的部分城市已经开始使用的交通路况检测系统，如摄像头和车辆传感器，以收集实时交通路况信息，但是仍然在数据精度和实时性上存在一些限制。雷达的优点是精度高，测量范围广，可以随时随地进行测量，但是目前来看该技术主要用于车辆感知、避障以及速度测量，对于交通路况感知领域使用相对较少。
3、在现有的导航路径规划领域，依旧存在推荐路径无法根据实时路况及车辆信息进行及时调整的问题，导致原有推荐路径会因为临时出现的交通拥堵状况不再成为当前驾驶车辆需要的较优选择，从而加剧原交通拥堵路段的拥堵程度并降低当前导航使用者的驾驶体验。
4、基于上述内容来看，目前迫切需要一种能实时监测路网交通状况变化，动态更新路径规划的系统，从而为驾驶员提供最优的出行方案和导航服务，并且保证数据的精准性和可靠性。
技术实现思路
1、针对上述问题，本发明提供一种基于北斗与雷达定位的交通路况感知与路线优化系统，通过北斗卫星数据获取模块和基于雷达设备的路网组件模块有机结合，可以获取更精准、可靠的数据，当用户行驶路段前方出现拥堵情况时，能为驾驶员提供准确的交通预测，实时动态地更新出符合实际情况的最优路径规划，达到改善交通拥堵、提高出行效率、增强驾驶安全性、节省时间和油耗问题的目的。
2、为解决以上问题，本发明采用的技术方案为：一种基于北斗与雷达定位的路况感知与路线优化系统，包括北斗卫星导航系统，所述北斗卫星导航系统连接有用于解算车辆位置信息的数据获取模块，还包括连接有雷达设备的路网组建模块；所述数据获取模块和路网组建模块连接有数据信息处理模块，所述数据信息处理模块包括用于提供实时路况信息的数据融合模块和计算路线的路径规划模块；所述数据融合模块连接有路径规划模块；所述数据融合模块和路径规划模块连接有向用户展示路况及导航信息的用户界面。
3、进一步的，所述的数据获取模块由基准站、一个或多个移动站和差分gnss解算模块组成；所述基准站接收来自北斗卫星的gnss信号，包含卫星的位置信息和时间信息，并根据这些信号计算出基准站的位置和时间信息；基准站根据自己的已知坐标和卫星的位置，计算出每颗卫星到基准站的几何距离，然后与接收到的伪距进行比较，得到误差，也就是伪距改正数，并将这些改正数以差分数据的形式发送给移动站；所述移动站接收来自北斗卫星的gnss信号，并根据这些信号计算出移动站的位置和时间信息；移动站同时接收来自基准站的差分数据，并根据基准站的改正数对自己的伪距进行修正，从而消除或减小误差，提高定位精度；所述差分gnss解算模块利用伪距的差分观测方程，根据移动站的观测数据和基准站的改正数，得到移动站的精确位置信息。
4、伪距差分算法的基本原理和步骤如下：
5、第一步：在一个已知坐标的点上安装一个gnss接收机作为基准站，连续跟踪和观测所有可见的卫星，测量基准站到卫星的伪距。
6、基准站到卫星的伪距为：
7、
8、其中，是基准站到第i颗卫星的伪距，是基准站到第i颗卫星的真实几何距离，c是光速，δtb是基准站的钟差，δti是第i颗卫星的钟差，是电离层延迟，是对流层延迟，是其他误差。
9、第二步：利用基准站的已知坐标，计算基准站到卫星的真实几何距离，然后与伪距进行比较，得到伪距的误差或修正值。
10、基准站到卫星的真实几何距离为：
11、
12、其中，(xi，yi，zi )是第i颗卫星的空间直角坐标，(xb ，yb，zb)是基准站的空间直角坐标。
13、伪距的误差或修正值为：
14、
15、第三步：将伪距的修正值通过无线电或网络传输给移动站，移动站利用这些修正值来改正自己的伪距观测值。
16、移动站到卫星的修正后的伪距为：
17、
18、其中，是移动站到第i颗卫星的原始伪距，是移动站到第i颗卫星的真实几何距离，δtr是移动站的钟差，是其他误差。
19、第四步：利用修正后的伪距，结合卫星的位置和时间信息，采用最小二乘法或其他方法，求解移动站的坐标和钟差。
20、移动站的坐标和钟差的求解为：
21、其中，n是观测到的卫星数，可以用移动站的坐标和卫星的坐标表示，即：
22、
23、其中，(xr，yr ，zr )是移动站的空间直角坐标。这是一个非线性方程组，可以用迭代法或最小二乘法求解。
24、进一步的，所述路网组建模块配备了不同类型的雷达，根据不同的需求和场景选择合适的雷达，如基于多普勒效应原理的连续波雷达可以用于测速，激光雷达可以用于测距，毫米波雷达可以用于综合测速测距。
25、首先，使用雷达设备来对路面和周围环境进行扫描，通过雷达设备接收目标反射的电磁波，并根据反射时间和信号特征来计算目标的位置、速度、类型等信息，从而获取三维点云数据。反射时间是指雷达发射电磁波到接收目标回波的时间间隔，可以用来计算目标距离。信号特征是指雷达回波的幅度、相位、频率、偏振等参数，可以用来识别目标的类型、速度、姿态等信息。
26、然后对三维点云数据进行预处理，如滤波、分割、聚类、分类等，以去除噪声、补充缺失值、降低维度、提高信噪比等，以便于后续的分析和应用。
27、接着，根据提取出的特征，生成路网的点云表示，并将路面划分为不同的路段和路口，以及标记出车道线、路边障碍物、动态目标等信息。点云表示是指用一组离散的点来表示三维空间中的物体或场景，每个点可以有不同的属性，如颜色、法向量、强度等。路网的点云表示是指用点云来表示路面和周围环境的形状和特征，具有高精度、低冗余、易处理等优点。
28、最后，将生成的路网和雷达的位置信息发送给数据信息处理模块，以供路况感知和路线优化使用。路网会进行持续数据采集，实时地图更新来影响路况，对于移动中的物体如车辆和行人，会实时更新其位置速度等信息，时刻保持生成的路网为最新的。
29、进一步的，所述数据信息处理模块包括数据融合模块和路径规划模块。
30、进一步的，数据融合模块从两个方面获取数据：一是数据获取模块，它通过基准站和移动站接收并校正gnss卫星信号，得到车辆的位置信息；二是路网组建模块，它通过雷达设备获取路面的三维点云数据，生成路网的拓扑结构和属性信息。数据融合模块获得数据后，需要进行以下几个步骤：
31、首先是数据预处理，对数据获取模块和路网组建模块提供的数据进行质量检测、格式转换、数据时间戳同步、转换相同坐标系等操作，以便于后续的数据融合。
32、接着是数据匹配，根据车辆的位置信息和路网的拓扑结构，确定车辆所在的路段和方向，以及车辆和路网的相对位置关系。
33、然后是数据融合，采用迭代最近点(iterative closest point，icp)算法，它的公式为：
34、
35、其中，r代表旋转变换矩阵，t代表平移变换矩阵。该算法主要是对数据匹配部分得到的对应点对点进行迭代优化，反复迭代以最小化点云之间的距离误差，从而找到最佳的平移和旋转变换，确保点云在共享坐标系中精确对齐。得到实时路况信息，包括路段的通行速度、拥堵程度和交通事件等。
36、icp算法的工作原理及步骤如下：
37、首先给定两个点云x和p，分别表示源点云和目标点云，以及一个初始变换矩阵t，x＝(x1，x2，x3…xn)，p＝(p1，p2，p3…pn)；
38、其次对源点云中的每个点xi ，寻找目标点云中距离最近的点pi ，作为其对应点，形成一组点对(xi，pi)；
39、然后根据点对，利用最小二乘法，求解出最优的旋转矩阵和平移向量，使得
40、
41、最小化，其中r和t分别表示旋转和平移变换；
42、接着用求解出的旋转矩阵和平移向量，更新源点云的变换矩阵；
43、重复以上步骤2-4，直到满足收敛条件，例如变换矩阵t的变化小于某个阈值或者迭代次数达到上限，最后输出最终变换矩阵t。
44、最后是数据输出，将数据融合得到的路况信息，以json(javascript objectnotation)格式，输出到路径规划模块和用户界面。json格式是一种常用的数据交换格式，它以键值对的形式表示数据，易于阅读和处理。
45、进一步的，所述路径规划模块包括状态估计模块、路线计算模块以及路径更新模块三个部分：
46、首先是状态估计模块，采用无迹卡尔曼滤波(unscented kalman filter，ukf)算法，对数据融合模块提供的路况信息进行状态估计和预测，得到未来一段时间内各路段的通行速度、拥堵程度和交通事件的概率分布。ukf算法是一种用于非线性系统的扩展卡尔曼滤波器(extended kalman filter，ekf)的变种，它能够有效地处理非高斯和非线性的系统，通过引入一组"无迹"采样点，巧妙地处理非线性系统，将数据获取模块和路网组建模块中的数据融合，提供准确的状态估计，如车辆位置和速度，适用于交通路况的预估。
47、ukf算法在交通路况预估方面的具体实现步骤如下：
48、第一步，初始化ukf算法的参数，包括状态向量、状态协方差矩阵、噪声协方差矩阵、权重向量等。
49、第二步，选择一组"无迹"采样点，即通过状态协方差矩阵的平方根分解，生成一组与状态向量维数相同的采样点，用于近似非线性系统的状态分布。
50、第三步，对每个采样点，使用数据融合模块提供的路况信息，如车辆位置、速度、加速度、道路长度、宽度、限速等，作为输入，通过非线性的状态转移函数，预测下一时刻的状态向量，即各路段的通行速度、拥堵程度和交通事件的概率。
51、第四步，根据预测的状态向量和权重向量，计算预测的状态均值和协方差矩阵，作为ukf算法的先验估计。
52、第五步，对每个采样点，使用非线性的观测函数，将预测的状态向量转换为观测向量，即各路段的实际通行速度、拥堵程度和交通事件的概率。
53、第六步，根据观测的向量和权重向量，计算观测的均值和协方差矩阵，以及状态向量和观测向量之间的互协方差矩阵，作为ukf算法的测量更新。
54、第七步，使用卡尔曼增益，结合先验估计和测量更新，得到后验估计，即更加准确的状态向量和协方差矩阵，作为ukf算法的输出。
55、第八步，重复步骤2-7，直到达到预定的时间范围或精度要求，完成交通预估的过程。
56、接着是路线计算模块，采用dijkstra算法，根据用户的出发点、目的地和交通路况预估，计算出最优的行驶路线，得到行驶距离最短的路线。dijkstra算法是一种经典的最短路径算法，它的公式为：其中，d(v)表示从源点到顶点v的最短路径长度，s表示已经确定了最短路径的顶点集合，w(u，v)表示从顶点u到顶点v的边的权重，它能够在有向图或者无向图中，找到两个顶点之间的最短路径。该算法的具体工作步骤和公式原理如下：
57、首先，设g＝(v，e，w)是一个带权有向图，s是源点，将图中的所有点分为两个集合，s是已经找到最短路径的点集，u是还未确定最短路径的点集。初始时，s只包含源点，u包含其他所有点，即s＝{s}，u＝v{s}。
58、然后，对于u中的每个点，记录从源点到该点的当前最短距离，用dist[v]表示。初始时，dist[v]等于源点到该点的边权，即dist[s]＝0，dist[v]＝w(s，v)，其中w(s，v)是s到v的边权，如果没有直接相连的边，则dist[v]为无穷大，即w(s，v)＝∞。
59、接着，从u中选出一个距离最小的点u，即u＝argmin{dist[v]|v∈u}，将其加入到s中，并从u中移除。这时，dist[u]就是源点到u的最短距离，不会再改变。
60、然后，更新u中的其他点的距离，即对于u中的每个点v，检查是否存在一条从u到v的边，如果存在，且dist[u]+w(u，v)<dist[v]，则更新dist[v]＝dist[u]+w(u，v)，这样，就保证了dist[v]是从源点经过s中的点到达v的最短距离。
61、重复上述两步，直到u为空，或者u中的点都不可达，这时算法结束，dist[v]就是源点到v的最短距离，如果dist[v]为无穷大，则说明源点到v没有路径。
62、最后是路径更新模块，根据用户的实时位置和数据融合模块提供的实时路况信息，对路线计算模块得到的最优路线进行调整和优化，以应对可能出现的交通拥堵、交通事故、道路封闭等情况。路径更新模块的原则是尽量保持原路线的连贯性，避免频繁的变更和折返，同时考虑用户的偏好和安全性。路径更新模块的结果，以语音和图形的形式，输出到用户界面，指导用户进行行驶。
63、进一步的，所述用户界面包括图形展示模块和语音模块。
64、图形展示模块主要由一个显示屏或手机屏和一个触摸屏组成。利用显示屏或手机屏，将路径规划模块的输出结果以图形的形式展示给用户，让用户可以直观地了解路线和路况，也可以通过触摸屏输入或修改相关信息，如目的地，提高用户的视觉体验和操作便利性。
65、图形展示模块的工作内容有：接收路径规划模块的输出数据，包括最优路线和路线动态更新的信息；在地图上绘制和更新路线，用不同的颜色或者图标标注路况；接收用户的触摸屏输入，根据用户的需求调整图形展示的参数或者路线计划。
66、语音模块：利用车载音响设备或者手机扬声器，将路径规划模块的输出结果以语音的形式播报给用户，让用户可以听到行驶指南和路线动态更新的提示，提高用户的听觉体验。
67、语音模块的工作内容有：接收路径规划模块的输出数据，包括最优路线和路线动态更新的信息；根据用户的偏好，选择合适的语音合成引擎，将行驶指南和路线动态更新的提示转换为语音信号，输出到用户界面上。
68、本发明是一种基于北斗与雷达定位的路况感知与路线优化系统，具有以下有益效果：提高定位精度和可靠性，该系统通过数据获取模块和路网组建模块校正和采集路面数据，生成路网信息，匹配和融合两种数据，避免信号误差；提高路况感知和预测能力，该系统通过算法，对路况信息进行估计和预测，使用户能够及时发现和避免交通问题，给用户准确的交通信息；提高路线优化和调整能力，该系统通过算法，根据用户的出发点、目的地和交通路况预估，计算出最好的行驶路线，同时根据用户的实时位置和实时路况信息，调整和优化路线，应对交通问题，给用户合理的行驶建议；提高用户的视觉和听觉体验，该系统通过图形展示模块和语音模块，用图形和语音展示路径规划模块的结果，让用户看清路线和路况，也可以用触摸屏修改信息，让用户舒服和满意。
1.一种基于北斗与雷达定位的路况感知与路线优化系统，包括北斗卫星导航系统，其特征在于：所述北斗卫星导航系统连接有用于解算车辆位置信息的数据获取模块，还包括连接有雷达设备的路网组建模块；所述数据获取模块和路网组建模块连接有数据信息处理模块，所述数据信息处理模块包括用于提供实时路况信息的数据融合模块和计算路线的路径规划模块；所述数据融合模块连接有路径规划模块；所述数据融合模块和路径规划模块连接有向用户展示路况及导航信息的用户界面。
2.根据权利要求1所述的一种基于北斗与雷达定位的路况感知与路线优化系统，其特征在于：所述数据获取模块由基准站、一个或多个移动站和差分gnss解算模块组成；所述基准站接收gnss卫星信号，所述移动站捕获车辆当前位置gnss数据且通过基准站发来的gnss卫星信号对车辆当前位置gnss数据进行校正。
3.根据权利要求2所述的一种基于北斗与雷达定位的路况感知与路线优化系统，其特征在于：所述gnss卫星信号包括卫星位置和时间信息。
4.根据权利要求2所述的一种基于北斗与雷达定位的路况感知与路线优化系统，其特征在于：所述差分gnss解算模块通过计算基准站到卫星的伪距和基准站到卫星的真实几何距离得到伪距的修正值，并根据该修正值得到移动站到卫星的修正后的伪距，并获得移动站的坐标和钟差的求解公式：
5.根据权利要求1所述的一种基于北斗与雷达定位的路况感知与路线优化系统，其特征在于：所述路网组建模块连接有连续波雷达、激光雷达和毫米波雷达组成的雷达设备；所述路网组建模块通过雷达设备获取的三维点云数据生成点云并组建路网。
6.根据权利要求1所述的一种基于北斗与雷达定位的路况感知与路线优化系统，其特征在于：所述数据获取模块和路网组建模块连接有数据融合模块；所述数据融合模块接收并融合数据获取模块提供的车辆位置信息和路网组建模块提供的实时路况信息；所述路径规划模块包括采用无迹卡尔曼滤波算法进行驾驶信息估计的状态估计模块和路线计算模块，路线计算模块采用dijkstra算法，根据更新每个顶点到源点的最短距离，直到算法结束，最终计算出最短路线。
7.根据权利要求6所述的一种基于北斗与雷达定位的路况感知与路线优化系统，其特征在于：所述路径规划模块还包括用于根据车辆位置信息和路网状况实时变化，实现最短路径结果动态更新的路径更新模块。
8.根据权利要求1所述的一种基于北斗与雷达定位的路况感知与路线优化系统，其特征在于：所述用户界面包括图形展示模块和语音模块。
9.根据权利要求8所述的一种基于北斗与雷达定位的路况感知与路线优化系统，其特征在于：所述图形展示模块为可动作交互的车载显示屏或手机屏幕，所述语音模块为车载音响设备或手机扬声器。
