[发明专利]一种面向遮挡测评环境的多传感高精度融合测量方法

权利要求书

1.一种面向遮挡测评环境的多传感高精度融合测量方法，其特征在于：在GNSS信号受到遮挡而无法实现可靠定位的环境下，针对单一激光SLAM存在的误差累积以及点云稀疏场景下定位精度下降的问题，通过在路侧布设主动特征来增加激光雷达的观测量，以提升激光雷达的定位精度；同时引入UWB传感器的距离观测量来识别激光雷达扫描到的主动特征编号，并结合车载惯性传感器来提升定位频率；最后通过因子图算法来实时准确地估计出车辆的位置信息，具体步骤如下：

步骤1：布置路侧主动特征与UWB基站

在激光雷达点云稀疏的区域每隔10-20m等间距的布置主动特征及UWB基站；其中主动特征为低成本的白色圆柱体泡沫，将主动特征安装在相应支架上，在每个主动特征的顶部都安装相应的UWB基站；以正东方向为OX，正北方向为OY，地球表面一个固定点为原点建立坐标系；各个主动特征及UWB基站在坐标系下的位置可通过提前标定获取；将UWB标签和激光雷达安装在行驶车辆的顶端；

步骤2：激光雷达观测量识别

根据步骤1中的布局，在同一时刻UWB标签所能接收到的观测信息为r₁，r₂，...，r_N，其中r_N表示第N个主动特征与车辆之间的距离值；激光雷达检测到主动特征后所能解算的观测信息为d、θ，其中d为路侧某个未知的主动特征与激光雷达之间的距离值，θ为该主动特征与激光雷达之间相对于正北方向的夹角的绝对值；由于激光雷达对主动特征进行检测时无法判断该主动特征的编号，而主动特征项部的UWB恰好弥补了这一缺点；通过将激光雷达解算的距离信息d与UWB标签接收到的距离信息作对比，若|d-r_i|＜ε，其中，i＝1，2，...，N，ε为事先设定的阈值，则可判定当前激光雷达检测的距离d与角度θ隶属的主动特征编号i；

步骤3：构建多传感器组合导航的因子图模型

假设Λ表示车辆的导航状态，包括三维位置X、三维速度V信息；定义车辆的启动时刻为初始时刻，则初始时刻到当前时刻t_k的导航状态可表示为：

相应的联合概率分布函数可表示为：

P(Λ_k|Z_k) (2)

其中Z_k为初始时刻到当前时刻t_k接收到的所有观测值；

则待估计的导航状态的最大后验估计可表示为：

联合概率分布式(2)可以因式分解为一个先验信息和独立过程及测量模型：

上式中P(Λ₀)表示所有可用的先验信息；表示Λ_i中与测量模型相关的变量；表示IMU的测量值，包含比力和角速率；表示IMU的后验概率密度；P(V_i|V_i-1)表示速度变量的后验概率密度；表示除IMU外所有传感器测量值的后验概率密度的乘积；

上述的因式分解可以表示成因子图模型，作为一种二分图模型，因子图包含了因子节点和状态节点两类；因子图中的每个因子节点都可以表示为式(4)中的一个独立项，并且每个因子f_i(·)表示为一个应当最小化的误差函数，假设这个误差函数为则因子f_i(·)可定义为：

其中d(·)表示代价函数；假设各个传感器的误差均服从高斯分布，则因子f_i(·)满足如下形式：

上式中，表示马氏距离，∑_i为传感器的误差协方差矩阵；则传感器的测量模型因子可定义为：

式中，h(·)为非线性测量函数，Z_i为传感器实际的测量值，因此式(4)的最大后验估计问题就变成了如下的非线性最小二乘问题：

所构建的因子图模型中，X_k和V_k分别代表k时刻车辆的位置和速度；其中X_k＝[xe_k，xn_k，xu_k]^T分别代表k时刻车辆的东向位置、北向位置和天向位置；V_k＝[ve_k，vn_k，vu_k]^T分别代表k时刻车辆的东向速度、北向速度和天向速度；f^Prior代表先验因子，f^MM代表运动模型因子，f^UWB代表UWB因子，f^v代表速度因子，f^SLAM代表激光雷达SLAM因子，f^AF代表激光雷达检测到的主动特征因子；通过建立各因子的误差函数，就能求得各个时刻车辆位置的最大后验估计：

子步骤1：IMU等效因子建立

IMU因子包含了运动模型因子f^MM和速度因子f^v，通过接收来自IMU的测量值来定义这两种因子；其中f^b与ω^b分别代表机体坐标系下载体的比力和角速度；因此，车辆的运动模型可表示为：

车辆的速度模型可表示为：

其中，ae、an、au分别为捷联惯导算法转换的载体在东北天坐标系下的东向、北向和天向加速度；和分别为车辆运动模型和速度模型的误差协方差矩阵；则IMU等效因子的误差函数可表示为：

子步骤2：UWB因子的建立

在每个采样时间k，车载UWB标签接收到的距离测量值可表示为r_k＝{r_k，1，r_k，2，...，r_k，j}，其中r_k，j表示k时刻标签接收到的路侧第j个基站的距离值；因此，第k个基站的观测方程可表示为：

其中，X_k＝[xe_k，xn_k，xu_k]^T表示k时刻车辆的三维位置；表示事先标定好的路侧第j个基站的三维坐标；||·||表示车辆位置与基站位置之间的欧氏距离；则UWB距离的观测方程可表示为：

其中为UWB因子的误差协方差矩阵；则UWB因子的误差函数为：

子步骤3：主动特征因子的建立

当激光雷达扫描到路侧主动特征后，将产生d和θ两个观测量，其中d和θ分别为路侧某个未知的主动特征与激光雷达之间的距离值以及该主动特征与激光雷达之间相对于正北方向的夹角的绝对值；通过步骤2可确定该主动特征的编号；因此，主动特征观测方程可表示为：

由于UWB基站安装在主动特征顶部，因此上式中的也代表路侧第j个主动特征的三维坐标；则主动特征的测量模型可表示为：

其中a_k，j＝[d_k，j，θ_k，j]^T，为主动特征因子的误差协方差矩阵；则主动特征因子的误差函数为：

子步骤4：激光雷达SLAM因子的建立

激光雷达SLAM技术通过点云匹配算法在提前构建好的地图中进行车辆定位，它能实时地输出车辆的位置信息；在k时刻激光雷达SLAM输出的观测量可表示为其中分别代表SLAM输出的车辆的东向位置、北向位置和天向位置信息；则SLAM的测量模型可表示为：

则SLAM因子的误差函数为：

通过计算IMU等效因子、UWB因子、主动特征因子和激光雷达SLAM因子的误差函数，就能得到k时刻系统状态的总体误差函数

通过最小化各等效因子的误差函数，即可求得每一时刻车辆的最优位置估计，实现GNSS失效情况下车辆位置的高精度测量。