[发明专利]一种低速无人车园区内组合导航方法及系统

权利要求书

1.一种低速无人车园区内组合导航方法，包括使用雷达SLAM模块生成路径信息1，其特征在于，还包括以下步骤：

步骤1：进行联合地图采集，生成路径地图；

步骤2：使用RTK导航模块生成路径信息2；

步骤3：融合模块接受所述路径信息1和路径信息2，加权融合后生成目标轨迹；

所述步骤3包括以下子步骤：

步骤31：递推计算出采样时刻k的R_ii(k)和R_ij(k)，其中，R_ii(k)表示X_i的自协方差函数，R_ij(k)表示X_i、X_j的互协方差函数，X_i表示传感器i的测量值，X_j表示传感器j的测量值；

步骤32：计算采样时刻k的其中，表示R_ij(k)的均值函数；

步骤33；计算采样时刻k的σ_i²，其中，σ_i²表示各传感器的方差；

步骤34：计算各传感器多次测量值的均值

步骤35：计算此时各传感器的最优加权因了最优加权因子的计算公式为：此时，对应的总方差最小值为其中，n为采样时刻；

步骤36：计算此时的估计值，计算方法为：

其中，为融合后的真值，W_i为各传感器的加权因子。

2.如权利要求1所述的低速无人车园区内组合导航方法，其特征在于，所述步骤1包括以下子步骤：

步骤11：定义地图信息中的单个路点信息；

步骤12：采集地图时，实时将所述单个路点信息存储在txt文本中；

步骤13：采集完毕后，得到地图路径信息并生成所述路径地图。

3.如权利要求2所述的低速无人车园区内组合导航方法，其特征在于，所述单个路点信息由采集时间、精度、维度、方向角、差分状态、纵向路点属性、横向路点属性、雷达SLAM定位点X、雷达SLAM定位点Y和SLAM定位置信度中至少一种组成。

4.如权利要求2所述的低速无人车园区内组合导航方法，其特征在于，所述步骤2包括以下子步骤：

步骤21：加载所述路径地图；

步骤22：实时接收当前经纬度、航向角信息；通过UTM投影将车身经纬度点转换为笛卡尔坐标；

步骤23：将所述路径地图中的路径点通过UTM投影转换成笛卡尔坐标；

步骤24：在笛卡尔坐标系下计算出车身点前方K米的所述路径点，其中K为自然数；

步骤25：将得到的所述路径点转换成以车辆坐标系原点为原点的一次目标轨迹点S0。

5.如权利要求4所述的低速无人车园区内组合导航方法，其特征在于，所述RTK导航模块还用于将当前RTK-导航定位状态实时发布给融合模块。

6.如权利要求4所述的低速无人车园区内组合导航方法，其特征在于，所述使用雷达SLAM模块生成路径信息1包括以下子步骤：

步骤2A：加载所述路径地图；

步骤2B：通过雷达SLAM定位实时计算车辆的位置信息和/或航向信息；

步骤2C：将当前车身点与地图中所述路径点进行比较，结合航向角搜索出车辆前方K米的点，其中K为自然数；

步骤2D：将得到的路径点转换成以车辆坐标系原点为远点的一次目标轨迹点S1。

7.如权利要求6所述的低速无人车园区内组合导航方法，其特征在于，所述雷达SLAM模块还用于将雷达SLAM定位结果的置信度实时发布给融合模块。

8.如权利要求7所述的低速无人车园区内组合导航方法，其特征在于，所述采样时刻k的R_ii(k)的计算公式为：

其中，m为采样时刻，i为独立的传感器。

9.一种低速无人车园区内组合导航系统，包括用于生成路径信息1的雷达SLAM模块，其特征在于，还包括以下模块：

地图采集模块：用于进行联合地图采集，生成路径地图；

RTK导航模块：用于生成路径信息2；

融合模块：用于接受所述路径信息1和路径信息2，加权融合后生成目标轨迹，融合方法包括以下子步骤：

步骤31：递推计算出采样时刻k的R_ii(k)和R_ij(k)，其中，R_ii(k)表示X_i的自协方差函数，R_ij(k)表示X_i、X_j的互协方差函数，X_i表示传感器i的测量值，X_j表示传感器j的测量值；

步骤32：计算采样时刻k的其中，表示R_ij(k)的均值函数；

步骤33；计算采样时刻k的σ_i²，其中，σ_i²表示各传感器的方差；

步骤34：计算各传感器多次测量值的均值

步骤35：计算此时各传感器的最优加权因子最优加权因子的计算公式为：此时，对应的总方差最小值为其中，n为采样时刻；

步骤36：计算此时的估计值，计算方法为：

其中，为融合后的真值，W_i为各传感器的加权因子；

所述系统使用如权利要求1所述的方法进行组合导航。