(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202211063976.4 (22)申请日 2022.08.31 (71)申请人 中国科学技术大学 地址 230026 安徽省合肥市包河区金寨路 96号 申请人 国网山东省电力公司电力科 学研究 院　 国网安徽省电力有限公司电力科 学 研究院 (72)发明人 董二宝　王梓名　秦佳峰　张磊　 张用　赵洺哲　吴少雷　吴凯　 (74)专利代理 机构 北京科迪生专利代理有限责 任公司 1 1251 专利代理师 关玲(51)Int.Cl. G01C 21/16(2006.01) G01C 21/20(2006.01) G06V 10/80(2022.01) G06T 7/73(2017.01) (54)发明名称 一种高空作业机械的视觉与惯导融合定位 装置及方法 (57)摘要 本发明提供一种高空作业机械的视觉与惯 导融合定位装置及方法， 装置包括固定的云台支 架、 单目相机、 IMU惯性传感器、 电源、 计算主控单 元和时间同步系统。 本发明通过在高空作业平台 中采集单目相机和IMU惯性传感器数据后， 先对 采集到的上述数据进行特征提取， IMU惯性传感 器进行预积分。 在通过特征匹配后进行云台获取 的当前数据帧的旋转矩阵和平移的求解。 由于高 空作业过程中数据较多， 在本发 明中采用了滑动 窗口进行关键帧的数量限制。 最后进行整个高空 作业的环 境构建以及作业位置定位的整体优化。 本发明添加了IMU预积分因子去除了帧帧匹配部 分， 运动估计结果更加鲁棒， 实现更好的位姿估 计。 权利要求书2页 说明书5页 附图5页 CN 115420283 A 2022.12.02 CN 115420283 A 1.一种高空作业机械的视觉与惯导融合定位装置， 其特征在于： 包括固定的云台支架、 单目相机、 IMU惯性传感器、 电源、 计算主控 单元和时间同步系统； 所述装置与斗臂车升降平 台的末端刚性固定， 随工作斗进行移动， 当斗臂车开始工作， 操纵液压臂控制旋转和升降， 所述装置在升降开始时刻同步开机， 所述单目相机以固定帧率曝光感知环境， 所述IMU惯 性 传感器进行预积分获取旋转和位移增量， 将图像数据和IMU惯性传感器数据反馈到所述计 算主控单元； 所述时间同步系统用于保证单目相机曝光图像时间与IMU惯性传感器数据采 集时间保持同步。 2.根据权利要求1所述的一种高空作业机械的视觉与惯导融合定位装置， 其特征在于： 所述单目相机为全局快门相机或卷帘快门相机， 根据视场角不同分为针孔相机或鱼眼相 机。 3.根据权利要求1所述的一种高空作业机械的视觉与惯导融合定位装置， 其特征在于： 所述IMU惯 性传感器通过PPS信号与卫星时钟进行同步， 信号通过分频器控制所述单目相机 曝光， 所述单目相机和IMU惯性传感器信号触发时延控制在ns级。 4.根据权利要求1所述的一种高空作业机械的视觉与惯导融合定位装置， 其特征在于： 所述固定帧率大于20 Hz。 5.一种根据权利要求1 ‑4之一所述的高空作业机械的视觉与惯导融合定位装置的定位 方法， 其特征在于： 通过设备安装与标定模块、 时间同步模块和高空定位建图算法模块 实现 所述定位方法； 通过所述设备安装与 标定模块设计云台支架， 将单目相机与IMU惯性传感器 刚性连接， 确保在高空作业平台升降的过程中， 所述单目相机和IMU惯性传感器之间的相对位置不会 发生改变， 同时将所示云台支架与升降的高空作业平台的工作斗刚性固连， 确保IMU惯 性传 感器如实反映升降的高空作业平台的工作斗的运动位姿； 通过所述 时间同步模块将单目相机曝光图像时间与IMU惯性传感器数据采集时间保持 同步； 通过所述高空定位建图算法模块用于 融合处理平台上相机感知的作业环境图像及IMU 惯性传感器感知的运动数据， 输出定位轨迹及工作斗的实时位姿， 所述高空定位建图算法 模块包括高空采集数据预处理模块、 算法初始化模块、 算法非线性优化模块、 重定位模块、 全局位姿图优化模块和位姿图管理模块。 6.根据权利要求5所述的定位方法， 其特征在于： 安装完毕后， 进行所述IMU惯性传感器 和单目相机的内外参标定， 建立单目相机的内参模型， 建立IMU惯性传感器的内参模型， 标 定高斯白噪声、 随机游走误差， 通过最小二乘 等方法构建所述IMU惯性传感器和单目相机 之 间的旋转平 移矩阵。 7.根据权利要求5所述的定位方法， 其特征在于： 所述时间同步模块的第一级为GNSS接 收模块， 通过卫星获取纳秒级精度的UTC真值时间数据， 通过FPGA模块转化为PPS信号， 将 PPS信号接入IMU/INS模块， 使IMU惯性传感器的惯性数据时间戳与UTC真值时间保持一致， 同时将PPS信号接入分频器模块， 根据所需 帧率对单目相机进 行触发， 所述单目相机触发信 号在整秒边沿与P PS信号对齐， 两信号时延在几十ns之内。 8.根据权利要求5所述的定位方法， 其特征在于： 所述高空采集数据预处理模块通过云 台上的感知数据采集装置将采集的图像和IMU惯性传感器的数据进行 预处理。权　利　要　求　书 1/2 页 2 CN 115420283 A 29.根据权利要求5所述的定位方法， 其特征在于： 通过所述算法初始化模块进行滑动窗 口的选择、 设置尺度因子、 将视觉得到的位姿， 转换到IMU惯性传感器坐标系下以及视觉惯 性校准。 10.根据权利要求5所述的定位方法， 其特征在于： 所述算法非线性优化模块用于添加 约束以保证算法稳定运行， 所述约束包括： IMU惯性传感器残差约束、 滑动窗口中特征点在 单目相机下视 觉重投影残差、 Marg边 缘化残差部分。 11.根据权利要求5所述的定位方法， 其特征在于： 所述重定位模块用于进行相同位置 的重新利用， 具体包括： (1)回环检测， 根据词袋模型估计相似度进行检测， 同时添加500角点， 每帧仅仅保留该 特征点丢弃图像信息； (2)回环检测成功， 剔除外点， 使用RANSAC和F矩阵测试是否是 回环， 采用随机抽样一致 算法和透 视点算法恢复旋转矩阵和位移； (3)紧耦合重 定位， 将当前的滑动窗口和获取的位姿图对齐， 进行重 定位优化。 12.根据权利要求5所述的定位方法， 其特征在于： 所述全局位姿图优化模块用于添加 关键帧， 当关键帧在局部优化时被边缘化后， 则加入全局位姿图， 所述关键帧有两种连接方 式： (1)顺序帧， 顺序连接； (2)该关键帧在全局位姿图中具有回环， 则和回路闭合帧连接 。 13.根据权利要求5所述的定位方法， 其特征在于： 所述位姿图管理模块用于避免无限 大的位姿图， 将具有回环的关键帧保留， 丢弃 该帧附近的关键帧。权　利　要　求　书 2/2 页 3 CN 115420283 A 3