(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202211526026.0 (22)申请日 2022.12.01 (71)申请人 成都信息工程大学 地址 610000 四川省成 都市西南 航空港经 济开发区学府路1段24 号 申请人 国网四川省电力公司电力科 学研究 院 (72)发明人 谢晓娜　徐智勇　常政威　陈明举　 邓元实　熊兴中　谢正军　蒲维　 吴杰　丁宣文　张葛祥　张江林　 刘甲甲　王振玉　 (74)专利代理 机构 成都行之专利代理事务所 (普通合伙) 51220 专利代理师 唐邦英(51)Int.Cl. G01C 21/00(2006.01) G01C 21/16(2006.01) G01C 21/20(2006.01) G01S 5/02(2010.01) G06T 7/73(2017.01) H04W 4/02(2018.01) (54)发明名称 一种基于UWB辅助的主动定位方法、 设备和 介质 (57)摘要 本发明公开了一种基于UWB辅助的主动定位 方法、 设备和介质， 涉及定位技术领域， 通过环境 图像数据的点特征和线特征、 拍摄 设备的角速度 和加速度， 构建双目视觉和惯 性里程计模型进行 初次定位， 引入回环检测与重定位算法， 对目标 环境图像进行回环修正与重定位， 引入优化的超 宽带定位算法， 对目标环境图像中的待测标签与 基站间的距离进行优化定位， 结合对目标环境图 像进行回环 修正与重定位数据和优化定位数据， 基于双目视觉和惯性里程计模型构建视觉/惯 性/超宽带组合模型， 通过视觉/惯性/超宽带组 合模型对初次定位数据进行修正， 获得精确定 位， 减小位置误差， 提高估计精度， 获得更为精确 的载体位姿。 权利要求书2页 说明书8页 附图2页 CN 115540854 A 2022.12.30 CN 115540854 A 1.一种基于UWB辅助的主动定位方法， 其特 征在于， 包括以下 具体步骤： S1、 获取目标环境图像， 获得环境图像数据的点特征和线特征， 获取拍摄设备的角速度 和加速度； S2、 构建双目视觉和惯性里程计模型， 结合环境图像数据的点特征和线特征、 拍摄设备 的角速度和 加速度， 进行初次定位； S3、 引入回环检测与重定位算法， 对目标环境图像进行回环修正与重定位， 引入优化的 超宽带定位 算法， 对目标环境图像中的待测标签与基站间的距离进行优化定位； S4、 结合对目标环境图像进行回环修正与重定位数据和优化定位数据， 基于双目视觉 和惯性里程计模 型构建视觉/惯 性/超宽带 组合模型， 通过视觉/惯 性/超宽带 组合模型对初 次定位数据进行修 正， 获得精确定位。 2.根据权利要求1所述的基于UWB辅助的主动定位方法， 其特征在于， 所述回环检测与 重定位算法具体包括： 获取当前输入的环境图像数据， 若当前输入的环境图像数据被判断为当前关键帧： 获取当前关键帧的特征， 将当前关键帧对应的特征与滑动 窗口中的特征进行匹配， 建 立重投影误差； 对当前关键帧的特 征三角化后与回环的关键帧之间建立特 征及位姿的关系。 3.根据权利要求2所述的基于UWB辅助的主动定位方法， 其特征在于， 所述当前输入的 环境图像数据被判断为当前关键帧包括： 对当前窗口中关键帧以外的所有关键帧进行检测， 检测是否存在回环： 若不存在回环， 则按照视 觉/惯性里程计算法进行基于滑动窗口 的位姿估计； 若存在回环， 将回环关键帧确定为当前关键帧， 将当前关键帧加入当前的滑动 窗口之 中。 4.根据权利要求2所述的基于UWB辅助的主动定位方法， 其特征在于， 还包括对所述重 投影误差进行误差消除： 获取当前回环帧中的图像数据， 确定当前回环帧的重投影误差的协方差矩阵， 确定当 前回环帧建立的特 征的重投影误差， 采用最小二乘法对获得 载体的位姿信息进行优化。 5.根据权利要求2所述的基于UWB辅助的主动定位方法， 其特征在于， 所述对当前关键 帧的特征三角化后与回环的关键帧之间建立特 征及位姿的关系包括： 将一个关键帧与其连续的前后两个关键帧及其对应的回环关键帧建立 边的关系； 将建立关系的三个关键帧加入位姿图中， 对所有关键帧的位姿进行优化， 获得优化后 的当前关键帧与回环的关键帧之间构建的相对位姿残差 。 6.根据权利要求1所述的基于UWB辅助的主动定位方法， 其特征在于， 所述优化的超宽 带定位算法具体包括： 获取超宽带基站位置、 测距信息和待求标签位置， 进行残差模型的构建； 以标签位置为优化 量， 输入到构建的残差模型中构建优化方程； 获取标签移动速度作为弱约束， 确定两个连续时刻间标签位置的平滑约束， 确定标签 移动速度的协方差矩阵， 对 优化方程进行求 解。 7.根据权利要求1所述的基于UWB辅助的主动定位方法， 其特征在于， 所述通过视觉/惯 性/超宽带组合模型对初次定位数据进行修 正具体包括：权　利　要　求　书 1/2 页 2 CN 115540854 A 2获取位姿图中固定的超宽带基站的位置和载体的位姿， 获取超宽带基站与 载体之间的 边和两个连续时刻载体之间的边， 构建相对位姿变化方程； 获取连续两个时刻间的位值与姿态相对边缘和需要估计的世界坐标系下位姿变换的 残差， 根据连续两个时刻间的位值与姿态相对边缘和需要估计的世界坐标系下位姿变换的 残差确定载体在世界坐标系下的实时位姿。 8.根据权利要求7所述的基于UWB辅助的主动定位方法， 其特征在于， 所述确定载体在 世界坐标系下的实时位姿还 包括： 将两个超宽带传感器测量 时刻之间的视觉/惯性/超宽带组合模型估计出的载体位姿、 特征通过关键帧转换到世界坐标系下， 从而获得世界坐标系下的输出 频率； 根据当前关键帧的特 征三角化后建立的点云转换至世界坐标系下。 9.一种电子设备， 包括存储器、 处理器及存储在存储器上并可在处理器上运行的计算 机程序， 其特征在于， 所述处理器执行所述程序时实现如权利要求1至8任一项所述的一种 基于UWB辅助的主动定位方法。 10.一种计算机可读存储介质， 其上存储有计算机程序， 其特征在于， 该程序被处理器 执行时实现如权利要求1至8任一项所述的一种基于UWB辅助的主动定位方法。权　利　要　求　书 2/2 页 3 CN 115540854 A 3