(19)国家知识产权局 (12)发明 专利 (10)授权公告 号 (45)授权公告日 (21)申请 号 202210694055.1 (22)申请日 2022.06.20 (65)同一申请的已公布的文献号 申请公布号 CN 114862932 A (43)申请公布日 2022.08.05 (73)专利权人 安徽建筑大学 地址 230601 安徽省合肥市经济技 术开发 区紫云路292号 (72)发明人 刘伟　蔡晨星　高婷　毛如轩　 陈雪辉　李昊　黄磊　景甜甜　 (74)专利代理 机构 合肥市泽信专利代理事务所 (普通合伙) 3414 4 专利代理师 方荣肖 (51)Int.Cl. G06T 7/521(2017.01)G06V 10/80(2022.01) G06T 7/73(2017.01) G01C 21/20(2006.01) G01S 17/86(2020.01) (56)对比文件 CN 111077549 A,2020.04.28 CN 108917 759 A,2018.1 1.30 CN 111521204 A,2020.08.1 1 CN 114459471 A,202 2.05.10 US 2017016870 A1,2017.01.19 CN 112859051 A,2021.0 5.28 王思鹏等.基 于改进MSCK F的无人机室内定 位方法. 《浙江大 学学报 （工学版） 》 .202 2, 审查员 江汉琼 (54)发明名称 基于BIM全局定位的位姿修正方法及运动畸 变矫正方法 (57)摘要 本发明涉及基于BIM全局定位的位姿修正方 法及运动畸变矫正方法。 基于BIM全局定位的位 姿修正方法包括将BIM全局点云地图在空间上划 分到不同三维网格中； 统计每个三维网格中点云 数量， 计算三维网格多维正态分布参数， 将激光 雷达实时数据的原始坐标由变换矩阵T变换到 BIM全局点云地图坐标系中； 根据多维正态分布 参数计算激光雷达转换点的概率密度， 根据概率 密度建立ND T配准函数； 对配准函数求解； 发布机 器人在BIM全局点云地图下的实时定位位姿； 将 激光雷达预测位姿与实时定位位姿进行ESKF融 合， 获得修正位姿。 本发明利用提取的BIM全局点 云地图与激光雷达实时 帧的点云数据进行配准， 消除局部传感器 推算的位姿误差 。 权利要求书2页 说明书6页 附图4页 CN 114862932 B 2022.12.30 CN 114862932 B 1.基于BIM全局定位的位姿修正方法， 其用于修正IMU传感器预测位姿， 其特征在于， 所 述位姿修正方法包括以下步骤： 获取BIM全局点云地 图， 将所述BIM全局点云地 图在空间上划分到不同的三维网格中； 统计每个所述三维网格中点云数量， 计算所述三维网格多维正态分布参数， 即点云均值与 协方差矩阵， 其中， 点云均值 为 协方差矩阵为 Σ n表示划分的三维网格数量， k表示n的序号， x表示网格内 所有扫描点的位置， T表示转置， T1表示由激光雷达坐标系到BIM全局点云地图坐标系的变 换矩阵； 将激光雷达的实时数据的原始坐标由所述变换矩阵T1变换到所述BIM全局点云地图坐 标系中， 得到位于BIM全局点云地图的坐标， 具体变换为[x ′i， y′i， 1]T＝T1[xi， yi， 1]T， 变换 矩阵为 [xi,yi]为第i个雷达实时数据在原始坐标系下的坐 标， [x′i， y′i]经变换矩阵T1转移到所述BIM全局点云地图坐标系后的坐标， φ表示旋转角 度， tx表示BIM全局点云地图坐标系下激光雷达坐标系原点到B IM全局点云地图坐标系下横 坐标x方向上的距离， ty表示相应纵坐标y方向上的距离； 根据所述三维网格多维正态分布参数计算所述激光雷达的转换点的概率密度， 概率密 度为:P(W): 根据所述概率密度建立NDT配准函数， 配准函数 为： Wi表示第i个转换点， 对所述配准函数求解， 寻找一组变换参数令所述配准函数最大， 设置迭代阈值与最大 迭代次数， 达 到收敛条件时结束迭代； 发布机器人在所述BIM全局点云地图下的实时定位 位姿； 获取所述激光雷达的预测位姿， 将所述激光雷达的预测位姿与 所述实时定位位姿通过 误差状态卡尔曼 滤波器进行融合， 获得机器人的修 正位姿。 2.根据权利 要求1所述的基于BIM全局定位的位姿修正方法， 其特征在于， 预先建立IMU 传感器运动学模型， 通过所述运动学模型与IMU传感器实时采集数据推测出所述激光雷达 预测位姿。 3.根据权利要求2所述的基于BIM全局定位的位姿修正方法， 其特征在于， 获取精确的 建筑物信息模 型BIM， 对BIM的特征进行提取， 得到三维空间特征点， 将所述三 维空间特征点 转化成所述BIM全局点云地图。 4.根据权利要求3所述的基于BIM全局定位的位姿修正方法， 其特征在于， 所述特征为 构件信息， 其包括大小、 空间位置、 形状。 5.根据权利要求4所述的基于BIM全局定位的位姿修正方法， 其特征在于， 利用优化算 法对所述配准 函数求解， 所述优化 算法包括牛 顿迭代算法。 6.基于BIM全局定位的运动畸变矫 正方法， 所述 运动畸变矫 正方法包括以下步骤：权　利　要　求　书 1/2 页 2 CN 114862932 B 2获取激光雷达单帧扫描时间内的机器人的位姿， 对所述位姿做线性近似和插值处理， 获取补偿变换矩阵； 将单帧时间内所有激光点基准坐标系下的坐标通过所述补偿变换矩阵进行矫正， 得到 矫正后的激光 点坐标， 封装激光 点坐标的点云数据， 完成矫 正； 其特征在于， 在获取所述补偿变换矩阵前， 对所述位姿进行修正， 所述位姿的位姿修正方法采用 如 权利要求1 ‑5中任意一项所述的基于BIM全局定位的位姿修 正方法。 7.根据权利要求6所述的基于BIM全局定位的运动畸变矫正方法， 其特征在于， 根据激 光雷达有序点云中每个点云的水平角计算出单个点云相对于单帧初始 时刻的激光雷达坐 标的相对时间， 根据相对时间对经过线性近似变换 的位姿变换做插值求解， 计算出每个激 光点在激光雷达坐标 下的补偿变换矩阵， 得到所述补偿变换矩阵。 8.根据权利要求7所述的基于BIM全局定位的运动畸变矫正方法， 其特征在于， 获取经 过线性近似变换的位姿变换包括以下步骤： 获取修正后的位姿数据， 对齐位姿数据与激光雷达点云的时间， 对单帧点云时间内的 位姿变换做线性近似处 理， 得到所述经 过线性近似变换的位姿变换。 9.根据权利要求8所述的基于BIM全局定位的运动畸变矫正方法， 其特征在于， 以单帧 初始时刻的激光雷达位置为原点建立基准坐标系O0XY， 单帧其余时刻 位置建立的坐标系为 激光雷达坐标系OtXY。 10.根据权利要求9所述的基于BIM全局定位的运动畸变矫正方法， 其特征在于， 获取激 光雷达点云数据， 将激光雷达点云数据按雷达线束和扫描时间转化为有序点云， 得到所述 激光雷达有序点云。权　利　要　求　书 2/2 页 3 CN 114862932 B 3