CN109323705B - 一种基于gnss的无人车行驶路径规划方法 - Google Patents

Abstract

本发明公开了一种无人车行驶路径规划方法，属于无人驾驶技术、路径规划技术、网络通信技术领域。本发明利用利用GNSS模块获取当前定位信息，通过网络模块与服务器进行通讯，并获取目的地信息。处理分析数据后，请求云端导航平台规划路径。之后解析云端平台的返回数据，针对路口位置计算出优化后的行驶坐标，最后生成行驶路线中的每个节点(节点为行驶路线中无人车需要经过的GNSS定位信息)，无人车可利用优化计算后的路径节点指导其通行。本发明可以用来实现无人车和半自动无人车的行驶路径规划。

Description

一种基于GNSS的无人车行驶路径规划方法

技术领域

本发明涉及车辆导航领域，尤其涉及一种基于GNSS和网络通信的车辆行驶及路口导航方法。

背景技术

随着计算机硬件计算能力的不断提升和图像处理技术的不断成熟，许多工作都可以交由以计算机为核心的系统完成，无人驾驶便是其受益者之一。利用图像处理技术可以实现对前方路况进行采集和分析，并规划出合理的路线指引无人车避开障碍抵达目的地。

目前基于移动端和Web端的导航已十分成熟，但因其对硬件有所要求，无法做到低成本化。通过Web服务访问云端开放导航平台可以通过路径规划得到的行驶路线，即每一段直线行驶的距离、路口处的转向信息和路径中的每段折线行驶的坐标。无人驾驶车辆大部分时间都在直路通行，通过图像算法实时检测红绿灯和斑马线等路口标志物来判断是否为路口无疑是对计算资源的极大浪费。因此，需要一种能够有效判断路口并给出转向路径的方法来缓解计算压力并降低运行成本。

发明内容

本发明提供了一种基于GNSS的路口检测方法，主要用于无人驾驶车辆的行驶辅助，在行进过程中默认车辆的避障功能实时开启。

本发明采用如下技术方案：一种基于GNSS的无人车行驶路径规划方法，包括如下步骤：

(1)利用定位模块获取当前定位信息。

(2)通过网络通信模块访问服务器获取目的地信息，处理器请求云端导航平台根据当前定位信息和目的地位置信息规划路径，并返回路径规划数据。

(3)步骤(2)中返回的路径规划数据具有树形结构，由多个节点构成从起始地到目的地的路径，该路径分为若干组，每段直线运动为一组，前后相邻的两个组之间重合的节点处为需要进行转向操作的坐标点；提取该坐标点的数据，获得到达路口需要转弯时的经纬度坐标。

(4)对于某一路口cross_i，设其经纬度坐标分别为lng_i、lat_i，其前一个节点的经纬度坐标为lng_i-1、lat_i-1，后一个节点的经纬度坐标为lng_i+1、lat_i+1，设这三个点所组成的三角形顶点分别为A、B、C，所在地每一度经纬度对应的距离分别为d_lng、d_lat，则可以求得：

根据余弦定理：

即路口处需要转过的角度。

(5)由于云端导航平台规划的路径为一折线段，设所用车辆转弯半径为r，车辆行驶路径应为以r为半径并相切于∠A的两边的圆弧。以A点为原点，地理正北方向为y轴，地理正东方向为x轴建立坐标系，先将经纬度坐标差转化为实际距离，即相应乘以d_lng、d_lat，再通过三角函数求得向量AB与x轴的夹角α，则可；通过左乘旋转矩阵

将AB边旋转至x轴正方向；左乘旋转矩阵后的A、B、C点坐标分别为(x₁，y₁)、(x₂，y₂)、(x₃，y₃)，根据几何关系求得相切于∠A的两边的圆方程：

再将圆方程与AB、AC所在的直线联立，得出两个交点，交点间的弧线为所求转弯时的行驶路径。

(6)以此求得的弧线方程上所有的点横坐标和纵坐标，先左乘旋转矩阵的逆矩阵

再分别除以d_lng和d_lat，最后加上原点A对应的经纬度坐标，得到在经纬度坐标系统下的弧线方程。通过∠A的大小对弧线进行等分，获得弧线上等分点的经纬度坐标。

(7)按照步骤(4)～(6)获得所有的路口的等分点经纬度坐标；连同路径中的非路口节点，按顺序依次压入队列中，形成新的路径。

进一步地，定位模块获取当前定位信息，并将当前定位的信息通过串口通信的方式传入处理器模块。

进一步地，处理器通过网络通信模块向云端导航平台传送当前位置信息和目的地位置信息，并请求路径规划。

进一步地，定位模块为GNSS模块。

进一步地，云端导航平台包括百度地图、高德地图。

本发明的有益效果在于：

1.极大降低成本。利用云端平台规划的折线路径，可合理计算出车辆需要行进的精确曲线路径，在导航精确的情况下可以极大地降低使用双目相机进行避障的无人驾驶车辆的计算成本。车辆可以在开启避障算法的情况下，仅通过简单的信号灯识别启停，并不需要复杂的路口检测与规划路径，便可沿导航线路通过路口。

2.环境适应性强。靠精确路径导航的车辆对环境明暗程度没有要求，对路口处的导航也与路口标志物无关，极大地增强了环境适应性。

附图说明

图1为本发明结构示意图；

图2为本发明系统示意图；

图3为节点连接后形成的路径；

图4为路口节点示意图；

图5为圆弧示意图；

图6为转弯时的行驶路径；

图7为树形结构的路径规划数据示意图；

附图中标记及对应的零部件名称：1-外壳、2-双目相机、3-底盘、31-车前轮、32-车后轮、4-上位机、41-GNSS模块、42-陀螺仪、43-4G模块、44-下位机、5-电池、6-电机。

具体实施方式

为使本发明的目的、技术方案和有点更加清晰明了，下面结合实例和附图，对本发明作进一步的详细说明，本发明的示意性实施方式及其说明仅用于解释本发明，并不作为对本发明的限定。

实例如图1所示，本发明一种基于双目相机和GNSS的路口检测方法搭载于无人车系统中，其包括GNSS模块、陀螺仪、网络通信模块、双目相机、电机模块、上位机、下位机、车轮。所述GNSS模块为GPS与北斗双模定位模块，与上位机相连，实时采集地理定位信息并发送至上位机；所述陀螺仪模块与上位机相连，将方向信息实时发送至上位机；所述网络通信模块为4G移动通信模块，与上位机相连，在有请求时进行网络通信；所述双目相机为双目相机，与上位机相连，进行对路况的实时采集并将获取的深度图像交由上位机处理；所述电机模块控制车轮前进后退及转向，与下位机相连；所述下位机模块为Arduino电路板，与上位机相连，控制无人车的行驶；所述上位机为微型台式机，运行平台为Windows，负责对深度图像的实时处理、请求网络通信、实时获取GNSS数据和获取陀螺仪数据并分析处理；所述电池模块与上位机、下位机、电机相连，为其三者供电。

在车辆启动后，从上位机输入目的地址，上位机从GNSS模块中解析出当前地理坐标，将目的地和当前位置通过4G模块发送至云端服务器获取路径规划，而后开启通路检测算法，借助陀螺仪传来的方向和GNSS模块返回的定位信息，按照规划的路径向下位机传输控制信号。

上述方案具体实现如下：

(1)利用GNSS模块获取当前定位信息。

(2)通过网络模块访问服务器获取目的地信息，经由处理器模块处理后，请求云端导航平台规划路径。

(3)解析并计算云端平台的返回数据，针对路口位置计算出优化后的行驶坐标，最后生成行驶路线中的节点。

在(1)的实现中，GNSS模块获取当前定位信息，并将当前定位的信息通过串口通信的方式传入处理器模块，处理器模块分析并提取当前的经纬度信息进行储存。具体实现为：GNSS模块会周期性获取当前位置的GPGSV(可见卫星信息)、GPGLL(地理定位信息)、GPRMC(推荐最小定位信息)、GPVTG(地面速度信息)、GPGGA(GNSS定位信息)、GPGSA(当前卫星信息)，并以$为开头从串口传入处理器模块。本发明采用GPGGA信息进行GNSS数据分析，其包含UTC时间、UTC日期、纬度和经度等信息，对串口传入的数据进行判断，只保存以$GPGGA开头的数据，并对其进行数据解析，提取其中的经纬度坐标，使用递推平均滤波法对其进行滤波，减轻GNSS信号漂移的影响，最后存入全局变量lng，lat中。

在(2)的实现中，借助网络通信模块或者以串口通信的方式向处理器送入目的地信息。若送入的为目的地经纬度信息，则处理器模块通过网络通信模块向云端导航平台传送当前位置信息和目的地位置信息，并请求路径规划。具体实现为：以(1)中获得的lng，lat坐标作为起始点，以传入的目的地坐标作为终点，将需要调用的api的前段、key、目的地和终点组合在一起，以get的方式通过通信模块访问互联网，接收返回的json数据。若送入的目的地信息为目的地详细名称，则处理器模块通过网络通信模块向云端导航平台以get的方式传送前段、key和目的地详细名称，请求地理逆编码，接收并解析返回的json数据，获取目的地的经纬度信息，而后的操作同前。

在(3)的实现中，解析(2)中返回的具有树形结构的路径规划数据，提取其中的路口需要转向时的坐标并将路口处的折线路径拟合为曲线路径，再将路径中的所有节点压入队列中。具体如下：

(3.1)步骤(2)中返回的路径规划数据具有树形结构，由多个节点构成从起始地到目的地的路径，将节点连接后形成的路径如图3所示，该路径分为若干组，每段直线运动为一组，组与组之间的重合节点处为需要进行转向操作的坐标点；提取每组的节点数据，获得到达路口需要转弯时的经纬度坐标。

(3.2)对于某一路口cross_i，如图4所示，设其经纬度坐标分别为lng_i＝120.000000°，lat_i＝30.001000°，其前一个节点的经纬度坐标为lng_i-1＝120.000000°、lat_i-1＝30.000000°，后一个节点的经纬度坐标为lng_i+1＝120.001000°、lat_i+1＝30.001000°，设这三个点所组成的三角形顶点分别为A、B、C。将地球视为球形，周长约为40000km，则所在地每一度经纬度对应的距离为d_lng≈96km、d_lat≈111km，则可以求得：

根据余弦定理可算得：

即路口处需要转过的角度。

(3.3)由于云端平台规划的路径为一折线段，设所用车辆转弯半径为r＝10m，合理的车辆行驶路径应为以r为半径并相切于∠A的两边的圆弧，如图5所示。以A点为原点，地理正北方向为y轴，地理正东方向为x轴建立坐标系，先将经纬度坐标差转化为实际距离，即相应乘以d_lng、d_lat，可通过简单的三角函数求得向量AB与x轴的夹角α＝90°，则可通过左乘矩阵

将AB边旋转至x轴正方向，可得左乘旋转矩阵后的A、B、C点坐标分别为(0,0)、(111,0)、(0,96)，根据几何关系则可以求得相切于∠A的两边的圆方程：(x-10)²+(y-10)²＝10²，再将圆方程与AB：y＝0、AC：x＝0联立，可得出两个交点E(10,0)、F(0,10)，交点间的弧线即为所求转弯时的行驶路径，即图6中的曲线BEFC。

(3.4)本实例求得的∠A为90°，将其进行三等分，获得两个等分点G、H，设(3.3)中求得的圆的圆心为M，则根据斜率可算出：

联立可得G(5,1.34)，H(8.94,2.98)。将G、H、E、F点先左乘前文旋转矩阵的逆矩阵，即

再分别除以d_lng和d_lat，最后加上原点A对应的经纬度坐标，便可得到在经纬度坐标系统下的坐标，分别为E(120.000000,30.000896)、G(120.000012,30.000948)、H(120.000027,30.000907)、F(120.000090,30.000100)。

(3.5)按照3.2～3.4获得所有的路口的等分点经纬度坐标；连同路径中的非路口节点，按顺序依次压入队列中，形成新的路径。

以上所述的具体实施方式，对本发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施方式，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (5)

1.一种基于GNSS的无人车行驶路径规划方法，其特征在于，包括如下步骤：

(1)利用定位模块获取当前定位信息；

(2)通过网络通信模块访问服务器获取目的地信息，处理器请求云端导航平台根据当前定位信息和目的地位置信息规划路径，并返回路径规划数据；

(3)步骤(2)中返回的路径规划数据具有树形结构，由多个节点构成从起始地到目的地的路径，该路径分为若干组，每段直线运动为一组，前后相邻的两个组之间重合的节点处为需要进行转向操作的坐标点；提取该坐标点的数据，获得到达路口需要转弯时的经纬度坐标；

(4)对于某一路口cross_i，设其经纬度坐标分别为lng_i、lat_i，其前一个节点的经纬度坐标为lng_i-1、lat_i-1，后一个节点的经纬度坐标为lng_i+1、lat_i+1，设这三个点所组成的三角形顶点分别为A、B、C，所在地每一度经纬度对应的距离分别为d_lng、d_lat，则可以求得：

根据余弦定理：

即路口处需要转过的角度；

(5)由于云端导航平台规划的路径为一折线段，设所用车辆转弯半径为r，车辆行驶路径应为以r为半径并相切于∠A的两边的圆弧；以A点为原点，地理正北方向为y轴，地理正东方向为x轴建立坐标系，先将经纬度坐标差转化为实际距离，即相应乘以d_lng、d_lat，再通过三角函数求得向量AB与x轴的夹角α，则可；通过左乘旋转矩阵

将AB边旋转至x轴正方向；左乘旋转矩阵后的A、B、C点坐标分别为(x₁，y₁)、(x₂，y₂)、(x₃，y₃)，根据几何关系求得相切于∠A的两边的圆方程：

再将圆方程与AB、AC所在的直线联立，得出两个交点，交点间的弧线为所求转弯时的行驶路径；

(6)以此求得的弧线方程上所有的点横坐标和纵坐标，先左乘旋转矩阵的逆矩阵

再分别除以d_lng和d_lat，最后加上原点A对应的经纬度坐标，得到在经纬度坐标系统下的弧线方程；通过∠A的大小对弧线进行等分，获得弧线上等分点的经纬度坐标；

(7)按照步骤(4)～(6)获得所有的路口的等分点经纬度坐标；连同路径中的非路口节点，按顺序依次压入队列中，形成新的路径。

2.根据权利要求1所述的方法，其特征在于，定位模块获取当前定位信息，并将当前定位的信息通过串口通信的方式传入处理器模块。

3.根据权利要求1所述的方法，其特征在于，处理器通过网络通信模块向云端导航平台传送当前位置信息和目的地位置信息，并请求路径规划。

4.根据权利要求1所述的方法，其特征在于，定位模块为GNSS模块。

5.根据权利要求1所述的方法，其特征在于，云端导航平台包括百度地图、高德地图。

Images