[发明专利]一种隧道内基于激光和UWB定位方法、装置

权利要求书

1.一种隧道内基于激光和UWB定位方法，应用于安装移动端UWB和激光传感器的可移动平台，其特征在于，所述方法包括：

获取关于隧道的地图，所述的地图上设有固定端UWB；其中，所述地图根据隧道的线路属性分成多个子地图；并获取各个所述的子地图和所述的地图的位姿转换关系；所述地图根据隧道的线路属性分成多个子地图包括：所述地图根据隧道的直道和弯道分成多个子地图；

获取所述的移动端UWB安装于所述的可移动平台的外参信息；

获取所述的移动端UWB和所述的固定端UWB之间的距离信息；根据所述的外参信息，以及所述距离信息接近于所述移动端UWB与所述固定端UWB的实际距离，构建第一约束函数；

同时通过激光传感器获取环境点云数据；根据所述的环境点云数据接近于所述子地图的坐标系的坐标点集，构建第二约束函数；

获取可移动平台在所述的地图中的预测位姿，并确定所述可移动平台所在的子地图；根据所述可移动平台在所述子地图的预测位姿接近于所述可移动平台在所述子地图的实际位姿，构建第三约束函数；

基于所述的第一约束函数、所述的第二约束函数和所述的第三约束函数，按照权重进行联合优化处理，得到所述可移动平台在所述子地图中的位姿；

根据所述子地图在所述地图的位姿转换关系，以及所述可移动平台在子地图中的位姿，计算可移动平台位于所述地图中的当前位姿。

2.根据权利要求1所述的方法，其特征在于，所述的所述地图根据隧道的线路属性分成多个子地图，包括如下步骤：若所述的子地图为直道的子地图，所述子地图的坐标系的第一坐标轴设定为与隧道方向平行；若所述的子地图为弯道的子地图，所述子地图的坐标系的第一坐标轴的方向与一个相邻的直道的子地图方向一致。

3.根据权利要求1所述的方法，其特征在于，所述的构建第一约束函数，包括如下计算步骤：

根据所述的移动端UWB安装于所述的可移动平台的外参信息，得到所述移动端UWB的坐标系到所述可移动平台的坐标系的外参转换矩阵；

构建所述可移动平台的坐标系到所述子地图的坐标系的第一转换矩阵函数；

根据所述的外参转换矩阵和第一转换矩阵函数，得到所述移动端UWB的坐标系到所述子地图的坐标系的第二转换矩阵函数，获得所述移动端UWB到所述子地图的坐标系的第二平移矩阵函数；

获取至少一个所述固定端UWB的坐标系到所述子地图的坐标系的转换矩阵，得到所述固定端UWB的坐标系到所述子地图的坐标系的平移矩阵；

根据所述距离信息接近于所述第二平移矩阵函数与所述固定端UWB的坐标系到所述的地图的坐标系平移矩阵的平移量之差，构建第一约束函数：

其中，ε₁(κ)为基于第k个固定端UWB关于可移动平台位姿的第一约束函数；map_j表示第j个子地图的坐标系，所述第j个子地图是所述可移动平台所在的子地图，b表示所述的可移动平台的坐标系，u_b表示所述的移动端UWB的坐标系，表示外参转换矩阵，表示第一转换矩阵函数，()t表示平移矩阵，表示所述的第二平移矩阵函数，表示所述第k个所述的固定端UWB的坐标系和第j个子地图的坐标系的平移矩阵，表示所述的移动端UWB与第k个所述的固定端UWB之间的距离。

4.根据权利要求3所述的方法，其特征在于，所述的构建第二约束函数，包括如下步骤：

根据基于所述的环境点云数据接近于所述子地图的坐标系的坐标点集，构造第二约束函数：

其中，map_j表示第j个子地图的坐标系，所述第j个子地图是所述的可移动平台所在的子地图，b表示可移动平台的坐标系，表示从可移动平台的坐标系到第j个子地图的坐标系的转换矩阵，p_x表示激光传感器测得的第x个当前激光点在可移动平台的坐标系下坐标，表示与p_x最接近的第j个子地图的坐标点集中的点。