基于吸附约束的绳系和视觉惯性测距的爬壁机器人全局定位（学习笔记）

最新推荐文章于 2024-06-02 14:31:52 发布

窝爱吃西瓜

最新推荐文章于 2024-06-02 14:31:52 发布

阅读量534

点赞数 21

文章标签：机器人

本文链接：https://blog.csdn.net/weixin_43327442/article/details/138557213

版权

论文原标题为：Global Localization Based on Tether and Visual-Inertial Odometry With Adsorption Constraints for Climbing Robots.

摘要：本文提出了一种基于绳系位移传感器、视觉惯性里程计(VIO)和工件计算机辅助设计(CAD)模型的爬壁机器人全局定位方法。系绳位移传感器测量机器人与系绳锚之间的距离，漂移很小，使机器人能够获得全局姿态。利用CAD模型提取机器人运动中的吸附约束，以减小VIO漂移。该方法在没有其他外部定位设备的情况下，实现了机器人爬上大型工件时高精度的全局定位。通过在实际大型工件上的爬行机器人样机测试，验证了该方法的性能。在所有实验中，我们的吸附约束方法优于现有的VIO。在长度为8 m的风力涡轮机叶片上，合并轨迹在全局定位中的最大漂移低至0.51%。
视觉惯性SLAM定位的局限性：在爬壁机器人上使用时，相机的视场(FOV)受到机器人吸附表面的限制。对于爬壁机器人上的相机，一次测量只能观察到工件的一小部分，几乎是平面的。再加上机器人不可避免的振动，围绕重力的方位误差会迅速积累，导致主体定位误差甚至失效，因此，传统的视觉惯性里程计(VIO)不能很好地应用于爬壁机器人。
主要贡献：提出基于系绳位移传感器、VIO和吸附工件的计算机辅助设计(CAD)模型的爬壁机器人全局定位系统。它可以通过初始化全局位姿和相对里程计获得机器人相对于吸附工件的全局位姿。全局位姿初始化是爬壁机器人定位的一种新颖便捷的方法，可以确定机器人与其吸附工件之间的初始位姿转换关系。相对里程计测量是基于视惯性约束和吸附约束的紧密耦合，可以大规模消除VIO的偏航角漂移，实现鲁棒相对定位。

图1 对吸附工件进行操作的爬壁机器人需要全局位姿来定位工作区域，需要鲁棒的相对里程计来跟踪规划操作路径。

4.定位难点：爬壁机器人在大型复杂工件上进行大规模定位的主要难点包括初始全局位姿测量和高
精度的鲁棒相对里程计测量。

5.IMU预积分：基于优化的VIO的常用方法是构造包含视觉约束和惯性约束的状态估计器。视觉约束由图像之间的重投影误差组成。在图像之间进行imu测量积分，统一传感器之间的测量频率，称为预积分。滑动窗口固定问题的尺度，只优化窗口内的状态。将丢弃的信息视为先验信息。通过最小化滑动窗口中约束误差项的和来优化IMU的姿态状态估计和偏置。

系统术语命名

6.状态向量定义

$\chi=\left[\mathbf{x}_{0}, \mathbf{x}_{1}, \mathbf{x}_{2} \ldots \ldots \mathbf{x}_{\mathrm{n}}\right]$ (1)

$\mathbf{x}_{k}=\left[\mathbf{p}_{b_{k}}^{w}, \mathbf{v}_{b_{k}}^{w}, \mathbf{q}_{b_{k}}^{w}, \mathbf{b}_{a}, \mathbf{b}_{g}\right], k \in[0, n]$ (2)

其中Xk是捕获第k张图像时的IMU状态。它包含IMU在世界框架中的位置、速度和方向，以及在机器人本体坐标系中的加速度偏差和陀螺仪偏差。我们将机器人本体坐标系定义为与IMU坐标系相同。

VIO通过视觉惯性BA（bundle adjustment）约束，公式为：

$\min _{\chi}\left\{\begin{array}{c} \left\|\mathbf{r}_{p}-\mathbf{H}_{p} \boldsymbol{\chi}\right\|^{2}+\sum_{k \in B}\left\|\mathbf{r}_{B}\left(\widehat{\mathbf{z}}_{b_{k+1}}^{b_{k}}, \boldsymbol{\chi}\right)\right\|_{\mathbf{P}_{b_{k+1}}^{b_{k}}}^{2} \\ +\sum_{(l, j) \in C} \rho_{c}\left(\left\|\mathbf{r}_{C}\left(\widehat{\mathbf{z}}_{l}^{c_{j}}, \chi\right)\right\|_{\mathbf{P}_{l}^{c_{j}}}^{2}\right) \end{array}\right\}$ （3）

其中 $\mathbf{r}_{B}\left(\widehat{\mathbf{z}}_{b_{k+1}}^{b_{k}}, \chi\right)$ 和 $\mathbf{r}_{C}\left(\widehat{\mathbf{z}}_{l}^{c_{j}}, \chi\right)$ 是IMU和视觉测量的残差。 $\left\{\mathbf{r}_{p}, \mathbf{H}_{p}\right\}$ 是边缘化的先验信息。

紧密耦合的VIO可以获取重力信息，这是一种无漂移的全局观测。主要的累积误差包括绕重力方向和3-D平移，特别是对于摄像机视场受限且振动强的爬壁机器人。为了解决这一问题，我们引入了系绳长度信息和工件的CAD模型。

绳系位移传感器用于获得VIO的世界坐标系（world frame）和全局坐标系（global frame）之间的变换。假设工件坐标系（workpiece frame）与全局坐标系相同，其z轴与重力对齐。

$\begin{bmatrix}\mathbf{R}_b^g&\mathbf{t}_b^g\\0&1\end{bmatrix}=\mathbf{T}_w^g\begin{bmatrix}\mathbf{R}_b^w&\mathbf{t}_b^w\\0&1\end{bmatrix}$ (4)

$\mathbf{T}_w^g=\begin{bmatrix}\mathbf{R}_w^g&\mathbf{t}_w^g\\0&1\end{bmatrix}$ (5)

通过公式(4)中的变换，我们可以将世界坐标系中的机器人姿态映射到全局坐标系中。将机器人位置投影到全局坐标系中的吸附表面上，得到吸附信息，包括投影位置和表面法线。在视觉惯性BA中加入由吸附信息构造的吸附约束项。吸附约束固定了VIO的四个浮动自由度，提高了爬壁机器人状态估计器的精度和鲁棒性。

图2 大型复杂工件攀爬机器人全局定位系统概述

7.吸附约束VIO与全局位姿初始化

假设被吸附工件的CAD模型是已知的。如图2所示，我们提出的方法有两个主要部分，即全局位姿初始化和具有吸附约束的鲁棒VIO。根据现有的研究，VIO能够在小规模友好环境[15]中提供精确的3-D运动跟踪，在这种情况下，里程计的漂移在短时间内几乎可以忽略不计。基于这一假设，我们利用系绳位移传感器和VIO的测量值，通过单点定位(SPP)算法计算锚点在世界坐标系中的位置。然后，根据被吸附工件的CAD模型初始化整体位姿。在全局姿态初始化之后，我们可以将机器人本体坐标系映射到全局坐标系上，并将机器人投影到被吸附的工件上获取表面吸附信息。最后，我们利用吸附信息进行基于视觉惯性约束和吸附约束的紧密耦合测程。

7.1全局位姿初始化和绳系位移传感器

在全局位姿初始化中，基于系绳位移传感器、VIO和被吸附工件的CAD模型来初始化世界坐标系和全局坐标系之间的转换。锚点与本体坐标系之间的距离可以从系绳位移传感器中获取。

$\mathbf{P}_{ap}^w=\begin{bmatrix}x_{ap}^w,y_{ap}^w,z_{ap}^w\end{bmatrix}^T$ （6）

$l_k^2=\left(x_b^w-x_{ap}^w\right)^2+\left(y_b^w-y_{ap}^w\right)^2+\left(z_b^w-z_{ap}^w\right)^2$ （7）

（公式6是锚点ap在世界坐标系w下的三维坐标；公式7是世界坐标系w下本体坐标系b和锚点ap之间距离的平方）

当初始化VIO时，我们使爬壁机器人沿着给定的期望运动轨迹运动，该运动轨迹在吸附表面切平面的两个维度（上下左右移动）上有足够的刺激。初始运动时，VIO提供运动控制的位置反馈信息。在初始化过程中，假定短时轨迹是准确的。因此，使用系绳位移传感器，可以通过SPP算法计算世界坐标系中的锚点位置，该算法被公式化为最小二乘问题

图3 基于锚点、VIO和吸附工件的CAD模型，初始化世界坐标系和全局坐标系之间的偏航角的迭代过程的顶视图。(a)初始化.(b)迭代(c)结束

假设系绳锚点的位置在全局坐标系中是已知的。世界坐标系和全局坐标系之间的转换关系只有四维的不确定性。在世界坐标系和全局坐标系之间存在锚点的对应关系

$\mathbf{P}_{ap}^w=\mathbf{R}_g^w\mathbf{P}_{ap}^g+\mathbf{t}_g^w$ （13）

偏航角的确定与吸附表面的约束，在这种情况下，我们可以完成校准的机器人的初始全局位姿的迭代。在迭代的初始状态下，我们设置世界坐标系和全局坐标系之间的偏航角偏差为0

VIO的初始化轨迹可以被映射到具有 $\mathrm{T}_w^g$ 的中间结果的全局帧。将全局坐标系中的初始化轨迹通过投影的方式投影到被吸附工件的CAD模型上。初始化轨迹和投影轨迹之间的偏航角偏差反映了世界坐标系和全局坐标系之间的旋转关系：

其中m和n分别是VIO轨迹和投影轨迹在全局坐标系中的平移向量，（·）xy是由法向量的xy分量形成的2-D向量。

迭代过程的步骤在算法1中示出。初始化轨迹和投影轨迹之间的偏航角偏差将逐渐消除，如图3所示。利用正确的偏航角，通过（16）中的世界坐标系和全局坐标系中的锚点位置来计算变换。

由于吸附表面的约束，爬行机器人在运动时只能保证表面切线上的二维激励。在退化的情况下，小曲率的吸附表面，机器人的轨迹可能退化到一个平面曲线。世界坐标系中的锚位置仍然可以通过公式（10）计算。然而，由于缺乏刺激，结果的正常成分是不可靠的。另外，曲面法线方向的沿着误差也会影响曲面切线方向的定位结果，这是投影法无法补偿的。为了解决这个问题，我们使用初始化轨迹和投影轨迹之间的转换来为（8）添加约束，并在第一次初始化之后重新计算锚点位置。在世界坐标系中，初始化轨迹和投影轨迹之间的平移为：

我们将投影轨迹的点拟合到一个平面上，以获得表面法线 $\mathbf{n}_{ap}^{w}$ 。为了消除初始轨迹和投影轨迹之间的平移差异，锚点位置应满足以下约束：

其中 $\mathbf{P}_{ap,o}^{w}$ 是原始计算结果， $\mathbf{P}_{ap}^{w}$ 是要计算的重新校准结果。对于（9）中的线性方程组，约束添加新方程为

利用优化的锚点位置，通过再次运行算法1来更新初始全局姿态。

7.2基于视觉、惯性和吸附约束的估计

在全局位姿初始化之后，我们可以基于被吸附工件的CAD模型获得机器人位置，在全局框架中的投影。CAD模型提供了两种关于投影点的吸附信息，包括点位置和表面法线。针对吸附约束，我们提出了一种多源信息融合估计器，该估计器使用视觉约束、惯性约束和吸附约束进行融合，以获得最优的位置估计。与VIO不同的是，我们的方法基于投影信息构建了包含位置和方向约束项的吸附约束。在该方法中，对VIO的四个浮动自由度进行了抑制，从而抑制了偏航角误差。

爬壁机器人的运动受到吸附表面的约束。在空间的六个维度中不可能有足够的刺激来在线初始化。因此，我们使用静态校准方法来初始化VIO模块。在初始化过程中，机器人应保持静止，并持续采集图像和IMU信息。如果图像视差或IMU测量值的方差大于阈值，则认为机器人正在运动，重新开始该过程。将平均角速度视为陀螺仪偏差，将平均加速度视为重力测量。完成后，VIO的世界坐标系将以相机坐标系的x轴作为x轴，重力作为z轴进行设置。当相机坐标系的x轴不垂直于重力时，则设置垂直于重力的分量。在随后的状态估计过程中，IMU的偏置将不断优化。

为了固定四个浮动自由度，提高VIO的精度和鲁棒性，我们提出了两个吸附约束的爬壁机器人的位姿估计过程。我们提出的BA公式如下：

$\mathbf{r}_{PR}\left(\widehat{\mathbf{z}}_{b_{k}},\chi\right)$ 是吸附约束项，其中包括偏航角误差和平移误差。我们提出的估计器的图模型如图4所示。

图4 基于视觉、惯性和吸附约束的紧耦合里程计图模型。

其中一个吸附约束条件是，三维位置应接近被吸附表面。而另一个则是机器人的偏航角要满足吸附面法线的约束。根据上述准则，我们构造了一个关于吸附面约束的误差项，如(22)所示。基于视觉、惯性和吸附面信息的状态估计器将消除仅VIO的偏航角漂移，并大大减少累积的平移误差

爬壁机器人所处的吸附面信息，可以在全局坐标系中以自身姿态获得。然而，利用CAD模型直接计算吸附面与机器人中心轴的交点和法线是非常耗时的。为了实现投影的实时操作，我们在点云上使用插值策略。具体来说，我们通过open cascade library将CAD模型离线转换为点云，并将其保存为八叉树结构。在转换过程中，计算每个点的表面法线。当机器人本体在世界坐标系中的姿态变换到全局坐标系时，通过投影获得吸附约束。投影操作在全局坐标系中实现，如图5所示。在大型工件上，传感器平台在吸附时与吸附表面保持恒定的高度。首先，对于VIO轨迹的每个点，我们通过机器人的姿态和恒定高度计算接触点。然后，我们在CADmodel的点云中搜索三个最接近接触点的非共线点formaplane。接下来，我们计算出平面与机体框架z轴的交点作为入射点。最后，投影法线是在最近的三个点上的法线的线性插值。而投影法线点由入射点、投影法线、等高计算。

图五.投影操作。在CAD模型转换的点云中，由离接触点最近的三个非共线点构成一个平面。入射点是平面与实体z轴的交点。投影法线是三个最近点上法线的线性插值。投影点由入射点和投影法线计算。

我们使用投影点位置和投影法线来构建(22)中的吸附约束。在参与估计时，将投影点和投影法线转换为世界框架。位置误差项表述为

俯仰角和滚转角受重力约束。因此，我们使用投影法线只更新偏航角。偏航误差项的公式为

其中(·)xy为法向量的xy分量组成的二维矢量， $\mathbf{n}_{bz}^{w}$ 和 $\mathbf{n}_{bz,pr}^w$ 为物体的z轴和投影法线，如图5所示。

窝爱吃西瓜

关注

21
点赞
踩
19

收藏

觉得还不错? 一键收藏
0
评论
基于吸附约束的绳系和视觉惯性测距的爬壁机器人全局定位（学习笔记）

最后，投影法线是在最近的三个点上的法线的线性插值。根据现有的研究，VIO能够在小规模友好环境[15]中提供精确的3-D运动跟踪，在这种情况下，里程计的漂移在短时间内几乎可以忽略不计。在全局位姿初始化之后，我们可以基于被吸附工件的CAD模型获得机器人位置，在全局框架中的投影。在全局位姿初始化中，基于系绳位移传感器、VIO和被吸附工件的CAD模型来初始化世界坐标系和全局坐标系之间的转换。图3 基于锚点、VIO和吸附工件的CAD模型，初始化世界坐标系和全局坐标系之间的偏航角的迭代过程的顶视图。
复制链接

扫一扫