一种基于UWB辅助的主动定位方法、设备和介质与流程

一种基于uwb辅助的主动定位方法、设备和介质
技术领域
1.本发明涉及定位技术领域，具体涉及一种基于uwb辅助的主动定位方法、设备和介质。


背景技术：

2.考虑到电力作业人员在复杂、电磁干扰严重、多遮挡的环境中工作，当视觉传感器失效时，回环检测算法无法进行，匹配上的点线特征也急剧下降，优化方程中，imu测量的残差成为主要部分。此时，根据视觉/惯性/超宽带组合模型依然能够依靠imu预积分进行位姿的估计，但是，估计精度将会大大降低。而当视觉传感器恢复时，如果系统路过曾经经过的地方，利用重定位算法能够立刻计算出较为精确的载体位姿。但是，若系统一直不经过曾经经过的地方，该段时间的位姿误差将一直存在。


技术实现要素：

3.本发明所要解决的技术问题是通过imu预积分进行位姿的估计无法对已经获取的数据进行修正，位姿误差大，目的在于提供一种基于uwb辅助的主动定位方法、设备和介质，通过构建双目视觉和惯性里程计模型，对目标环境图像进行回环修正与重定位，引入优化的超宽带定位算法，对目标环境图像中的待测标签与基站间的距离进行优化定位，建立视觉/惯性/超宽带组合模型对初次定位数据进行修正，获得精确定位，减小位姿误差，提高估计精度，获得更为精确的载体位姿。
4.本发明通过下述技术方案实现：本发明第一方面提供一种基于uwb辅助的主动定位方法，包括以下具体步骤：s1、获取目标环境图像，获得环境图像数据的点特征和线特征，获取拍摄设备的角速度和加速度；s2、构建双目视觉和惯性里程计模型，结合环境图像数据的点特征和线特征、拍摄设备的角速度和加速度，进行初次定位；s3、引入回环检测与重定位算法，对目标环境图像进行回环修正与重定位，引入优化的超宽带定位算法，对目标环境图像中的待测标签与基站间的距离进行优化定位；s4、结合对目标环境图像进行回环修正与重定位数据和优化定位数据，基于双目视觉和惯性里程计模型构建视觉/惯性/超宽带组合模型，通过视觉/惯性/超宽带组合模型对初次定位数据进行修正，获得精确定位。
5.本发明通过环境图像数据的点特征和线特征、拍摄设备的角速度和加速度，构建双目视觉和惯性里程计模型进行初次定位，引入回环检测与重定位算法，对目标环境图像进行回环修正与重定位，引入优化的超宽带定位算法，对目标环境图像中的待测标签与基站间的距离进行优化定位。结合目标环境图像进行回环修正与重定位数据和优化定位，基于双目视觉和惯性里程计模型构建视觉/惯性/超宽带组合模型，通过视觉/惯性/超宽带组合模型对初次定位数据进行修正，获得精确定位，提高估计精度，获得更为精确的载体位
姿。
6.进一步的，所述回环检测与重定位算法具体包括：获取当前输入的环境图像数据，若当前输入的环境图像数据被判断为当前关键帧：获取当前关键帧的特征，将当前关键帧对应的特征与滑动窗口中的特征进行匹配，建立重投影误差；对当前关键帧的特征三角化后与回环的关键帧之间建立特征及位姿的关系。
7.进一步的，所述当前输入的环境图像数据被判断为当前关键帧包括：对当前窗口中关键帧以外的所有关键帧进行检测，检测是否存在回环：若不存在回环，则按照视觉/惯性里程计算法进行基于滑动窗口的位姿估计；若存在回环，将回环关键帧确定为当前关键帧，将当前关键帧加入当前的滑动窗口之中。
8.进一步的，还包括对所述重投影误差进行误差消除：获取当前回环帧中的图像数据，确定当前回环帧的重投影误差的协方差矩阵，确定当前回环帧建立的特征的重投影误差，采用最小二乘法对获得载体的位姿信息进行优化。
9.进一步的，所述对当前关键帧的特征三角化后与回环的关键帧之间建立特征及位姿的关系包括：将一个关键帧与其连续的前后两个关键帧及其对应的回环关键帧建立边的关系，将建立关系的三个关键帧加入位姿图中，对所有关键帧的位姿进行优化，获得优化后的当前关键帧与回环的关键帧之间构建的相对位姿残差。
10.进一步的，所述优化的超宽带定位算法具体包括：获取超宽带基站位置、测距信息和待求标签位置，进行残差模型的构建；以标签位置为优化量，输入到构建的残差模型中构建优化方程；获取标签移动速度作为弱约束，确定两个连续时刻间标签位置的平滑约束，确定标签移动速度的协方差矩阵，对优化方程进行求解。
11.进一步的，所述通过视觉/惯性/超宽带组合模型对初次定位数据进行修正具体包括：获取位姿图中固定的超宽带基站的位置和载体的位姿，获取超宽带基站与载体之间的边和两个连续时刻载体之间的边，构建相对位姿变化方程；获取连续两个时刻间的位值与姿态相对边缘和需要估计的世界坐标系下位姿变换的残差，根据连续两个时刻间的位值与姿态相对边缘和需要估计的世界坐标系下位姿变换的残差确定载体在世界坐标系下的实时位姿。
12.进一步的，所述确定载体在世界坐标系下的实时位姿还包括：将两个超宽带传感器测量时刻之间的视觉/惯性/超宽带组合模型估计出的载体位姿、特征通过关键帧转换到世界坐标系下，从而获得世界坐标系下的输出频率；根据当前关键帧的特征三角化后建立的点云转换至世界坐标系下。
13.本发明第二方面提供一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述程序时用于实现一种基
于uwb辅助的主动定位方法。
14.本发明第三方面提供一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时用于实现一种基于uwb辅助的主动定位方法。
15.本发明与现有技术相比，具有如下的优点和有益效果：通过构建双目视觉和惯性里程计模型，对目标环境图像进行回环修正与重定位，引入优化的超宽带定位算法，对目标环境图像中的待测标签与基站间的距离进行优化定位，建立视觉/惯性/超宽带组合模型对初次定位数据进行修正，获得精确定位，实现估计精度的提高，获得更为精确的载体位姿。
附图说明
16.为了更清楚地说明本发明示例性实施方式的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本发明的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。在附图中：图1为本发明实施例中的uwb辅助的主动定位流程；图2为本发明实施例中的uwb辅助的主动视频定位方法；图3为本发明实施例中的回环检测与重定位算法流程图；图4为本发明实施例中的视觉/惯性/超宽带组合模型示意图。
具体实施方式
17.为使本发明的目的、技术方案和优点更加清楚明白，下面结合实施例和附图，对本发明作进一步的详细说明，本发明的示意性实施方式及其说明仅用于解释本发明，并不作为对本发明的限定。
18.实施例1如图1和图2所示，本实施例第一方面提供一种基于uwb辅助的主动定位方法，包括以下具体步骤：s1、获取目标环境图像，获得环境图像数据的点特征和线特征，获取拍摄设备的角速度和加速度；s2、构建双目视觉和惯性里程计模型，结合环境图像数据的点特征和线特征、拍摄设备的角速度和加速度，进行初次定位；s3、引入回环检测与重定位算法，对目标环境图像进行回环修正与重定位，引入优化的超宽带定位算法，对目标环境图像中的待测标签与基站间的距离进行优化定位；s4、结合对目标环境图像进行回环修正与重定位数据和优化定位数据，基于双目视觉和惯性里程计模型构建视觉/惯性/超宽带组合模型，通过视觉/惯性/超宽带组合模型对初次定位数据进行修正，获得精确定位。
19.在一些可能的实施例中，回环检测与重定位算法具体包括：获取当前输入的环境图像数据，若当前输入的环境图像数据被判断为当前关键帧：当前输入的环境图像数据被判断为关键帧包括：
对当前窗口中关键帧以外的所有关键帧进行检测，检测是否存在回环：若不存在回环，则按照视觉/惯性里程计算法进行基于滑动窗口的位姿估计；若存在回环，将回环关键帧确定为当前关键帧，将当前关键帧加入当前的滑动窗口之中。
20.获取当前关键帧的特征，将当前关键帧对应的特征与滑动窗口中的特征进行匹配，建立重投影误差；对当前关键帧的特征三角化后与回环的关键帧之间建立特征及位姿的关系：对当前关键帧的特征三角化后与回环的关键帧之间建立特征及位姿的关系包括：将一个关键帧与其连续的前后两个关键帧及其对应的回环关键帧建立边的关系，将建立关系的三个关键帧加入位姿图中，对所有关键帧的位姿进行优化，获得优化后的当前关键帧与回环的关键帧之间构建的相对位姿残差。
21.在一些可能的实施例中，还包括对重投影误差进行误差消除：获取当前回环帧中的图像数据，确定当前回环帧的重投影误差的协方差矩阵，确定当前回环帧建立的特征的重投影误差，采用最小二乘法对获得载体的位姿信息进行优化。
22.在一些可能的实施例中，优化的超宽带定位算法具体包括：获取超宽带基站位置、测距信息和待求标签位置，进行残差模型的构建；以标签位置为优化量，输入到构建的残差模型中构建优化方程；获取标签移动速度作为弱约束，确定两个连续时刻间标签位置的平滑约束，确定标签移动速度的协方差矩阵，对优化方程进行求解；待求标签位置为即将要进行位姿测量的图像数据。
23.在一些可能的实施例中，通过视觉/惯性/超宽带组合模型对初次定位数据进行修正具体包括：获取位姿图中固定的超宽带基站的位置和载体的位姿，获取超宽带基站与载体之间的边和两个连续时刻载体之间的边，构建相对位姿变化方程；获取连续两个时刻间的位值与姿态相对边缘和需要估计的世界坐标系下位姿变换的残差，根据连续两个时刻间的位值与姿态相对边缘和需要估计的世界坐标系下位姿变换的残差确定载体在世界坐标系下的实时位姿。
24.确定载体在世界坐标系下的实时位姿还包括：将两个超宽带传感器测量时刻之间的视觉/惯性/超宽带组合模型估计出的载体位姿、特征通过关键帧转换到世界坐标系下，从而获得世界坐标系下的输出频率；根据当前关键帧的特征三角化后建立的点云转换至世界坐标系下。
25.本实施例第二方面提供一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，处理器执行程序时用于实现一种基于uwb辅助的主动定位方法。
26.本实施例第三方面提供一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时用于实现一种基于uwb辅助的主动定位方法。
27.实施例2如图3所示，考虑到算法实时性、效率与准确性之间的平衡，以及电力场景光照、季
节、天气等带来的环境变化和视角变化都具有很好的鲁棒性，回环检测与重定位算法具体包括：当新来的一帧被判断为关键帧后，就会先进入回环检测线程中，利用基于图像块均匀采样的回环检测算法，在除现有窗口中关键帧以外的所有关键帧中对其进行检测。如果没有发现回环，则按照视觉/惯性里程计的算法进行基于滑动窗口的位姿估计。如果检测出回环并返回了回环对应的关键帧，则将回环关键帧加入当前的滑动窗口之中，并将回环关键帧对应的特征与滑动窗口中的特征进行匹配，建立重投影误差。但是，并不使用该回环关键帧附近的imu数据，同时，在优化的时候，固定回环帧已经获取的位置和姿态。因而，此时建立的优化方程如下：其中：是关于第i个回环帧建立的特征的重投影误差，是第i个回环帧的重投影误差的协方差矩阵。与是回环帧之前求解得到的姿态和位置，在此次优化中它们固定不变。采用levenberg marquardt方法对以上优化方程进行优化求解，获得载体的位姿信息。完成优化求解后，将该关键帧的特征三角化后加入点云中进行点云构建，并与其回环的关键帧之间建立起特征及位姿的关系。
28.而为了更加有效的将回环检测的误差消除，当一个关键帧被边缘化出滑动窗口后，会将其加入位姿图中。在位姿图中，关键帧作为图的节点，而图的边则是两个关键帧之间的相对位姿关系。第i帧和第j帧之间的相对位姿关系具体如下：间的相对位姿关系。第i帧和第j帧之间的相对位姿关系具体如下：其中，为第i帧和第j帧之间的相对位移，而是第i帧和第j帧之间的相对航向角。由于惯性/视觉系统两个水平姿态角可观，因而在位姿图中仅需考虑4个自由度。此外，对于一个关键帧，它只与其连续的前后两个关键帧及其对应的回环关键帧建立边的关系。位姿图中被加入新的三个关键帧后，对所有关键帧的位姿做一个优化。优化方程如下：
其中，是构建的相对位姿残差，是需要优化的量，f
퐺푟푎푝
h是构建的优化方程，是协方差矩阵，根据经验值设置。该优化，能够将回环检测出的累积误差的影响均匀的分摊到所有关键帧上，从而尽可能的减小其影响。
29.当视觉传感器失效时，回环检测算法无法进行，匹配上的点线特征也急剧下降，优化方程中，imu测量的残差成为主要部分。此时，根据视觉/惯性/超宽带组合模型依然能够依靠imu预积分进行位姿的估计，但是，估计精度将会大大降低。而当视觉传感器恢复时，如果系统路过曾经经过的地方，利用重定位算法能够立刻计算出较为精确的载体位姿。但是，若系统一直不经过曾经经过的地方，该段时间的位姿误差将一直存在。
30.在三维空间中，至少需要布置3个基站，这里使用4个基站能够取得较好的定位效果。根据基站与标签之间距离信息进行定位的方式其基本原理都是基于最小二乘法，构建距离差进行优化方程的建立。具体如下：针对上述超宽带基站位置、测距信息及待求标签位置，可以构建如下残差：其中，i对应的是第i个基站。据此，能够以标签位置为优化量，构建如下优化方程：其中，是第i个基站与标签之间的测量协方差，可根据超宽带传感器参数设置。在的系统中，将其均设为0.001。同时，利用huber核函数增加优化的鲁棒性。
31.不论是最小二乘法还是上述优化方程，对于任意一时刻标签的位置都只有基站与标签间测量距离的约束，如果测量值不准，很容易造成定位精度的下降。因此，考虑利用标签移动速度作为一个弱约束，增加两个连续时刻间标签位置的约束。具体优化方程如下：其中，新增的两个连续时刻间标签位置的平滑约束。通过其关于速度的协方差矩阵来对其进行加权：其中，是设置的最大载体最大速度，
∆
푡
是k和k 1时刻间的时间间隔，由于该约束是一个弱约束，因此，可以通过调节因子s调节其大小，减小其权重。
32.对于以上优化方程，同样采用基于滑动窗口的levenberg marquardt方法对其进
行优化，滑动窗口大小设为20，而超宽带标签的初始位置，利用最小二乘法计算得到。
33.对于超宽带定位系统，利用载体速度对连续两个时刻间的载体位姿进行了弱约束，但是该约束并不完全准确。视觉/惯性/超宽带组合模型的本质就是估计两个时刻间载体的相对位姿，它缺少的是载体位姿的全局约束。而超宽带定位系统则可以提供全局的位姿约束。因此，将这两种系统结合起来，设计视觉/惯性/超宽带的组合定位系统。
34.如图4所示，圆圈代表载体的位姿信息，矩形代表超宽带传感器的基站，他们能够与载体上搭载的传感器标签进行通信，计算出基站与标签之间的距离，六边形则代表了两个时刻之间，由视觉/惯性/超宽带组合模型提供的位姿约束。可以看到，该组合系统利用w系下固定位置的超宽带基站对载体进行全局位置的约束，而通过视觉/惯性/超宽带组合模型提供连续时刻载体间相对位姿的约束。整个组合系统的定位都以世界坐标系w为参考坐标系。而这里的每个时刻对应的是超宽带传感器的测量时刻。
35.视觉/惯性/超宽带组合模型同样利用图优化的思想进行设计。整个位姿图里包含两种端点，一是固定的超宽带基站的位置，二是载体的位姿。此外，位姿图里包含两种边，一是超宽带基站与载体之间的边。另一种是两个连续时刻载体之间的边，它们通过相对位姿变化进行描述为：其中，为需要估计的世界坐标系下的以超宽带标签为基准的载体的位姿。则为视觉/惯性/超宽带组合模型测量得到的导航坐标系下的载体位姿。则为起始时刻导航坐标系与超宽带标签坐标系之间的杆臂误差，它同样可以通过优化得到。描述了视觉/惯性/超宽带组合模型测量得到的连续两个时刻间的位值与姿态相对边缘与需要估计的世界坐标系下位姿变换的残差。我们的目的是估计出合适的世界坐标系下的载体位姿，能够使得该残差最接近零。
36.根据以上分析，视觉/惯性/超宽带组合模型的优化状态量为：优化方程可以写成如下形式：其中，为视觉/惯性/超宽带组合模型测量的协方差矩阵，根据其定位精度与经验值综合设定。在本项目中，将位置的x轴和y轴方向对应的协方差值设为0.0001，将位置的z轴方向对应的协方差矩阵设为0.00001，将姿态三个方向对应的协方差值设为0.000001。而一旦相机失效，将位置三个方向对应的协方差矩阵值设为0.0025。
37.一般情况下，在组合系统中，使用的超宽带传感器的数据频率小于或等于视觉/惯
性/超宽带组合模型。并且，它们很难做到时间上的绝对对齐。因此，以超宽带传感器的输出时刻为基准，对于某一基准时刻的视觉/惯性/超宽带组合模型测量值，利用该时刻前后两帧的视觉/惯性组合的输出做线性插值获取。
38.每次进行优化时，滑窗内的、等视觉/惯性/超宽带组合模型的测量值均使用最新的估计值。每次优化结束后，根据估计出的和杆臂误差以及视觉/惯性/超宽带组合模型输出的、能够计算出世界坐标系w与导航坐标系n之间的转换矩阵：对于超宽带传感器输出频率低于视觉/惯性/超宽带组合模型的情况，能够将两个超宽带传感器测量时刻之间的视觉/惯性/超宽带组合模型估计出的载体位姿、特征通过转换到世界坐标系下，从而获得更高的世界坐标系下的输出频率。在系统运行结束时，对所有的载体状态进行一个全局的优化，从而估计出最为精确的，然后将建立的点云通过转换至世界坐标系下。
39.此外，当系统开始运行时，首先进行的是视觉/惯性/超宽带组合模型的初始化，初始化完成后，视觉/惯性/超宽带组合模型开始进行载体（以imu坐标系为基准）在导航坐标系下的估计。与此同时，超宽带传感器也在不断的进行基站与标签之间的测量。当视觉/惯性/超宽带组合模型设置的滑动窗口具有20帧时，视觉/惯性/超宽带组合模型开始进行优化，并实时估计出载体（以超宽带标签坐标系为基准）在世界坐标系下的位姿。
40.以上所述的具体实施方式，对本发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施方式而已，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。