张文, 丁雨林, 陈咏华, 孙振国
清华大学 机械工程系，先进成形制造教育部重点实验室，北京 100084
收稿日期：2021-10-05
基金项目：苏州—清华创新引领行动计划专项(2016SZ0218)
作者简介：张文(1992—)，男，博士研究生
通讯作者：孙振国，副教授，E-mail: sunzhg@tsinghua.edu.cn

摘要：针对相对封闭、磁干扰等特殊环境下传感器应用受限，导致爬壁机器人自主定位误差随时间累积的问题，该文提出并实现了一种基于外部RGB-D相机和惯性测量单元(inertial measurement unit, IMU)组合的爬壁机器人自主定位方法。该方法采用深度学习和核相关滤波(kernelized correlation filter, KCF)组合的目标跟踪方法进行初步位置定位；在此基础上，利用法向量方向投影的方法筛选出机器人外壳顶部的中心点，实现机器人定位中的位置定位。推导了机器人底盘法向量、横滚角与航向角的定量关系，设计了串联的扩展Kalman滤波器(extended Kalman filter, EKF)计算横滚角、俯仰角和航向角，实现机器人定位中的姿态估计。实验结果表明：该方法使爬壁机器人位置定位误差小于0.02 m，姿态估计的航向角和横滚角误差小于2.5°，俯仰角误差小于1.5°，有效地提高了爬壁机器人定位精度。
关键词：RGB-D相机三维点云扩展Kalman滤波器爬壁机器人惯性测量单元
Autonomous positioning for wall climbing robots based on a combination of an external camera and a robot-mounted inertial measurement unit
ZHANG Wen, DING Yulin, CHEN Yonghua, SUN Zhenguo
Key Laboratory for Advanced Materials Processing Technology of Ministry of Education, Department of Mechanical Engineering, Tsinghua University, Beijing 100084, China

Abstract: Sensor accuracy in special environments can be very limited due to closed systems and magnetic interference. For example, sensors on wall climbing robots can experience accumulation of autonomous positioning errors with time. The paper presents an autonomous positioning method for wall climbing robots based on an external RGB-D camera and a robot-mounted inertial measurement unit (IMU). This method uses the target tracking method with a deep learning and kernelized correlation filter (KCF) for preliminary positioning. A normal direction projection method is then used to locate the center on the top of the robot shell for the robot position positioning. The system determines the normal, the roll angle and the heading of the robot with a series EKF filter calculating the roll angle, pitch angle and heading to estimate the robot attitude. Tests show that the wall climbing robot positioning error is within 0.02 m, the heading error and the roll angle error for the attitude estimate are both within 2.5°, and the pitch angle error is within 1.5°. This system effectively improves the wall climbing robot positioning accuracy.
Key words: RGB-D cameras3D point cloudextended Kalman filterswall climbing robotsinertial measurement unit
在大型钢制构件的自动探伤和清洗等领域^[1]，爬壁机器人通过自主定位获取自身在钢制构件表面的位置用于路径规划与制定控制策略，同时根据自身的姿态调整作业探头以提高工作效率。根据NB/T47013.3—2015《承压设备无损检测第3部分：超声检测》^[2]对缺陷的记录要求，用于超声探伤的爬壁机器人位置定位误差要保持在0.025 m以内。根据文[3]，当位置定位平均误差不超过0.18 m，姿态估计误差不超过6.9°时，清洗机器人能够正常工作。然而，在现有机器人自主定位方法中，储罐、引水压力钢管等^[4]相对封闭的工作环境限制了GPS^[5]在机器人定位中的应用。同时，钢制构件表面作业的磁吸附爬壁机器人附带的磁干扰限制了磁强度计在姿态估计中的应用^[6]。
惯性导航系统具有自主性，不受外界环境的干扰，通过对安装在机器人上的惯性测量单元(inertial measurement unit, IMU)测量值积分来获得当前速度和位置。由于IMU的漂移属性，在没有磁强度计校准的条件下，航向角误差和位置误差随时间累积^[7]。为此，Venkatnarayan等^[8]提出一种WiFi辅助的惯性里程计方法，利用WiFi信号校正IMU的漂移误差; Fan等^[9]提出了一种改进的自适应Kalman滤波算法，通过融合IMU和多个超宽带(ultra wide band, UWB)距离信息进行二维平面的位置定位。但爬壁机器人工作的钢制壁面对无线信号(如WiFi、UWB等)吸收严重，导致相关方法失效。
视觉定位方法是近年来机器人定位的研究热点。根据视觉传感器的安装位置可以分为机载视觉方法和外部视觉方法。在机载视觉方法中，相机跟随机器人运动。以单目彩色相机作为机载视觉传感器的定位方法中，Mur-Artal等^[10]提出了著名的ORB-SLAM定位方法，通过图像中的定向二进制简单描述符(oriented FAST and rotated brief, ORB)进行跟踪、定位和建图。以RGB-D相机作为机载视觉传感器的方法中，Newcombe等^[11]提出的KinectFusion方法通过迭代最近点(iterative closest point, ICP)方法与观测点云配准进行定位，计算量较大。激光雷达也具有获取空间结构信息的功能，Jiang等^[12]通过三维激光雷达实现机器人在空间的位姿估计。上述方法要求相机视野中存在可持续跟踪的特征点或特征线。爬壁机器人吸附在钢制构件表面运动，机载相机的视野范围小，高度相似的锈迹或油漆面导致缺乏稳定的特征点或线等信息，无法通过特征匹配进行运动跟踪和重定位。因此，机载视觉方法不适用于爬壁机器人的定位。
在外部视觉方法中，相机保持静止，机器人在相机视野内运动。Romero-Ramirez等^[13]提出一种多尺度ArUco标记检测的方法，通过单目相机实现待测物体的位姿估计，但ArUco标记的检测容易受到环境光照的影响。商用运动捕捉系统Vicon^[14]通过在机器人上安装反射红外脉冲的球形标记，采用多个高清彩色相机组合的方案实现位姿估计，然而该系统适用于固定空间内的定位，且需要安装多个相机。Zhao等^[15]提出的三维均值偏移目标跟踪(3D mean-shift object tracking, MS3D)方法通过融合RGB-D相机提供的颜色和深度分布信息实现了机器人的位置定位，但该方法未能实现爬壁机器人的姿态估计。
本文提出一种外部RGB-D视觉与机载IMU组合的自主定位方法，该方法利用法向量方向投影的方法实现高精度位置定位; 通过融合外部视觉获得的机器人底盘法向量信息、机载IMU测得的重力矢量信息和运动角速度信息，解决了其航向角误差随时间累积的问题，实现了机器人的高精度姿态估计。
1 系统框架本文建立的爬壁机器人自主定位系统示意图如图 1所示，外部视觉选用RGB-D相机，机器人上搭载IMU。定义相机坐标系O_R-X_RY_RZ_R，相机的光心为坐标原点O_R，相机的光轴为Z_R轴; 定义导航坐标系为O_n-X_nY_nZ_n，原点O_n与O_R重合; 定义IMU坐标系为O-X_iY_iZ_i，与机器人本体坐标系O-X_bY_bZ_b重合。定义IMU输出的三轴角速度为(ω_x^k, ω_y^k, ω_z^k)，三轴角速度漂移为(b_x^k, b_y^k, b_z^k)，三轴加速度为(a_x^k, a_y^k, a_z^k)。定义爬壁机器人外壳顶部的中心点在导航坐标系中的三维坐标(p_x, p_y, p_z)为机器人的位置。引入姿态的Euler角表示方式，横滚角?，俯仰角θ和航向角ψ。Euler姿态角定义如图 2所示，将O-X_bY_bZ_b移动到O_n，并围绕Z_b轴转动ψ得到O_n-X₁Y₁Z_b，将O_n-X₁Y₁Z_b围绕Y₁轴转动-θ得到O_n-X_nY₁Z₁，将O_n-X_nY₁Z₁围绕X_n轴转动?得到O_n-X_nY_nZ_n。将本体坐标系到导航坐标系的转换矩阵记为姿态矩阵${ }_{\mathrm{b}}^{\mathrm{n}} \boldsymbol{C} $，与定义的Euler姿态角顺序相对应的${ }_{\mathrm{b}}^{\mathrm{n}} \boldsymbol{C} $表示如下：

${ }_{\mathrm{b}}^{\mathrm{n}} \boldsymbol{C}=\left[\begin{array}{ccc}\theta_{\mathrm{c}} \psi_{\mathrm{c}} & -\phi_{\mathrm{c}} \psi_{\mathrm{s}}+\phi_{\mathrm{s}} \theta_{\mathrm{s}} \psi_{\mathrm{c}} & \phi_{\mathrm{s}} \psi_{\mathrm{s}}+\phi_{\mathrm{c}} \theta_{\mathrm{s}} \psi_{\mathrm{c}} \\\theta_{\mathrm{c}} \psi_{\mathrm{s}} & \phi_{\mathrm{c}} \psi_{\mathrm{c}}+\phi_{\mathrm{s}} \theta_{\mathrm{s}} \psi_{\mathrm{s}} & -\phi_{\mathrm{s}} \psi_{\mathrm{c}}+\phi_{\mathrm{c}} \theta_{\mathrm{s}} \psi_{\mathrm{s}} \\-\theta_{\mathrm{s}} & \phi_{\mathrm{s}} \theta_{\mathrm{c}} & \phi_{\mathrm{c}} \theta_{\mathrm{c}}\end{array}\right] .$

(1)

图 1 爬壁机器人自主定位系统示意图

图选项

图 2 Euler姿态角定义

图选项

其中下角标“s”代表对应的正弦函数值，“c”代表对应的余弦函数值。
本文提出的RGB-D+IMU自主定位方法结构框图如图 3所示。首先，采用基于深度学习的Yolo-tiny法^[16]和核相关滤波(kernelized correlation filter, KCF)^[17]组合的目标跟踪方法进行初步位置定位; 其次，创建大型构件的三维点云，采用法向量方向投影的方法实现机器人位置定位; 最后，以重力矢量信息作为观测量，设计扩展Kalman滤波器(extended Kalman filter, EKF)进行第1次滤波计算横滚角、俯仰角; 以机器人底盘法向量和上一步计算的横滚角作为观测量，设计EKF进行第2次滤波计算航向角。

图 3 RGB-D+IMU自主定位方法结构框图

图选项

2 RGB-D+IMU自主定位方法2.1 爬壁机器人初步位置定位RGB-D相机输出的每张深度图像生成含有超过20万个三维空间点的点云，仅有爬壁机器人所在区域的三维空间点发生改变。通过初步位置定位，框选出钢制构件RGB图像中的机器人目标矩形框，将有效减少待处理的三维空间点数量。
基于深度学习的Yolo-tiny方法^[16]具有准确率高、计算速度快等特点。首先，采用Yolo-tiny方法获得爬壁机器人的初始目标矩形框; 其次，采用相同硬件配置下计算速度更快的KCF方法^[17]对机器人进行实时跟踪。爬壁机器人初步位置定位流程如图 4所示，当目标跟踪响应值不大于阈值时，目标矩形边框与实际位置有偏差，采用Yolo-tiny方法重新初始化。通过上述过程，得到机器人目标矩形框，实现初步位置定位。

图 4 爬壁机器人初步位置定位流程图

图选项

2.2 基于法向量方向投影的位置定位方法通过2.1节中的初步位置定位可以获得爬壁机器人在彩色图像中的目标矩形框位置(图 5)。然而，目标矩形框范围超过机器人自身的大小，通过上述方法只能获得位置的粗略估计值。同时，对于在机器人外壳上进行颜色标记(如ArUco标记^[13])的定位方法，测量精度容易受到环境光照的影响。针对上述问题，本文提出了一种基于法向量方向投影的位置定位方法，根据光路方向上距离的改变，及其在机器人底盘法向量方向的投影长度来分离出机器人外壳体顶部点云，从而筛选出机器人外壳体上的中心点。

图 5 钢制构件RGB图像及对应的三维点云

图选项

在进行位置定位前，需要获取大型构件和爬壁机器人的三维点云。RGB-D相机将三维空间点映射到二维图像平面的过程符合针孔相机模型。如图 6所示，定义像素坐标系UO_cV，以图像平面的左上角顶点为原点O_c，U轴和V轴分别平行于相机坐标系O_R-X_RY_RZ_R中的X_R轴和Y_R轴。图像平面内的中心点C(c_x, c_y)，在相机坐标系中坐标是(0, 0, f)，f是相机的焦距。

图 6 相机坐标系与像素坐标系

图选项

空间中一点A(x, y, z)投影到图像平面上的点a(u, v)，通过式(2)恢复该点在相机坐标系下的三维坐标：

$(x, y, z)^{\mathrm{T}}=\boldsymbol{K}\left(u-c_{x}, v-c_{y}, 1\right)^{\mathrm{T}} d / s, $

(2)

$\boldsymbol{K}=\left[\begin{array}{lll}f_{x}^{-1} & & \\& f_{y}^{-1} & \\& & 1\end{array}\right].$

(3)

其中：f_x和f_y分别是相机在X_R和Y_R方向上的焦距，s是深度的距离因子，d是该点到相机平面的距离。通过OpenCV开源库对获取的彩色图像和深度图像进行标定，并获取参数c_x、c_y、f_x、f_y的值。根据式(2)计算钢制构件的三维点云如图 5b所示。
由于爬壁机器人底盘与大型构件直径尺寸相差大，可将其接触面处近似为平面。如图 7所示，在RGB-D相机视野中，光路L₁~L₄上在大型构件表面的点B₁~B₄因爬壁机器人的遮挡，分别移动到机器人外壳体表面上的A₁~A₄点。照射在机器人顶部的光线L₁和L₂，在光路方向上的距离分别减小了|A₁B₁|和|A₂B₂|。照射在机器人侧面的光线L₃和L₄，在光路方向上的距离分别减小了|A₃B₃|、|A₄B₄|。

图 7 机器人遮挡引起的距离改变示意图

图选项

设像素坐标系中点a(u, v)对应机器人表面上的点A(x, y, z)，当无机器人遮挡时，a点对应大型构件表面上的点B(x′, y′, z′)，倾斜线段AB表示如下：

$\boldsymbol{A B}=\boldsymbol{O B}-\boldsymbol{O A}=\left(\frac{u-c_{x}}{f_{x}}, \frac{v-c_{y}}{f_{y}}, 1\right) \frac{1}{s} \Delta \boldsymbol{d}(u, v).$

(4)

其中Δd(u, v)是在像素点a(u, v)处有机器人遮挡时产生的深度差。
定义机器人底盘法向量为n(n_x^k, n_y^k, n_z^k)^T，与机器人和大型钢制构件接触点处的法向量共线，根据大型构件的三维点云计算。将倾斜线段AB在n方向上投影见式(5)。机器人顶部对应的倾斜线段投影长度等于机器人高度，侧面对应的倾斜线段投影长度h(j)小于机器人高度H。

$h(j)=\boldsymbol{A B} \cdot \boldsymbol{n}, $

(5)

$\left(p_{x}, p_{y}, p_{z}\right)^{\mathrm{T}}={ }_{\mathrm{R}}^{\mathrm{n}} \boldsymbol{C}\left(p_{x}^{\prime}, p_{y}^{\prime}, p_{z}^{\prime}\right)^{\mathrm{T}}={ }_{\mathrm{R}}^{\mathrm{n}} \boldsymbol{C} \frac{1}{N} \sum\limits_{1}^{N} k_{j}, $

(6)

在机器人顶部的三维点集{k_j|h(j)=H}中，根据式(6)计算该点集的质心(p′_x, p ′_y, p ′_z)，并将其从相机坐标系转换到导航坐标系，获得机器人的位置(p_x, p_y, p_z)，其中$ _{\rm{R}}^{\rm{n}}\mathit{\boldsymbol{C}}$是从相机坐标系到导航坐标系的转换矩阵，因两坐标系原点重合，平移向量为0。
2.3 基于串联EKF的姿态估计爬壁机器人通过式(6)获得位置定位信息到达指定地点后，由于清洗、检测等不同的任务要求，还需进一步确定爬壁机器人的姿态。为此，本文提出了一种基于串联EKF的姿态估计方法实现姿态定位，结构如图 3所示。以IMU测量的重力矢量信息作为观测量，设计EKF进行第一次滤波计算横滚角、俯仰角; 以机器人底盘法向量和第一次EKF计算的横滚角作为观测量，设计EKF进行第二次滤波计算航向角。
采用局部方向余弦法计算爬壁机器人姿态，表示如下：

${ }_{\mathrm{b}}^{\mathrm{n}} \dot{C}_{k}={ }_{\mathrm{b}}^{\mathrm{n}} \boldsymbol{C}\left[\omega_{k} \times\right], $

(7)

$\left[\omega_{k} \times\right]=\left[\begin{array}{ccc}0 & -\left(\omega_{z}^{k}-b_{z}^{k}\right) & \omega_{y}^{k}-b_{y}^{k} \\\omega_{z}^{k}-b_{z}^{k} & 0 & -\left(\omega_{x}^{k}-b_{x}^{k}\right) \\-\left(\omega_{y}^{k}-b_{y}^{k}\right) & \omega_{x}^{k}-b_{x}^{k} & 0\end{array}\right], $

(8)

${ }_{\mathrm{b}}^{\mathrm{n}} \boldsymbol{C}_{k+1}={ }_{\mathrm{b}}^{\mathrm{n}} \boldsymbol{C}_{k}+{ }_{\mathrm{b}}^{\mathrm{n}} \dot{\boldsymbol{C}}_{k} \cdot t_{\mathrm{s}} .$

(9)

其中：ω_x^k、ω_y^k、ω_z^k分别是IMU三轴角速度，b_x^k、b_y^k、b_z^k分别是IMU三轴角速度漂移。
选用$_{\rm{b}}^{\rm{n}}\mathit{\boldsymbol{C}} $矩阵的第三行元素(-θ_s, ?_sθ_c, ?_cθ_c)和IMU三轴角速度漂移作为第一个EKF的状态量¹x_k，第一个EKF的状态方程如式(11)，根据式(11)计算出系统状态转移矩阵¹F_k如式(13)。其中，W_c^k和W_b^k为0均值的过程白噪声向量，为简化公式，C₃₁^k、C₃₂^k、C₃₃^k分别表示-θ_s、?_sθ_c、?_cθ_c，t_s是系统采样时间，

${ }^{1} \boldsymbol{x}_{k}=\left(C_{31}^{k} C_{32}^{k} C_{33}^{k} b_{x}^{k} b_{y}^{k} b_{z}^{k}\right)^{\mathrm{T}}, $

(10)

$\begin{gathered}{ }^{1} \boldsymbol{x}_{k+1}=f\left({ }^{1} x_{k}\right)= \\{ }^{1} \boldsymbol{x}_{k}+t_{\mathrm{s}}\left(\begin{array}{c}{\left[C_{3 \times}\right]_{k}} \\0_{3 \times 3}\end{array}\right)\left(\begin{array}{c}\omega_{x}^{k}-b_{x}^{k} \\\omega_{y}^{k}-b_{y}^{k} \\\omega_{z}^{k}-b_{z}^{k}\end{array}\right)+\left(\begin{array}{c}\boldsymbol{W}_{\mathrm{c}}^{k} \\\boldsymbol{W}_{\mathrm{b}}^{k}\end{array}\right), \end{gathered}$

(11)

$\left[C_{3 \times}\right]_{k}=\left(\begin{array}{ccc}0 & -C_{33}^{k} & C_{32}^{k} \\C_{33}^{k} & 0 & -C_{31}^{k} \\-C_{32}^{k} & C_{31}^{k} & 0\end{array}\right), $

(12)

${ }^{1} \boldsymbol{F}_{k}=\frac{\partial f\left({ }^{1} \boldsymbol{x}_{k}\right)}{\partial\left({ }^{1} \boldsymbol{x}_{k}\right)}.$

(13)

爬壁机器人低速运动，其加速度近似等于重力矢量，以IMU测得的重力矢量作为观测量，表示如下：

${ }^{1} \boldsymbol{z}_{k}=\left(a_{x}^{k} a_{y}^{k} a_{z}^{k}\right)^{\mathrm{T}} .$

(14)

第一个EKF的观测方程和观测矩阵H₁分别表示如下：

${ }^{1} \boldsymbol{z}_{k}=\boldsymbol{H}_{1}{ }^{1} \boldsymbol{x}_{k}+{ }^{1} \boldsymbol{v}_{k}, $

(15)

$\boldsymbol{H}_{1}=\left(\begin{array}{ll}-g \boldsymbol{I}_{3} & \bf{0}_{3 \times 3}\end{array}\right).$

(16)

式中¹v_k是观测噪声向量。
第一次EKF过程的步骤如下：
步骤1 ? 从IMU读入ω_x^k、ω_y^k、ω_z^k和a_x^k、a_y^k、a_z^k，分别代入状态方程式(11)和观测方程式(15)计算在k-1时刻预测的状态量¹x_k|k-1和预测的观测量¹z_k|k-1。
步骤2 ? 根据式(17)计算协方差矩阵P_k|k-1，根据式(18)计算Kalman增益K_k，其中系统噪声协方差矩阵¹Q和观测噪声协方差矩阵¹R为主对角阵，对角元素为对应噪声分量的方差值，通过静止状态下的测量值估算得到:

$\begin{gathered}\boldsymbol{P}_{k \mid k-1}={ }^{1} \boldsymbol{F}_{k-1} \boldsymbol{P}_{k-1 \mid k-1}{ }^{1} \boldsymbol{F}_{k-1}^{\mathrm{T}}+{ }^{1} \boldsymbol{Q}, \end{gathered}$

(17)

$\boldsymbol{K}_{k}=\boldsymbol{P}_{k \mid k-1} \boldsymbol{H}_{1}^{\mathrm{T}}\left(\boldsymbol{H}_{1} \boldsymbol{P}_{k \mid k-1} \boldsymbol{H}_{1}^{\mathrm{T}}+{ }^{1} \boldsymbol{R}\right)^{-1} .$

(18)

步骤3 ? 根据式(19)更新状态量x_k|k，根据式(20)更新协方差矩阵P_k|k，

$\begin{gathered}\boldsymbol{x}_{k \mid k}={ }^{1} \boldsymbol{x}_{k \mid k-1}+\boldsymbol{K}_{k}\left({ }^{1} \boldsymbol{z}_{k}-{ }^{1} \boldsymbol{z}_{k \mid k-1}\right), \end{gathered}$

(19)

$\boldsymbol{P}_{k \mid k}=\boldsymbol{P}_{k \mid k-1}-\boldsymbol{K}_{k}\left(\boldsymbol{H}_{1} \boldsymbol{P}_{k \mid k-1} \boldsymbol{H}_{1}^{\mathrm{T}}+{ }^{1} \boldsymbol{R}\right) \boldsymbol{K}_{k}^{\mathrm{T}} .$

(20)

以航向角的余弦值和正弦值作为第二次EKF的状态量，表示如下：

${ }^{2} \boldsymbol{x}_{k}=\left(\cos \psi_{k} \sin \psi_{k}\right)^{\mathrm{T}}.$

(21)

通过第一次EKF获得k时刻的横滚角?_k和俯仰角θ_k以及k+1时刻的俯仰角θ_k+1，代入式(6)—(9)，计算$_{\rm{b}}^{\rm{n}}\mathit{\boldsymbol{C}} $第1行第1列和第2行第1列的元素，可得第二次EKF的状态方程和系统状态转移矩阵²F_k表示如下：

${ }^{2} \boldsymbol{x}_{k+1}={ }^{2} \boldsymbol{F}_{k} \cdot{ }^{2} \boldsymbol{x}_{k}+\boldsymbol{W}_{\psi}^{k}, $

(22)

$\begin{aligned}&{ }^{2} \boldsymbol{F}_{k}(1, 1)=\frac{\cos \theta_{k}+t_{\mathrm{s}} \widetilde{\omega}_{z}^{k} \sin \phi_{k} \sin \theta_{k}-t_{\mathrm{s}} \widetilde{\omega}_{y}^{k} \cos \phi_{k} \sin \theta_{k}}{\cos \theta_{k+1}}, \\&{ }^{2} \boldsymbol{F}_{k}(1, 2)=\frac{-t_{\mathrm{s}} \widetilde{\omega}_{z}^{k} \cos \phi_{k}-t_{\mathrm{s}} \widetilde{\omega}_{y}^{k} \sin \phi_{k}}{\cos \theta_{k+1}} , \\&{ }^{2} \boldsymbol{F}_{k}(2, 1)=-{ }^{2} \boldsymbol{F}_{k}(1, 2) , \\&{ }^{2} \boldsymbol{F}_{k}(2, 2)={ }^{2} \boldsymbol{F}_{k}(1, 1), \end{aligned}$

(23)

$\left\{\begin{array}{l}\widetilde{\omega}_{y}^{k}=\omega_{y}^{k}-b_{y}^{k}, \\\widetilde{\omega}_{z}^{k}=\omega_{z}^{k}-b_{z}^{k} .\end{array}\right.$

(24)

其中t_s是IMU的采样时间间隔，ω_y^k、ω_z^k分别是k时刻IMU中Y_i轴和Z_i轴角速度，b_y^k、b_z^k分别是k时刻IMU中Y_i轴和Z_i轴角速度漂移，W_ψ^k是0均值的过程白噪声向量。
在磁干扰条件下，不能通过磁强计来校准航向角。将2.2节中获得的机器人底盘法向量n(n_x^k, n_y^k, n_z^k)^T从相机坐标系转换到导航坐标系中为n′(n_x^k, n_z^k, -n_y^k)^T，再通过姿态矩阵$_{\rm{b}}^{\rm{n}}\mathit{\boldsymbol{C}} $^T投影到机器人本体坐标系后获得的向量与Y_b轴平行，即：

$\left(\begin{array}{lll}0 & 1 & 0\end{array}\right)^{\mathrm{T}}={ }_{\mathrm{b}}^{\mathrm{n}} \boldsymbol{C}^{\mathrm{T}} \cdot\left(\begin{array}{lll}n_{x}^{k} & n_{z}^{k} & -n_{y}^{k}\end{array}\right)^{\mathrm{T}} \text {, }$

(25)

$\left(n_{x}^{k}\right)^{2}+\left(n_{z}^{k}\right)^{2}+\left(-n_{y}^{k}\right)^{2}=1.$

(26)

将式(6)和式(26)代入式(25)，消去θ_k，得到机器人底盘法向量n(n_x^k, n_y^k, n_z^k)^T与机器人航向角的关系表示如下：

$\cos \phi_{k}=\left(-\sin \psi_{k} \quad 0 \quad \cos \psi_{k}\right) \boldsymbol{n}.$

(27)

根据式(27)写出第二次EKF的观测方程如式(28)，以横滚角的余弦值作为系统的观测量，

$\begin{gathered}{ }^{2} \boldsymbol{z}_{k}=\cos \phi_{k}, \\{ }^{2} \boldsymbol{z}_{k}=\boldsymbol{H}_{2}{ }^{2} \boldsymbol{x}_{k}+{ }^{2} \boldsymbol{v}_{k}.\end{gathered}$

(28)

其中观测矩阵H₂=(n_z^k -n_x^k)，²v_k是观测噪声。
第2次EKF过程与第一次EKF相同，用参数²F_k、H₂、²Q、²R及式(22), 式(28)分别替代¹F_k、H₁、¹Q、¹R及式(11), 式(15)代入步骤1—3。其中²Q和²R分别是第2次EKF的系统噪声协方差矩阵和观测噪声协方差矩阵。
3 实验及讨论在充分光照条件下，采用ArUco标记^[13]方法计算爬壁机器人位置和姿态作为真实值。采用跟踪准确率高的MS3D方法^[15]作为位置定位实验对照。采用方向余弦矩阵(direction cosine matrix, DCM)方法^[18]作为姿态估计实验对照。DCM方法与本文方法在横滚角和俯仰角估计中都采用重力矢量作为观测量，测量误差仅与经验参数相关，因此本文将重点比较2种方法的航向角测量精度。
实验装置如图 8所示，爬壁机器人，长0.25 m，宽0.19 m，高0.27 m; 钢制构件，高3 m，宽2.5 m; RGB-D相机，Microsoft Kinect v2，采集频率是10 Hz，最大有效测量范围宽6.3 m，高5.2 m; IMU，ADIS16445，采样频率100 Hz，其中的加速度计偏置稳定度为0.075×10^-3 g，输出噪声为2.25×10^-3 g，灵敏度为0.25×10^-3 g/LSB。为保证横滚角和俯仰角的测量精度，IMU中加速度计的灵敏度不超过3.9×10^-3 g/LSB，输出噪声不超过4.3×10^-3 g。在Ubuntu 16.04系统运行，CPU为Intel I7-3610 HQ，显卡为GTX1050Ti，8 GB内存。本文提出的RGB-D+IMU自主定位方法单次运行时间0.096 s。实验过程如图 9所示，地面遥控爬壁机器人在钢制构件表面沿着近似水平(见图 9a)和竖直(见图 9b)的路线运动，每组实验重复20次。

图 8 爬壁机器人实际试验

图选项

图 9 爬壁机器人运动的绝对轨迹图

图选项

图 10显示了爬壁机器人在近似水平和竖直路线上的定位误差，本文方法在大部分时间段内的定位精度优于MS3D方法。MS3D方法在水平路线(图 10a)中的平均位置定位误差为0.07 m，在竖直路线(图 10b)中的平均位置定位误差为0.05 m，最大位置定位误差超过0.18 m; 本文方法平均位置定位误差为0.01 m，最大位置定位误差不超过0.02 m。

图 10 爬壁机器人位置定位误差

图选项

爬壁机器人姿态估计误差如图 11所示。图 11a对应水平路线中的运动，本文方法计算的横滚角误差绝对值在2.5°以内，俯仰角误差绝对值在1.5°以内。图 11b对应竖直路线中的运动，本文方法计算的横滚角和俯仰角误差绝对值都保持在1.5°以内。机器人在水平路线中运动，多次制动微调方向引入了运动加速度，导致横滚角的计算误差增大。姿态估计中航向角计算误差如图 11c和11d所示，本文方法的计算误差绝对值最大值为2.5°，DCM方法的计算误差在起始阶段趋于0，但随着时间增长，航向角误差绝对值有累积趋势，并超过3°。

图 11 爬壁机器人姿态估计误差

图选项

4 结论本文提出并实现了一种外部RGB-D相机与机载IMU组合的爬壁机器人自主定位方法。该方法采用Yolo-tiny和KCF组合的目标跟踪方法进行初步位置定位; 在此基础上提出并实现了一种法向量方向投影的方法来筛选机器人外壳体上的中心点，实现机器人的位置定位; 以机载IMU测得的重力矢量信息作为观测量，设计了第1个EKF计算横滚角和俯仰角; 以通过外部视觉获得的机器人底盘法向量和通过第1个EKF计算的横滚角作为观测量，设计了第2个EKF计算航向角，实现机器人的姿态估计。
实验结果表明：相比于MS3D方法，本文方法使爬壁机器人位置定位误差从0.18 m下降至0.02 m左右; 姿态估计的航向角误差和横滚角误差绝对值基本保持在2.5°以内，俯仰角误差绝对值保持在1.5°以内，相比于DCM方法，航向角误差基本不随时间累积。
当机器人工作范围超过单个RGB-D相机的最大视野时，需要通过多个RGB-D相机组合的方式扩大视野范围。

参考文献

[1]	SHUKLA A, KARKI H. Application of robotics in offshore oil and gas industry: A review Part Ⅱ[J]. Robotics and Autonomous Systems, 2016, 75: 508-524. DOI:10.1016/j.robot.2015.09.013
[2]	中华人民共和国国家能源局. 承压设备无损检测第3部分超声检测: NB/T47013.3—2015[S]. 北京: 中国标准出版社, 2015. National Energy Administration of the People's Republic of China. Nondestructive testing of pressure equipments Part 3: Ultrasonic testing: NB/T47013.3-2015[S]. Beijing: Standards Press of China, 2015. (in Chinese)
[3]	CHANG C L, CHANG C Y, TANG Z Y, et al. High-efficiency automatic recharging mechanism for cleaning robot using multi-sensor[J]. Sensors, 2018, 18(11): 3911. DOI:10.3390/s18113911
[4]	ZHU J S, SUN Z G, HUANG W, et al. Design of a master-slave composite wall climbing robot system for penstock assembly welding[C]//12th International Conference on Intelligent Robotics and Applications. Shenyang, China: Springer, 2019: 123-134.
[5]	KIM Y, AN J, LEE J. Robust navigational system for a transporter using GPS/INS fusion[J]. IEEE Transactions on Industrial Electronics, 2018, 65(4): 3346-3354. DOI:10.1109/TIE.2017.2752137
[6]	FAN J Z, XU T, FANG Q Q, et al. A novel style design of a permanent-magnetic adsorption mechanism for a wall-climbing robot[J]. Journal of Mechanisms and Robotics, 2020, 12(3): 035001. DOI:10.1115/1.4045655
[7]	邢海峰, 陈志勇, 张新喜, 等. MIMU在不同情况下的可观测性分析[J]. 清华大学学报(自然科学版), 2020, 60(1): 82-88. XING H F, CHEN Z Y, ZHANG X X, et al. Observability analysis of MIMU devices in different conditions[J]. Journal of Tsinghua University (Science and Technology), 2020, 60(1): 82-88. (in Chinese)
[8]	VENKATNARAYAN R H, SHAHZAD M. Enhancing indoor inertial odometry with WiFi[J]. Proceedings of the ACM on Interactive, Mobile, Wearable and Ubiquitous Technologies, 2019, 3(2): 47.
[9]	FAN Q G, SUN B W, SUN Y, et al. Data fusion for indoor mobile robot positioning based on tightly coupled INS/UWB[J]. The Journal of Navigation, 2017, 70(5): 1079-1097. DOI:10.1017/S0373463317000194
[10]	MUR-ARTAL R, TARDóS J D. ORB-SLAM2: An open-source SLAM system for monocular, stereo, and RGB- D cameras[J]. IEEE Transactions on Robotics, 2017, 33(5): 1255-1262. DOI:10.1109/TRO.2017.2705103
[11]	NEWCOMBE R A, IZADI S, HILLIGES O, et al. KinectFusion: Real-time dense surface mapping and tracking[C]//2011 10th IEEE International Symposium on Mixed and Augmented Reality. Basel, Switzerland: IEEE, 2011: 127-136.
[12]	JIANG P, CHEN L, GUO H, et al. Novel indoor positioning algorithm based on Lidar/inertial measurement unit integrated system[J]. International Journal of Advanced Robotic Systems, 2021, 18(2): 1729881421999923.
[13]	ROMERO-RAMIREZ F J, MU?OZ-SALINAS R, MEDINA-CARNICER R. Speeded up detection of squared fiducial markers[J]. Image and vision Computing, 2018, 76: 38-47. DOI:10.1016/j.imavis.2018.05.004
[14]	MERRIAUX P, DUPUIS Y, BOUTTEAU R, et al. A study of Vicon system positioning performance[J]. Sensors, 2017, 17(7): 1591. DOI:10.3390/s17071591
[15]	ZHAO Y H, MENEGATTI E. MS3D: Mean-shift object tracking boosted by joint back projection of color and depth[C]//15th International Conference on Intelligent Autonomous Systems. Switzerland: Springer, 2018: 222-236.
[16]	REDMON J, FARHADI A. YOLOv3: An incremental improvement[J]. arXiv, 2018, 1804.
[17]	HENRIQUES J F, CASEIRO R, MARTINS P, et al. High-speed tracking with kernelized correlation filters[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 2015, 37(3): 583-596.
[18]	WANG Y, RAJAMANI R. Direction cosine matrix estimation with an inertial measurement unit[J]. Mechanical Systems and Signal Processing, 2018, 109: 268-284.