Active Pose Relocalization for Intelligent Substation Inspection Robot

(未完成：加 思维导图、段落分析、pipeline)

Active Pose Relocalization for Intelligent Substation Inspection Robot

智能变电站巡检机器人主动姿态重定位

　　摘要：变电站中广泛应用的智能巡检机器人在对电气设备进行日常巡检时，要求采集与标定图像一致的巡检图像。然而，由于导航误差和机械磨损，检测机器人捕捉到符合要求的检测图像是一项具有挑战性的工作。为了解决这个问题，本文提出了一种主动姿态重定位（APR）方法。具体来说，建立描述图像平面中的像素误差与机器人位姿误差之间关系的误差模型。然后，提出一种基于误差模型的解耦三级PI控制策略，将机器人重新定位到校准位姿，其中，提出一种基于单应变换的平移误差估计算法来计算校准位姿和检查位姿之间的绝对平移尺度，避免了经典2D-2D位姿估计算法的退化问题。最后，通过分别在虚拟和现实环境中的 10 个校准点的比较重定位实验证明了所提出的 APR 方法的性能。

　　索引术语——主动位姿重定位、检查机器人、PI 控制器、变电站检查、平移尺度估计。

1 引言

A. 动机

　　智能巡检机器人因其非接触、多参数和高效率测量的优势而被广泛应用于变电站电力设备的运行状态监测。这些机器人可以搭载热红外成像传感器、云台变焦摄像机、局部放电检测装置等，独立完成巡检任务，及时发现电力设备的内部热缺陷和外部机械缺陷。姿态重定位的目标是将巡检机器人重新定位到校准姿态，这是由于巡检机器人需要捕获与校准图像（或模板图像）一致的巡检图像。

B. 文献综述

　　据我们所知，APR是变电站巡检机器人应用领域中的一个新术语，目前还没有成熟的解决方案。类似地，主动相机重定位（ACR）旨在动态地将RGB或RGBD相机重新定位到产生输入参考图像的相同6D姿态。这些方法依赖于高精度的6自由度（6-DoF）轨道运动平台，其精度高于轮式巡检机器人运动平台和PTZ。与此相反，静态相机重定位（SCR）方法旨在从输入图像序列中被动估计相机姿态，并将最佳姿态对应的图像作为输出，特别是在大规模基于图像的定位应用中。与APR不同，SCR中巡检机器人的姿态是手动设置或固定的，这意味着SCR的精度仍然受到导航误差和机械磨损的限制。此外，视觉伺服（VS）致力于控制机器人的运动以实现轨迹跟踪，与我们的APR工作相近。VS可以分为两种类型：1）基于图像的视觉伺服（IBVS）和2）基于位置的视觉伺服（PBVS）。IBVS和PBVS方法都依赖于一些显著的标记来计算目标的运动，而在APR场景中没有标记。尽管相关技术取得了进展和优势，但它们仍然受到场景不适用或精度低的限制。巡检机器人作为变电站中关键的检查设备，确保了电力设备的安全稳定运行。因此，开发一种适应巡检机器人的鲁棒APR方法是实现可靠变电站检查的当务之急。

C. 贡献

　　为了解决上述问题，本文提出了一种APR方法。这项研究的主要贡献有三个方面：
　　1. 研究建立了一个误差模型，描述了校准和检查状态下姿态和像素误差之间的关系，并提出了一种平移误差估计算法，计算校准和检查姿态之间的绝对平移比例。
　　2. 基于误差模型和平移误差估计算法，展示了一种APR控制策略，通过调整机器人实现精确和鲁棒的姿态重定位，将控制过程分为三个解耦阶段，避免了上述退化问题。同时，理论上证明了控制块的收敛性。
　　3. 实验结果表明，与相关技术相比，APR方法捕获的检查图像与校准图像更加一致，验证了APR方法在变电站检查场景中的高精度和可行性。

D. 组织结构

　　本文的其余部分安排如下。第II节介绍了误差模型和问题表述。然后，第III节详细描述了所提出的APR方法，并在第IV节展示了相应的比较实验。最后，第V节简要总结了这项工作。

2 问题表述

A. 巡检机器人姿态模型

　　刚体的姿态可以用4×4矩阵表示，其中SE(3)、R∈R^3×^3和t∈R^3×^1分别是特殊欧几里得群、正交旋转矩阵和平移向量。为了方便，我们使用两个元组⟨R, t⟩来等价表示，并定义Tij = ⟨Rij, tij⟩表示从Pi到Pj的变换，即Pj = TijPi。巡检机器人简化为三个刚体部分，即底盘、PTZ和高清（HD）摄像机，它们对应的姿势分别用Ph、Pp和Pc表示。最初，它们的变换矩阵表示为Thp = ⟨1, thp⟩和Tpc = ⟨1, tpc⟩，其中thp和tpc是固定的机器人装配参数。

B. 姿态误差模型

　　如图2所示，根据介绍，巡检机器人有两种状态，即校准状态和检查状态。在校准状态下，底盘和PTZ姿态之间的变换由Pp = ThpPh = ⟨RxRy, thp⟩表示，其中Ph被视为单位矩阵，Rx和Ry分别是对应于PTZ校准俯仰角θx和旋转角θy的正交矩阵。在校准状态下，底盘和PTZ的位置偏离校准值，即当前姿态变为P*h = ⟨1, th⟩Ph和P*p = T*hpP*h = ⟨R*xR*y, thp⟩P*h，其中th = [thx, thy, thz]^T是底盘的平移误差向量，R*xR*y是PTZ的旋转矩阵。结合(1)-(3)，可以得到校准和检查状态下摄像机的姿态Pc = TpcThpPh = ⟨RxRy, thp + tpc⟩和P*c = TpcT*hpP*h = ⟨R*xR*y, R*xR*y*th + thp + tpc⟩。然后，摄像机姿态误差Tc = PcP*−1c = ⟨RcxRcy, tc⟩，其中Rcx和Rcy是对应于俯仰角和旋转角误差的正交矩阵，tc = [tcx, tcy, tcz]^T是HD摄像机的平移误差向量。对于校准和检查图像之间的像素误差，首先，3D点Xc = [Xc, Yc, Zc]^T在校准摄像机坐标系Oc−xyz中的点与其在校准图像平面中的对应像素点m = [u, v]^T之间的关系由u = fu Xc/Zc + u0, v = fv Yc/Zc + v0给出，其中fu和fv分别是水平和垂直方向的比例因子，fu = fv适用于正方形像素摄像机，[u0, v0]^T表示基于通用zk摄像机模型的摄像机主点的像素坐标。然后，我们可以得到检查摄像机坐标系中的3D点X*c = [X*c, Y*c, Z*c]^T和像素点m* = [u*, v*]^T对应的Xc，即X*c = [X*c, Y*c, Z*c]^T和m* = [u*, v*]^T。因此，校准和检查图像之间的像素误差为Δu = fu (Xc/Zc - X*c/Z*c)，Δv = fv (Yc/Zc - Y*c/Z*c)。

C. 问题陈述

　　为了重新定位巡检机器人的摄像机以捕获与校准图像相同的巡检图像，即Tc = 1和∥(Δu, Δv)∥^2 = 0，APR方法主要考虑以下两个问题。1) 需要估计姿态误差Tc。通常，我们可以通过结合基于点的图像配准方法和5点算法从校准和检查图像像素中获得校准和检查姿态之间的相对旋转矩阵RxRy和归一化平移向量tc = tc/∥tc∥^2。因此，需要通过APR估计tc。2) 为了准确控制巡检机器人纠正估计的姿态误差Tc，需要设计APR控制策略。根据变电站的空间结构，可以合理地假设以下两点：假设1：校准点附近的路面是绝对平面的，即thy = 0。假设2：从电力设备到HD摄像机的深度远大于底盘的初始平移误差，即Zc ≫ ∥th∥^2。

3 提出的APR方法

A. 平移误差估计

　　假设Pc和P*c之间没有相对旋转，即RxRy = R*xR*y和RcxRcy = 1，Xc和X*c之间的关系变为X*c = Xc - [tcx, tcy, tcz]^T。根据RTyRTx = cos θy sin θx sin θy cos θx sin θy 0 cos θx - sin θx - sin θy sin θx cos θy cosθx cos θy，还有关系th = -RTyRTx*tc = [tcx cos θy + tcy sin θx sin θy + tcz cos θx sin θy, tcy cos θx - tcz sin θx - tcx sin θy + tcy sin θx cos θy + tcz cos θx cos θy]。由于thy = 0，可以得到tcz = tcy cot θx和th = [tcx cos θy + tcy sin θy sin θx, 0 - tcx sin θy + tcy cos θy sin θx]。将(11)代入(10)，tc对应的像素误差为Δu = fu (Xc/Zc - Xc - tcx/Zc - tcz)，Δv = fv (Yc/Zc - Yc - tcy/Zc - tcz)。考虑到假设Zc ≫ ∥th∥^2和方程∥th∥^2 = ∥tc∥^2，(15)和(16)可以简化为Δu = fu tcx/Zc，Δv = fv tcy/Zc。现在可以通过结合(13)和(17)估计平移误差tc为tc = [Δu/fu, Δv/fv, Δv cot θx/fv]。为了解决(18)中的Δu和Δv，特别是，在Xc的校准状态下选择的3D点位于摄像机的光轴上，即Xc = [0, 0, Zc]^T，m = [u0, v0]^T。以Xc为中心，建立了一个边长为2w的虚拟正方形G，表示为Gc = [G1 G2 G3 G4] = [-w w -w w w w -w -w Zc Zc Zc Zc]。校准图像中对应于Gc的像素点为gc = [g1 g2 g3 g4] = [u0 - fuw/Zc u0 + fuw/Zc u0 - fuw/Zc u0 + fuw/Zc v0 + fvw/Zc v0 + fvw/Zc v0 - fvw/Zc v0 - fvw/Zc]。由于电力设备可以被视为相对于摄像机的远距离物体，单应性矩阵用于表示校准和检查图像视图之间的变换。应用基于点的图像配准方法，我们可以获得像素单应性矩阵H。然后，在检查图像中的g*c = [g*1 g*2 g*3 g*4] = H[gc]。基于单应性变换的平面属性，m* = [u*, v*]T对应于m是线g*1g*3和g*2g*4的交点，并且对于任何边宽2w都保持不变。为了确保H的尺度等于虚拟正方形Gc的单应性变换的尺度，根据(7)将Zc的值设置为fu。最后，在(18)中获得Δu = u0 - u*和Δv = v0 - v*。显然，估计算法可以避免5点算法在∥tc∥^2 → 0时遭受的解决方案退化问题，因为当∥tc∥^2变小的时候，单应性矩阵的解决方案将更加准确。

B. APR控制策略

　　观察(6)中的姿态误差模型和(18)中的平移误差，我们可以发现旋转误差RcxRcy与平移误差tc是解耦的，并不是相反的，并且Δu和Δv对应的平移误差是相互解耦的。因此，可以首先纠正PTZ的旋转误差。然而，假设1和假设2在现实世界的变电站中可能不会严格成立，这导致了(18)的近似计算误差。因此，考虑到计算误差和机械磨损，提出了一种APR控制策略，用于控制巡检机器人自动纠正姿态误差。该策略对机械磨损具有鲁棒性，并包含三个解耦阶段，如图3所示。具体细节如下。第一阶段（PTZ姿态纠正）：在这个阶段，纠正PTZ的姿态误差。具体来说，使用在机器人运动估计中广泛使用的特征检测器Speeded Up Robust Features (SURF)来找到校准图像Ic和实时检查图像Ir之间的匹配点Dc和Dr。然后，可以通过经典的5点算法和姿态解算算法从Dc和Dr中准确估计RcxRcy。通过分解RcxRcy，可以获得俯仰和旋转角度误差Δθx = arctan(RcxRcy(3, 2)/RcxRcy(3, 3))和Δθy = arctan(-RcxRcy(3, 1)/∥RcxRcy(3, 2), RcxRcy(3, 3)∥^2)。此外，Δθx和Δθy应该通过乘以逆雅可比矩阵Jθ = diag(ωx, ωy)转换为执行时间τcx和τcy，即[τcx, τcy]^T = Jθ[Δθx, Δθy]^T，其中ωx和ωy分别是PTZ的俯仰和旋转角速度。然而，由于机械磨损，Δθx和Δθy不能通过一步准确纠正。因此，应用了广泛使用的鲁棒控制器PI控制器，直到∥(θcx, θcy)∥^2 ≤ εθ，产生执行时间ˆτcx和ˆτcy。第二阶段（PTZ纵向平移纠正）：由于PTZ的姿态误差在校正阶段1中得到了纠正，即RcxRcy = 1，Δv对应的PTZ的平移误差tcy和tcz可以通过(18)估计。底盘的相应平移由th = -RTyRTxZc[0 Δv/fv Δv cot θx/fv] = Jv[0 Δv Δv] = ZcΔv/fv sin θx[sin θy 0 cos θy]给出，其中Jv = -RTyRTxdiag(1, Zc/fv, Zc cot θx/fv)。显然，arctan(thx/thz) = θy或θy + π。通常，俯仰角θx ∈ (0, π/2)和旋转角θy ∈ (0, 2π)。因此，sin θx > 0，并且底盘的平移方向由θy和Δv决定。arctan(thx/thz)对应于不同条件的值如图4所示。可以发现，当Δv > 0时，PTZ应该向前平移，当Δv < 0时，应该向后平移。这意味着在这个阶段，PTZ将由底盘纵向平移。与第一阶段类似，考虑到机械磨损，PI控制器也应用于产生平移ˆth，直到|Δv| ≤ εv。第三阶段（PTZ横向平移纠正）：与第二阶段类似，Δu对应的平移th给出th = -RTyRTxZc[Δu/fu 0 0] = Ju[Δu 0 0] = ZcΔu/fu[cos θy 0 - sin θy]，其中Ju = -RTyRTxdiag(Zc/fu, 1, 1)。th的方向是θy - π/2或θy + π/2。与图4类似，图5显示了基于(28)的th的方向对应于不同条件下的值。显然，当Δu > 0时，PTZ应该向右平移，当Δu < 0时，应该向左平移。在这个阶段，PTZ将横向平移。PI控制器再次应用于产生平移ˆth，直到|Δu| ≤ εu。三个阶段的控制策略在算法1中总结。值得注意的是，如果我们选择通过执行(25)和(28)同时纠正平移误差th，机器人需要在每次迭代后转动并向前或向后移动，这是耗时的。相比之下，基于解耦策略的机器人总共只需要转动两次，控制更加鲁棒。此外，在第二和第三阶段的迭代平移误差纠正中，∥th∥^2变得更小，进一步增强了假设Zc ≫ ∥th∥^2。

C. 收敛性分析

　　图3(a)中的每个阶段的控制块可以统一表示为图6。我们给出定理1来显示其收敛性。

4 实验

A. 实验设置

　　为了评估所提出的APR方法的性能，我们建立了真实世界和仿真实验平台。1) 真实世界实验平台：真实世界平台的设置如图7所示，主要包括高压电力设备：气体绝缘开关设备（GIS）和支撑绝缘子。巡检机器人：由PTZ、路由器和底盘组成。PTZ配备了红外摄像机和高达32倍变焦的高清摄像机。高清摄像机的图像分辨率、主点和1倍焦距fu分别为2688×1520、[1356.09, 774.79]和2596.99毫米。路由器用于将实时巡检图像上传到上位计算机，接收和分发PTZ和底盘的控制命令。底盘由装有Ubuntu 18.04和机器人操作系统（ROS 1，kinetic分布）的车载计算机控制。PTZ和路由器由底盘电池供电。PTZ、底盘和路由器之间的通信通过以太网进行。上位计算机：一台配备1.9 GHz英特尔酷睿CPU、16 GB RAM的笔记本电脑。APR程序基于PTZ和底盘的软件开发工具包（SDK）为Microsoft基础类（MFC）平台开发，负责通过路由器的WiFi网络自动接收实时巡检图像，并为PTZ和底盘产生实时控制命令。2) 仿真实验平台：建立在Unreal Engine 4.26上的仿真平台集成了一个中国真实变电站的虚拟环境模型和巡检机器人，如图8所示，可以完全控制，适用于理想的可重复测试。虚拟机器人的线性和角速度以及虚拟PTZ摄像机的分辨率和内在参数与我们的真实世界巡检机器人相同。并且根据[8]中的研究，向底盘和PTZ的运动控制命令中添加了高斯随机白噪声，以模拟机械磨损。3) 数据设置：为了减少照明对校准图像质量的影响，我们根据变电站检查中预设校准点的原则，在虚拟和真实环境中分别为巡检机器人设置了10个校准点。虚拟环境中的初始姿态误差的数量级与现实世界中的数量级一致。PTZ角度和最大平移误差分别设置为3°和500毫米。虚拟和现实世界实验中的算法参数相同。具体来说，PI控制器的参数设置为kp = 0.4和ki = 0.04。角度阈值εθ选择为0.1°。像素阈值εu和εv都选择为1.0像素。此外，为了测试所提出的APR控制策略在纠正平移误差方面的准确性和鲁棒性，我们将APR与应用于细粒度检测和重摄的ACR和CRP控制策略进行了比较。在ACR中，平移向量的尺度是手动预设的，并通过5点算法和二分法理论频繁更新。CRP方法使用3D重建技术分别估计校准、第一帧和当前图像中共同特征点的深度，间接获得当前姿态和校准姿态之间的平移向量，以避免退化问题。所有方法从同一位置开始，然后在1倍焦距下执行姿态估计和调整，并最终以校准变焦捕获检查图像。

B. 评估标准

　　为了定量评估不同方法捕获的校准和检查图像之间的一致性，我们提出了两个指标，即图像覆盖率和平均密集光流（ADOF）。图像覆盖率定义为检查图像和校准图像之间重叠像素的数量与图像分辨率的比率。ADOF定义为ADOF(Ic, IHr) = Σ|F(Ic, IHr)i,j|^2 / (M×N)，其中M×N = 2688×1520是图像分辨率，F(Ic, IHr)表示通过Farnback算法计算的密集光流，IHr是校准图像和单应性变换后的检查图像。显然，图像覆盖率越高，ADOF越小，校准和检查图像之间的一致性就越高。

C. 仿真比较结果

　　在虚拟环境中，我们每个校准点执行不同的策略四次，从相同的初始姿态开始。图9报告了ACR、CRP和APR方法在不同指标方面的仿真比较结果。其中，图9(a)显示APR实现了最小的平移误差，中位数值为8.13毫米，比ACR和CRP的精度高一个数量级。从图9(b)和(c)中可以发现，APR方法的运行时间在60秒以内，比ACR和CRP快，尽管APR似乎需要更多的迭代次数。APR需要更多迭代但消耗的运行时间更少的原因是，底盘在整个APR控制过程中只需要转动两次，每次迭代只需要向前或向后移动，而在ACR和CRP策略中，由于PTZ摄像机的平移依赖于底盘的运动，底盘在每次迭代更新摄像机向量时需要转动一次，然后向前或向后移动，这是耗时的，并降低了平移鲁棒性。考虑到常规变电站检查任务更注重重定位精度和图像质量而不是检查时间，APR的运行时间是可以接受的。此外，如图9(d)至(g)所示，由APR捕获的校准和检查图像之间的覆盖率和像素偏差优于ACR和CRP方法，意味着APR获得了与校准图像最一致的检查图像。观察每个箱线图中的数据分布，我们可以发现APR方法是最准确和鲁棒的。仿真结果验证了APR可以为变电站巡检机器人的平移误差纠正提供比比较策略更简单的控制。

D. 真实世界比较结果

　　与仿真实验类似，我们每个校准点执行不同的策略一次，从相同的初始姿态开始。图10(a)展示了ACR、CRP和APR方法在平移误差方面的比较结果，APR实现了26.50毫米的中位数误差，而ACR和CRP的中位数误差分别为102.83毫米和167.91毫米。APR的优越性在于，所提出的平移误差估计算法不需要应用经典的2D-2D姿态估计方法，而是依赖于计算更鲁棒的单应性矩阵，避免了ACR中的平移解决方案退化问题和平移误差传播问题。换句话说，5点平移估计算法用于计算ACR和CRP策略中摄像机的归一化平移误差向量，一旦∥tc∥^2变小，即5点算法将给出不准确甚至错误的平移向量。与ACR和CRP不同，基于单应性矩阵的所提出的平移误差估计算法在∥tc∥^2 → 0时更可靠和准确，用于计算机器人的平移向量。此外，图10(b)至(e)中不同方法在图像质量方面的定量比较结果报告，APR方法显示出最佳的图像覆盖率和最小的像素偏差，与仿真实验结果相同。图11提供了不同策略的更直观的定性比较结果。显然，APR在每个校准点捕获的检查图像与校准图像最为一致。特别是，在校准点(d)、(g)和(h)处，米和套管不在ACR和CRP捕获的检查图像的视场中。这种重定位失败将导致现实世界变电站常规检查中许多检查图像不可用，如图1(b)所述。可以理解的是，所有策略在现实世界中的准确性低于虚拟环境中的准确性，因为现实世界中的图像纹理比渲染纹理更复杂，地面在现实世界中不是绝对平面的，这降低了姿态估计和平移误差纠正的准确性。尽管如此，现实世界的实验结果进一步验证了我们的平移误差估计算法和平移误差纠正控制策略在变电站巡检机器人姿态重定位方面的高精度和强鲁棒性。

5 结论和讨论

　　本文提出了一种变电站巡检机器人的主动姿态重定位（APR）方法，旨在自动将巡检机器人重新定位到校准姿态，并捕获与校准图像一致的检查图像。首先，引入了基于构建的姿态误差模型的平移误差估计算法。这种估计算法可以产生平移机器人的真实尺度。然后，考虑到运动执行器的机械磨损，展示了一种姿态解耦控制策略，以准确地重新定位机器人捕获检查图像。与现有技术相比，实验结果表明：
　　1. 提出的APR只需要知道PTZ摄像机的校准角度和内在参数，就实现了比比较策略更快、更鲁棒的控制，以纠正变电站巡检机器人的平移误差。
　　2. 基于比现有技术更准确的单应性矩阵解决方案，APR避免了平移解决方案退化问题，并显示出在变电站巡检机器人姿态重定位方面的高精度和强鲁棒性。
　　我们的工作可以为基于轮式机器人运动平台和PTZ的其他姿态重定位应用提供参考。然而，一些实际问题，如影响APR性能的不平坦路面，仍需在未来解决。