[发明专利]毫米波雷达惯性组合的火场烟雾场景定位建图方法与装置

权利要求书

1.一种毫米波雷达惯性组合的火场烟雾场景定位建图方法，其特征在于，将惯性器件与毫米波雷达搭载在进入火场烟雾场景的人体上，所述定位建图方法包括如下步骤：

步骤1，获取惯性数据与毫米波雷达点云数据；

步骤2，基于惯性数据得到人体的姿态信息，并基于人体运动学的约束与惯性数据进行人体的步长估计，并确定人体的位置信息；

步骤3，对稀疏高噪声的毫米波雷达点云数据进行提升处理，得到稠密化点云数据；

步骤4，通过人体的姿态信息与位置信息将稠密化点云数据映射到全局坐标系下构建地图，实现人体在烟雾环境下的实时定位与建图定位；

步骤2中，所述基于惯性数据得到人体的姿态信息，具体为：

基于惯性数据得到人体在两个时刻帧之间的姿态变化量，为：

式中，为单位矩阵，为旋转矢量，为惯性数据中的角速度信息，为单位时间，为旋转矢量的叉乘矩阵；

基于姿态变化量更新姿态矩阵，为：

式中，为时刻惯性器件相对于全局坐标系的姿态矩阵，为时刻惯性器件相对于全局坐标系的姿态矩阵；

根据姿态矩阵得到在时刻人体的航向角，依次类推得到人体在任意时刻的航向角，即姿态信息。

2.根据权利要求1所述的毫米波雷达惯性组合的火场烟雾场景定位建图方法，其特征在于，步骤2中，所述基于人体运动学的约束与惯性数据进行人体的步长估计，具体为：

获取惯性数据得到人体运动过程中的加速度值序列，将其转换为加速度值变化曲线，将加速度值变化曲线中两个波峰之间对应的区域记为一步；

计算得到任意一步的步长估计值，为：

式中，为与人体身高相关的比例系数，为这一步对应的加速度值序列中的最大值，为这一步对应的加速度值序列中的最小值。

3.根据权利要求2所述的毫米波雷达惯性组合的火场烟雾场景定位建图方法，其特征在于，步骤2中，所述确定人体的位置信息，具体为：

式中，为人体在时刻的位置信息，为人体在时刻的位置信息，为人体在时刻的航向角，为东向坐标，为北向坐标。

4.根据权利要求1至3任一项所述的毫米波雷达惯性组合的火场烟雾场景定位建图方法，其特征在于，步骤3中，所述对稀疏高噪声的毫米波雷达点云数据进行提升处理，具体为：

构建基于编码解码结构的点云数据稠密提升网络，采集单通道的毫米波雷达点云数据和激光雷达点云数据构建数据集，以稠密的激光雷达点云作为标签训练该点云数据稠密提升网络，使点云数据稠密提升网络能够将稀疏的毫米波雷达点云映射到稠密化的点云，其中，采集的激光雷达稠密点云记为，与基于毫米波雷达数据生成的稠密点云比较它们之间的差值，设为点云重建损失函数，为：

为了生成更真实的稠密点云，去除噪声，引入对抗生成损失函数，另外训练一个辨别网络，用于判定输入的点云是否为稠密点云，可以得到如下点云对抗重建损失函数，为：

优化总的损失函数，训练点云数据稠密提升网络，并将训练好的点云数据稠密提升网络用于部署，实时输入测试稀疏毫米波雷达数据，输出稠密化的点云，为：

式中，为稀疏的毫米波雷达点云数据，为稠密化点云数据。

5.根据权利要求4所述的毫米波雷达惯性组合的火场烟雾场景定位建图方法，其特征在于，所采集的单通道的毫米波雷达点云数据为火场烟雾场景下的毫米波雷达点云数据，以使得点云数据稠密提升网络能够适应烟雾场景下的点云稠密化过程。