一种低速无人车园区内组合导航方法及系统与流程

本发明涉及智能导航的
技术领域：
，特别是一种低速无人车园区内组合导航方法及系统。
背景技术：
：无人驾驶技术已然成为了当下最热门的研究方向之一，精确定位是车辆实现自动驾驶最基本的环节。在国内，对于商用车而言自动驾驶受到法律的约束，同时存在很多技术壁垒，短时间内基本无法形成真正意义上的自动驾驶商品车。然而对于低速车，自动驾驶可以寻求很多落地的突破口。本发明依据低速车运行环境的特点(半开放、路程短、楼宇树木遮挡严重等)发明了一种基于雷达slam与rtk-导航的组合导航方法。对于园区内自动驾驶场地车而言，单独依赖rtk-导航会受环境制约，很难实现持续的自动驾驶。申请号为cn201810266168.5的发明专利公开了一种基于2d激光雷达和北斗定位的郁闭果园导航系统，包括2d激光雷达,北斗定位移动站、北斗定位基站、2个电源模块、pc机和控制器。2d激光雷达安装在履带拖拉机前端,北斗定位移动站安装在履带拖拉机顶端,北斗定位基站安装在郁闭果园内；一个电源模块为2d激光雷达、北斗定位移动站、pc机和控制器提供电能,另一个电源模块为北斗定位基站提供电能；2d激光雷达、北斗定位移动站和控制器均与pc机连接。该方法采用非a即b的融合方式，当rtk-导航失效采用道路边缘点导航，并没有对二者进行融合，没有对局部路径进行深度融合。申请号为cn201710801741.3的发明专利公开了一种基于rtk北斗和激光雷达的巡检机器人导航系统及方法，包括机器人移动站和后台管理服务器,所述机器人移动站包括机器人机体以及设置于机器人机体上的控制模块、定位导航模块、无线通信模块和电源管理模块,所述定位导航模块包括激光雷达和rtk/sins单元；导航地图采用全局地图与局部地图相结合的设计方案,全局地图构建采用机器人记录轨迹方式绘制,导航方式采用预瞄pid算法,局部地图构建采用激光雷达记录障碍物离散数据点,经聚类、曲线拟合等步骤还原障碍物边缘信息,采用人工势场路径规划避障方式。该方法利用rtk-导航进行定位，适用范围比较窄。技术实现要素：为了解决上述的技术问题，本发明提出一种低速无人车园区内组合导航方法及系统，将移动站定位天线改为前天线，同时将雷达坐标原点平移到导航前天线位置，保证了采集地图轨迹的一致性；其次联合采集地图，地图中同一个路点既包括雷达slam定位坐标也包括rtk-导航经纬度坐标；然后雷达slam模块与rtk-导航模块加载同一个地图进行一次路径规划，所得路径点均以车辆坐标系为原点；最后通过融合模块处理两个定位模块的局部路径，最终生成无人车的目标路径。本发明的第一目的是提供了一种低速无人车园区内组合导航方法，包括使用雷达slam模块生成路径信息1，还包括以下步骤：步骤1：进行联合地图采集，生成路径地图；步骤2：使用rtk导航模块生成路径信息2；步骤3：融合模块接受所述路径信息1和路径信息2，加权融合后生成目标轨迹。优选的是，所述步骤1包括以下子步骤：步骤11：定义地图信息中的单个路点信息；步骤12：采集地图时，实时将所述单个路点信息存储在txt文本中；步骤13：采集完毕后，得到地图路径信息并生成所述路径地图。在上述任一方案中优选的是，所述单个路点信息由采集时间、精度、维度、方向角、差分状态、纵向路点属性、横向路点属性、雷达slam定位点x、雷达slam定位点y和slam定位置信度中至少一种组成。在上述任一方案中优选的是，所述步骤2包括以下子步骤：步骤21：加载所述路径地图；步骤22：实时接收当前经纬度、航向角信息；通过utm投影将车身经纬度点转换为笛卡尔坐标；步骤23：将所述路径地图中的路径点通过utm投影转换成笛卡尔坐标；步骤24：在笛卡尔坐标系下计算出车身点前方k米的所述路径点，其中k为自然数；步骤25：将得到的所述路径点转换成以车辆坐标系原点为原点的一次目标轨迹点s0。在上述任一方案中优选的是，所述rtk导航模块还用于将当前rtk-导航定位状态实时发布给融合模块。在上述任一方案中优选的是，所述使用雷达slam模块生成路径信息1包括以下子步骤：步骤2a：加载所述路径地图；步骤2b：通过雷达slam定位实时计算车辆的位置信息和/或航向信息；步骤2c：将当前车身点与地图中所述路径点进行比较，结合航向角搜索出车辆前方k米的点，其中k为自然数；步骤2d：将得到的路径点转换成以车辆坐标系原点为远点的一次目标轨迹点s1。在上述任一方案中优选的是，所述雷达slam模块还用于将雷达slam定位结果的置信度实时发布给融合模块。在上述任一方案中优选的是，所述步骤3包括以下子步骤：步骤31：递推计算出采样时刻k的rii(k)和rij(k)，其中，rii(k)表示xi的自协方差函数，rij(k)表示xi、xj的互协方差函数，xi表示传感器i的测量值，xj表示传感器j的测量值；步骤32：计算采样时刻k的其中，表示rij(k)的均值函数；步骤33；计算采样时刻k的σi2，其中，σi2表示各传感器的方差；步骤34：计算各传感器多次测量值的均值步骤35：计算此时各传感器的最优加权因子wi*；步骤36：计算此时的估计值。在上述任一方案中优选的是，所述采样时刻k的rii(k)的计算公式为：其中，m为采样时刻，i为独立的传感器。在上述任一方案中优选的是，所述采样时刻k的rij(k)的计算公式为：其中，j为独立的传感器。在上述任一方案中优选的是，所述采样时刻k的的计算公式为：其中，n为采样时刻。在上述任一方案中优选的是，所述采样时刻k的σi2的计算公式为：σi2＝e[ei2]＝rii-rij其中，e为期望因子，ei为零均值平稳噪声。在上述任一方案中优选的是，由于xi、xj互不相关，且均值为零，与x也不相关，故xi、xj的互协方差函数rij满足：rij＝e[xixj]＝e[x2]。在上述任一方案中优选的是，xi的自协方差函数rii满足：rii＝e[xi2]＝e[x2]+e[ei2]。在上述任一方案中优选的是，所述各传感器多次测量值的均值的计算公式为：其中，m为历史数据的个数。在上述任一方案中优选的是，所述最优加权因子wi*的计算公式为：此时，对应的总方差最小值为在上述任一方案中优选的是，所述估算值的计算方法为：其中，为融合后的真值，wi为各传感器的加权因子。发明的第二目的是提供了一种低速无人车园区内组合导航系统，包括用于生成路径信息1的雷达slam模块，还包括以下模块：地图采集模块：用于进行联合地图采集，生成路径地图；rtk导航模块：用于生成路径信息2；融合模块：用于接受所述路径信息1和路径信息2，加权融合后生成目标轨迹；所述系统使用如权利要求1所述的方法进行组合导航。优选的是，所述雷达slam模块还用于规划出一次目标轨迹点s0，所述一次轨迹为在车辆坐标系下的目标路点。在上述任一方案中优选的是，所述rtk导航模块还用于规划出一次目标轨迹点s1。在上述任一方案中优选的是，所述rtk导航模块为双天线导航，前天线连接到导航接收机的后天线位置，后天线连接到导航接收机的前天线位置。在上述任一方案中优选的是，ol-xlylzl为所述雷达slam的坐标系，xl轴正方向为雷达数据输出线缆的相反方向，yl轴正方向为xl轴正方向逆时针旋转90°，zl轴正方向指向天空。27.在上述任一方案中优选的是，所述雷达slam模块的安装位置距地面高度为hl，距车辆坐标系原点的前向位移为xl，水平方向无偏为yl，则雷达相对于车辆坐标系的偏移向量表示为tl＝[xl0hl]。因此雷达坐标系与车辆坐标的转换关系为：其中，ov-xvytzv为前雷达的坐标系。在上述任一方案中优选的是，所述rtk导航模块具有如下功能：1)加载联合采集的所述路径地图；2)通过utm投影将车身经纬度点转换为笛卡尔坐标；3)将地图中的路径点通过utm投影转换成笛卡尔坐标；4)在笛卡尔坐标系下计算出车身点前方k米的所述路径点，其中k为自然数；5)将得到的所述路径点转换成以车辆坐标系原点为原点的一次目标轨迹点s0。在上述任一方案中优选的是，所述雷达slam模块具有如下功能：1)加载联合采集的所述路径地图；2)通过雷达slam定位实时计算车辆的位置信息和/或航向信息；3)将当前车身点与地图中所述路径点进行比较，结合航向角搜索出车辆前方k米的点，其中k为自然数；4)将得到的路径点转换成以车辆坐标系原点为远点的一次目标轨迹点s1。在上述任一方案中优选的是，所述融合模块用于接收所述rtk导航模块实时发布的当前rtk-导航定位状态和/或所述雷达slam模块实时发布的雷达slam定位结果的置信度。本发明提出了一种低速无人车园区内组合导航方法，增加了园区内低速车自动驾驶的可靠性与系统的鲁棒性。附图说明图1为按照本发明的低速无人车园区内组合导航方法的一优选实施例的流程图。图2为按照本发明的低速无人车园区内组合导航系统的一优选实施例的模块图。图3为按照本发明的低速无人车园区内组合导航系统的另一实施例的雷达坐标系与导航坐标系标定图。图4为按照本发明的低速无人车园区内组合导航方法的如图3所示实施例的联合地图采集方法流程图。图5为按照本发明的低速无人车园区内组合导航方法的如图3所示实施例的组合导航方法流程图。图6为按照本发明的低速无人车园区内组合导航系统的如图3所示实施例的模块间关系图。图7为按照本发明的低速无人车园区内组合导航系统的如图3所示实施例的多传感器自适应加权融合模型图。具体实施方式下面结合附图和具体的实施例对本发明做进一步的阐述。实施例一如图1所示，执行步骤100，进行联合地图采集，生成路径地图。在步骤100中执行步骤101，定义地图信息中的单个路点信息，单个路点信息由采集时间、精度、维度、方向角、差分状态、纵向路点属性、横向路点属性、雷达slam定位点x、雷达slam定位点y和slam定位置信度中至少一种组成。执行步骤102，采集地图时，实时将所述单个路点信息存储在txt文本中。执行步骤103，采集完毕后，得到地图路径信息并生成所述路径地图。执行步骤110，使用rtk导航模块生成路径信息2，rtk导航模块还用于将当前rtk-导航定位状态实时发布给融合模块。在步骤100中执行步骤111，加载所述路径地图。执行步骤112，实时接收当前经纬度、航向角信息，通过utm投影将车身经纬度点转换为笛卡尔坐标。执行步骤113，将所述路径地图中的路径点通过utm投影转换成笛卡尔坐标。执行步骤114，在笛卡尔坐标系下计算出车身点前方k米的所述路径点，其中k为自然数。执行步骤115，将得到的所述路径点转换成以车辆坐标系原点为原点的一次目标轨迹点s0。执行步骤120，使用雷达slam模块生成路径信息1，雷达slam模块还用于将雷达slam定位结果的置信度实时发布给融合模块。在步骤120中执行步骤121，加载所述路径地图。执行步骤122，通过雷达slam定位实时计算车辆的位置信息和/或航向信息。执行步骤123，将当前车身点与地图中所述路径点进行比较，结合航向角搜索出车辆前方k米的点，其中k为自然数。执行步骤124，将得到的路径点转换成以车辆坐标系原点为远点的一次目标轨迹点s1。执行步骤130，融合模块接受所述路径信息1和路径信息2，加权融合后生成目标轨迹。在步骤100中执行步骤31，递推计算出采样时刻k的rii(k)和rij(k)，计算公式为：其中，rii(k)表示xi的自协方差函数，rij(k)表示xi、xj的互协方差函数，xi表示传感器i的测量值，xj表示传感器j的测量值，m为采样时刻，i为独立的传感器，j为独立的传感器。执行步骤32，计算采样时刻k的计算公式为：其中，其中，表示rij(k)的均值函数，n为采样时刻。执行步骤33，计算采样时刻k的σi2，计算公式为：σi2＝e[ei2]＝rii-rij其中，σi2表示各传感器的方差，e为期望因子，ei为零均值平稳噪声。由于xi、xj互不相关，且均值为零，与x也不相关，故xi、xj的互协方差函数rij满足：rij＝e[xixj]＝e[x2]，xi的自协方差函数rii满足：rii＝e[xi2]＝e[x2]+e[ei2]。执行步骤34，计算各传感器多次测量值的均值计算公式为：其中，m为历史数据的个数。执行步骤35，计算此时各传感器的最优加权因子计算公式为：此时，对应的总方差最小值为执行步骤36，计算此时的估计值，计算公式为：其中，为融合后的真值，wi为各传感器的加权因子。实施例二如图2所示，一种低速无人车园区内组合导航系统包括地图采集模块200、rtk导航模块210、雷达slam模块220和融合模块230。地图采集模块200：用于进行联合地图采集，生成路径地图。rtk导航模块210：用于生成路径信息2，规划出一次目标轨迹点s1。rtk导航模块210具有如下功能：1)加载联合采集的所述路径地图；2)通过utm投影将车身经纬度点转换为笛卡尔坐标；3)将地图中的路径点通过utm投影转换成笛卡尔坐标；4)在笛卡尔坐标系下计算出车身点前方k米的所述路径点，其中k为自然数；5)将得到的所述路径点转换成以车辆坐标系原点为原点的一系列点s0。rtk导航模块210为双天线导航，前天线连接到导航接收机的后天线位置，后天线连接到导航接收机的前天线位置。ol-xlylzl为所述雷达slam的坐标系，xl轴正方向为雷达数据输出线缆的相反方向，yl轴正方向为xl轴正方向逆时针旋转90°，zl轴正方向指向天空。雷达slam的安装位置距地面高度为hl，距车辆坐标系原点的前向位移为xl，水平方向无偏为yl，则雷达相对于车辆坐标系的偏移向量表示为tl＝[xl0hl]。因此雷达坐标系与车辆坐标的转换关系为：其中，ov-xvyvzv为前雷达的坐标系。雷达slam模块220：用于生成路径信息1，规划出一次目标轨迹点s0，一次轨迹为在车辆坐标系下的目标路点。雷达slam模块220具有如下功能：1)加载联合采集的所述路径地图；2)通过雷达slam定位实时计算车辆的位置信息和/或航向信息；3)将当前车身点与地图中所述路径点进行比较，结合航向角搜索出车辆前方k米的点，其中k为自然数；4)将得到的路径点转换成以车辆坐标系原点为远点的一系列点s1.融合模块230：用于接受所述路径信息1和路径信息2，加权融合后生成目标轨迹。融合模块230用于接收所述rtk导航模块实时发布的当前rtk-导航定位状态和/或所述雷达slam模块实时发布的雷达slam定位结果的置信度。实施例三本发明基于雷达slam与rtk-导航的规划轨迹实现无人车的组合导航，即雷达slam模块规划出一次目标轨迹点s0，rtk-导航模块规划出一次目标轨迹点s1，本发明通过加权融合s0与s1得到无人车最终的目标轨迹，地图采集方法如图4所示，组合导航方法如图5所示。本发明所指的一次轨迹为：在车辆坐标系下的目标路点。为了保证雷达slam轨迹与rtk-导航轨迹的一致性，需要对二者的安装位置进行标定。本发明采用平移矩阵将雷达坐标原点转换到导航坐标原点。本发明采用的rtk-导航装置为双天线导航，一般的双天线导航后天线为定位天线，前天线为定向天线，本发明为了将雷达坐标原点与导航定位点统一故将前天线连接到导航接收机的后天线位置，后天线连接到导航接收机的前天线位置。前天线位置即为车辆坐标系原点。二者的位置关系如图3所示。其中，ol-xlylzl为雷达坐标系，xl轴正方向为雷达数据输出线缆的相反方向，yl轴正方向为xl轴正方向逆时针旋转90°，zl轴正方向指向天空；假设雷达的安装位置距地面高度为hl，距车辆坐标系原点的前向位移为xl，水平方向无偏为yl，则雷达相对于车辆坐标系的偏移向量表示为tl＝[xl0hl]。因此雷达坐标系与车辆坐标的转换关系为：联合地图采集首先定义地图信息。如表1所示，自动驾驶地图中单个路点信息由：采集时间、经度、纬度、方位角(采集地图时采集车的航向角)、差分状态(地图采集点定位评判依据)、路点属性(路段标记：主要标记路段纵向变化，如弯道、直道、u_turn等)与路点属性(路段标记：主要标记路段横向变化，如单行线、多车道等)，雷达slam定位点x(全局坐标)，雷达slam定位点y(全局坐标)，slam定位置信度(评价当前slam定位可靠性)组成。表1单个路点信息采集地图时，实时将单个路点信息存储到txt文本中，采集完毕后得到地图路径信息。子模块规划得到两条基于车辆坐标系轨迹本发明子模块关系图如图6所示：rtk-导航模块主要功能如下：加载联合采集的路径地图；实时接收当前经纬度、航向角信息；通过utm投影将车身经纬度点转换为笛卡尔坐标；同时将地图中的路径点即表1中(longitude，latitude)通过utm投影转换成笛卡尔坐标；在笛卡尔坐标系下计算出车身点前方30米的路径点；将得到的路径点转换成以车辆坐标系原点为原点的一系列点s0；另外，该模块还需实时发布当前rtk-导航定位状态以供融合模块使用。雷达slam模块主要功能如下：加载联合采集的路径地图；通过雷达slam定位实时计算车辆的位置信息、航向信息；将当前车身点与地图中路径点即表1中(lidarx，lidary)进行比较，结合航向角搜索出车辆前方30米的点；将得到的路径点转换成以车辆坐标系原点为远点的一系列点s1；另外，该模块还需实时发布雷达slam定位结果的置信度以供融合模块使用。两条轨迹加权融合并生成最终目标轨迹一般的，rtk-导航定位评价如表2，本发明在评价rtk-导航定位状态同样采用表2的评价方式。rtk状态01245定位精度无定位单点定位伪距定位差分定位浮点定位备注无定位定位不可用定位不可用定位精确定位可参考表2rtk-导航定位指标评价表本发明雷达slam模块定位评价如表3，依据点云匹配率定义定位置信度。表3雷达slam定位指标评价表融合模块如图6所示，融合模块需同时接受rtk-导航模块与雷达slam模块规划的局部路径信息，依据各自的定位状态进行两个路径信息加权融合。依据二者实时定位状态本发明采用表4的融合决策方案，表中4中a表示选用rtk-导航规划的轨迹为目标轨迹；b表示选用雷达slam规划的轨迹为目标轨迹；c表示无法生成目标轨迹-对于同时雷达slam定位与rtk-导航定位不可靠的情况，本发明为了无人车安全起见不再考虑轨迹生成；ab表示二者加权融合后的得到的目标轨迹。表4轨迹融合方案表对于表4中ab的情况，本发明采用基于自适应加权的决策融合模型，如图7所示。设有n个传感器对某一对象参数进行测量，其测量值分别为x1、x2、…、xn，然后进行加权融合。该算法的总体思想是在保证总方差最小的条件下，根据各传感器的测量值，通过自适应的方式寻找与之对应的最优加权因子，使融合后的值最优。具体实施过程如下：步骤一：用式(1)和式(2)递推地算出采样时刻k的rii(k)和rij(k)；步骤二：根据式(3)求出采样时刻k的步骤三：根据式(4)求得采样时刻k的σi2；σi2＝e[ei2]＝rii-rij(4)因为xi、xj互不相关，且均值为零，与x也不相关，故xi、xj的互协方差函数rij满足下式：rij＝e[xixj]＝e[x2](5)xi的自协方差函数rii满足：rii＝e[xi2]＝e[x2]+e[ei2](6)步骤四：根据式(7)求取各传感器多次测量值的均值；其中m为历史数据的个数。步骤五：根据式(8)求出此时各传感器的最优加权因子wi*；此时，对应的总方差最小值为：步骤六：根据式(10)求得此时的估计值。通过表4的对rtk-导航模块与雷达slam模块规划的轨迹进行融合最终得到无人车的目标轨迹。实施例四设两相互独立的传感器i、j，其测量值分别为xi、xj，对应的测量误差(即零均值平稳噪声)为ei、ej，待估计真值为x，则有：xi＝x+ei和xj＝x+ej，。传感器i的方差为：为了更好地理解本发明，以上结合本发明的具体实施例做了详细描述，但并非是对本发明的限制。凡是依据本发明的技术实质对以上实施例所做的任何简单修改，均仍属于本发明技术方案的范围。本说明书中每个实施例重点说明的都是与其它实施例的不同之处，各个实施例之间相同或相似的部分相互参见即可。对于系统实施例而言，由于其与方法实施例基本对应，所以描述的比较简单，相关之处参见方法实施例的部分说明即可。当前第1页12