(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202211030508.7 (22)申请日 2022.08.26 (71)申请人 中国电子科技 集团公司第五十四研 究所 地址 050080 河北省石家庄市桥西区中山 西路589号 (72)发明人 蔚保国　张京奎　盛传贞　武子谦　 陈永昌　金炜桐　 (74)专利代理 机构 北京方圆嘉 禾知识产权代理 有限公司 1 1385 专利代理师 程华 (51)Int.Cl. G01S 17/06(2006.01) G01S 19/49(2010.01) G01S 19/53(2010.01)G06T 7/33(2017.01) G06T 7/73(2017.01) (54)发明名称 一种室内外无缝定位与3D建图方法及系统 (57)摘要 本发明公开了一种室内外无缝定位与3D建 图方法及系统， 涉及定位及建图领域， 该方法包 括： 对激光雷达数据和惯性测量传感器数据分别 进行数据预处理； 基于去畸变激光雷达数据和预 积分处理后的惯性测量传感器数据， 采用基于图 优化的方式得到载体的第一位姿信息； 采用卡尔 曼滤波方式， 基于GPS数据与预积分处理后的惯 性测量传感器数据计算载体的第二位姿信息由 第二位姿信息对第一位姿信息进行坐标变换； 将 第一位姿信息作为预测， 将坐标变换后的第二位 姿信息作为观测， 采用卡尔曼滤波器进行滤波优 化； 采用雷达惯性里程计对滤波后的位姿信息进 行更新； 根据更新后的位姿信息对去畸变激光雷 达数据进行配准， 生成三维点云地图。 本发明能 提高定位精度。 权利要求书3页 说明书12页 附图3页 CN 115291227 A 2022.11.04 CN 115291227 A 1.一种室内外无 缝定位与3D建图方法， 其特 征在于， 包括： 获取目标空间内载体的激光雷达数据、 惯性测量传感器数据和GP S数据； 对所述激光雷达数据和所述惯性测量传感器数据分别进行数据 预处理， 得到去畸变激 光雷达数据和预积分处 理后的惯性测量传感器数据； 采用雷达惯性里程计， 基于所述去畸变激光雷达数据和所述预积分处理后的惯性测量 传感器数据， 采用基于图优化的方式得到所述载体的第一 位姿信息； 采用卡尔曼滤波方式， 基于所述GPS数据与所述预积分处理后的惯性测量传感器数据 计算所述载体的第二 位姿信息； 基于所述第二位姿信息对所述第一位姿信息进行坐标变换， 得到基于GPS坐标的第二 位姿信息； 将所述第一位姿信息作为预测， 将所述基于GPS坐标的第 二位姿信息作为观测， 采用卡 尔曼滤波器进行 滤波优化， 得到滤波后的位姿信息； 采用所述雷达惯性里程计对所述滤波后的位姿信息进行更新， 得到更新后的位姿信 息； 根据所述更新后的位姿信息对所述去畸变激光雷达数据进行配准， 生成三维点云地 图。 2.根据权利要求1所述的一种室内外无缝定位与3D建图方法， 其特征在于， 所述对所述 激光雷达数据和所述惯性测量传感器数据分别进 行数据预 处理， 得到去畸变激光雷达数据 和预积分处 理后的惯性测量传感器数据， 具体包括： 采用时间戳和惯性传感器的运动轨迹确定在采集各帧所述激光雷达数据时激光雷达 的位置和姿态， 得到每 帧所述激光雷达数据的坐标值； 所述激光雷达数据是采用所述激光 雷达采集到的； 所述惯性测量传感器数据是采用所述惯性传感器采集到的； 通过外参将每帧所述激光雷达数据的坐标值转换至该帧所述激光雷达数据起始时刻 的激光雷达坐标系下， 得到去畸变激光雷达数据； 采用预积分处理的公式对所述激光雷达采集到的关键帧激光雷达数据间的惯性传感 器数据进行预积分处理， 得到惯性传感器数据约束因子； 所述惯性传感器数据约束因子包 括： 激光雷达 关键帧激光雷达数据之间的位置、 速度和旋转约束； 所述关键帧激光雷达数据 为相邻两帧激光雷达数据的运动距离和旋转角度均大于相应的设定阈值的激光雷达数据； 将所述惯性传感器数据约束因子确定为预积分处 理后的惯性测量传感器数据。 3.根据权利要求1所述的一种室内外无缝定位与3D建图方法， 其特征在于， 所述采用雷 达惯性里程计， 基于所述去畸变激光雷达数据和所述预积分处理后的惯性测量传感器数 据， 采用基于图优化的方式得到所述载体的第一 位姿信息， 具体包括： 判断所述去畸变激光雷达数据的中的各帧激光雷达数据的Z轴坐标是否小于预设高度 阈值， 若是， 则将小于预设高度阈值的激光雷达数据删除， 得到删除后的去畸变激光雷达数 据； 利用所述删除后的去畸变激光雷达数据与关键帧激光雷达数据生成的三维点云地图 进行基于最近点迭代算法的匹配， 得到激光雷达约束因子； 所述关键 帧激光雷达数据为相 邻两帧激光雷达数据的运动距离和旋转角度均大于相应的设定阈值的激光雷达数据； 基于所述激光雷达约束因子和所述所述预积分处理后的惯性测量传感器数据进行联权　利　要　求　书 1/3 页 2 CN 115291227 A 2合优化计算， 得到所述第一 位姿信息 。 4.根据权利要求1所述的一种室内外无缝定位与3D建图方法， 其特征在于， 所述采用卡 尔曼滤波方式， 基于所述GPS数据与所述预积分处理后的惯性测量传感器数据计算所述载 体的第二 位姿信息， 具体包括： 根据所述预积分处理后的惯性测量传感器数据， 采用惯性导航系统进行预测， 得到惯 性传感位置、 惯性传感位置姿态和惯性传感位置 速度； 将所述惯性传感位置、 所述惯性传感位置姿态、 所述惯性传感位置速度以及所述GPS数 据中的双差观测值进行卡尔曼滤波， 得到误差修正后的位置、 误差修正后的位姿和 误差修 正后的位姿速度； 所述双差观测值包括伪距的双差观测值和载波相位的双差观测值； 所述 伪距的双差观测值和所述载波相位的双差观测值均由站星差分得到； 将所述误差修正后的位置、 所述误差修正后的位姿和所述误差修正后的位姿速度确定 为所述第二 位姿信息 。 5.根据权利要求1所述的一种室内外无缝定位与3D建图方法， 其特征在于， 所述基于所 述第二位姿信息对所述第一位姿信息进 行坐标变换， 得到基于 GPS坐标的第二位姿信息， 具 体包括： 基于七参数模型对所述第一位姿信息进行坐标转换， 得到基于GPS坐标的第二位姿信 息。 6.一种室内外无 缝定位与3D建图系统， 其特 征在于， 包括： 数据获取模块， 用于获取目标空间内载体的激光雷达数据、 惯性测量传感器数据和GPS 数据； 预处理模块， 用于对所述激光雷达数据和所述惯性测量传感器数据分别进行数据 预处 理， 得到去畸变激光雷达数据和预积分处 理后的惯性测量传感器数据； 第一位姿计算模块， 用于采用雷达惯性里程计， 基于所述去畸变激光雷达数据和所述 预积分处理后的惯性测量传感器数据， 采用基于图优化的方式得到所述载体的第一位姿信 息； 第二位姿计算模块， 用于采用卡尔曼滤波方式， 基于所述GPS数据与所述预积分处理后 的惯性测量传感器数据计算所述载体的第二 位姿信息； 坐标转换模块， 用于基于所述第二位姿信息对所述第一位姿信息进行坐标变换， 得到 基于GPS坐标的第二 位姿信息； 滤波模块， 利用卡将所述第一位姿信息作为预测， 将所述基于GPS坐标的第二位姿信息 作为观测， 采用卡尔曼 滤波器进行 滤波优化， 得到滤波后的位姿信息； 姿态更新模块， 用于采用所述雷达惯性里程计对所述滤波后的位姿信息进行更新， 得 到更新后的位姿信息； 配准模块， 用于根据所述更新后的位姿信息对所述去畸变激光雷达数据进行配准， 生 成三维点云地图。 7.根据权利要求6所述的一种室内外无缝定位与3D建图系统， 其特征在于， 所述预处理 模块， 具体包括： 坐标值确定单元， 用于采用时间戳和惯性传感器的运动轨迹确定在采集各帧所述激光 雷达数据时激光雷达的位置和姿态， 得到每 帧所述激光雷达数据的坐标值； 所述激光雷达权　利　要　求　书 2/3 页 3 CN 115291227 A 3