[发明专利]一种矿井下多传感器融合的外部参数标定及精准定位方法

权利要求书

1.一种矿井下多传感器融合的外部参数标定方法，其特征在于，所述多传感器包括激光雷达传感器、惯性测量单元和超宽带模组，所述超宽带模组包括UWB移动节点和若干UWB锚节点，所述激光雷达传感器、惯性测量单元和UWB移动节点分别设置在井下移动设备上，所述若干UWB锚节点分布于井巷内不同位置；该外部参数标定方法如下：

(1)获取井巷内各UWB锚节点在世界坐标系下的绝对位置，获取各个传感器的观测信息；

(2)控制井下移动设备进行包含一定旋转的运动，确定激光雷达传感器与惯性测量单元之间的旋转外参，并自动判断旋转标定过程是否完成；

通过下式确定激光雷达传感器与惯性测量单元之间的旋转外参：

其中，为相邻时间间隔惯性测量单元的相对旋转量；为待估计的激光雷达传感器与惯性测量单元之间的旋转外参；为相邻时间间隔激光雷达传感器的相对旋转量；

判断旋转标定过程是否完成的方法如下：

收集N对激光雷达传感器的观测和对应时间间隔内的惯性测量单元的观测，构造超定方程：

其中：

其中，为的四元数表达，Q_N为超定矩阵，λ_i表征第i对观测的噪声和外点系数，和分别是和的四元数表达，是四元数前三个元素q_xyz的反对称矩阵，q_w为四元数的第四个元素，Ι₃为3×3的单位矩阵；

计算超定矩阵Q_N的第二小的奇异值σ^min2，与预先设定的阈值σ^threshold进行比较，如果满足σ^min2＞σ^threshold，则超定方程有解，终止旋转标定过程；

(3)构建基于滑动窗口的线性估计器，估计重力的方向、各UWB锚节点在局部坐标系下的位置、激光雷达传感器与惯性测量单元之间的平移外参、UWB移动节点与惯性测量单元之间的平移外参、惯性测量单元在局部坐标系下的位置和速度，并自动判断线性估计器是否收敛；

以滑动窗口内第一帧激光雷达传感器观测对应的惯性测量单元的位姿作为起点构建局部坐标系，使用滑动窗口确定所使用的所有传感器观测，线性估计器通过最小化所有传感器观测残差的马氏范数之和确定待估计的变量：

其中，χ为待估计变量：

其中，x_n,x_n+1,…,x_n+N表示惯性测量单元的状态，包含位置和重力的方向下标n指代惯性测量单元，下标N是当前滑动窗口中惯性测量单元的状态数量；为激光雷达传感器与惯性测量单元之间的平移外参；为UWB移动节点与惯性测量单元之间的平移外参；p_m,p_m+1,…,p_m+M表示UWB锚节点在局部坐标系下的位置，下标m指代UWB观测，下标M是当前窗口中所有可用的UWB观测数量；r_P为边缘化后的先验因子残差项，H_p为边缘化后先验因子残差项的Jacobian矩阵；为惯性测量单元观测残差项，采用预积分方法进行构建，为惯性测量单元观测方程，为对应的协方差矩阵，B为滑动窗口中所有的惯性测量单元预积分观测数量；为激光雷达传感器观测残差项，通过拟合临近点到子图中的平面，利用点到面的距离作为匹配度量进行构建，为激光雷达传感器观测方程，P_s为对应的协方差矩阵，A为所有与子图进行匹配的雷达观测的数量；为UWB观测残差项，利用UWB移动节点到UWB锚节点的距离观测构建，为UWB观测方程，为对应的协方差矩阵，C为滑动窗口中所有UWB观测数量；

求解线性方程：

(Λ_P+Λ_B+Λ_L+Λ_U)χ＝(b_P+b_B+b_L+b_U)

其中，Λ_B,b_B是惯性测量单元观测的信息矩阵和向量；Λ_L,b_L是激光雷达传感器观测的信息矩阵和向量；Λ_U,b_U是UWB观测的信息矩阵和向量；Λ_p,b_p是先验因子的信息矩阵和向量；

判断线性估计器是否收敛的方法如下：

计算(Λ_P+Λ_B+Λ_L+Λ_U)^-1的最大奇异值λ^max，将其与预设阈值λ^threshold进行比较，若λ^max＜λ^threshold则判断线性估计器收敛；

(4)利用估计出的UWB锚节点在局部坐标系下的位置坐标，与UWB锚节点在世界坐标系下的绝对位置坐标进行对齐，确定局部坐标系与世界坐标系之间的外参变换；

将各UWB锚节点在世界坐标系下的的绝对坐标转化为UTM坐标系下的坐标P_i，同时获取局部坐标系下各UWB锚节点的位置估计值P_i，构建误差方程：

其中，M为当前滑窗中可用的UWB观测数量，为局部坐标系与世界坐标系的外参变换；

将新观测到的第M+1个位置坐标添加到上述误差方程，得到e_M+1，利用收敛条件迭代求解外参变换其中，Δ为归一化变量，ε为预设阈值；

(5)输出世界坐标系下所有估计的参数值。