0. 基础

基础知识点请参考基础篇。

1. 视觉伺服

参考：Visual servoing

1.1 视觉伺服基础

参考1：Tutorial: Image-based visual servo
参考2：关于视觉伺服控制的一些基本概念、原理、公式推导（建议观看）

1.1.1 基本理论

• 基于图像的视觉伺服(IBVS)：image-based visual servo
  基于图像点特征的视觉伺服；
  原理推导

• 基于位置的视觉伺服(PBVS)：position-based visual servo

• 视觉伺服控制系统
  以【实现：搭载深度相机的无人机在竖立的二维码正前方保持悬停】为例。
  

  • 被控对象：无人机、相机、二维码等组成的广义对象。

  • 系统输出： 视觉特征（visual features）。

  • 输出期望值：视觉特征期望值。

  • 系统误差：视觉特征误差。

  • 控制输入：相机坐标系下相机的速度向量Vc=[u,w]，其中u为线速度，w角速度。其实就是控制飞机的飞行速度(因为相机与飞机是固连的)。

  • 控制律：如下。理论推导请参考，或直接搜索视觉伺服。
    
    其中lambda是控制器的常数增益，一般设为0.5。

  • 相互作用矩阵Lx的表达式为：
    
    其中 Lx 为交互矩阵(interaction matrix)， 又称特征雅可比矩阵(feature Jacobian)。
    Lx一般不可以完全得到，可通过估计的方式获得：视觉伺服学习笔记——基本方法

  • 分析
    众所周知，在控制系统中，如何定义系统输出、输出期望值、系统误差 是跟我们的控制目标有关的。
    在本例子中，要实现的控制目标是：保持飞机（也就是相机）悬停在二维码正前方设定距离处。
    如果将该二维码看做一个3D点 p，那么对于相机而言，只要使得点 p 在相机坐标系下的坐标保持在一个固定值如【0, 0，L 】即可实现控制目标。
    这样一来，暂时可以把 p 点在相机坐标系下的坐标定义为控制系统的输出量，将二维码的中心定义为p点，那么【0, 0，L 】自然就是系统的输出量期望值，期望值与输出量之间的误差自然就是系统误差。
    但是对于2D视觉伺服控制系统而言，为了方便运算一般要把所有纲量统一到图像坐标系来做运算，分析如下：
    对于RGB-D相机：已知一个3D点 p 在相机坐标系下的坐标值为【X, Y, Z】；
    对于RGB相机：已知点 p 在像素坐标系中的坐标值(下标)为【u,v】；
    已知相机内参为 u0，v0，px，py ；
    那么，根据相机内参标定的原理公式可知，点 p 在图像坐标系中的坐标值的计算表达式如下：
    
    点 p 在图像坐标系中的坐标值【x,y】就是最终要定义的控制系统输出量，把【0, 0，L 】代替【X, Y, Z】或把图像中心坐标代替【u,v】就可以计算得到控制系统期望值，那么系统误差就是两者之差了。
    在传统的视觉伺服控制系统中，一般把点 p (或其他合适的点) 在图像坐标系中的坐标值【x,y】定义为 视觉特征（visual features），但是这并不是定义视觉特征的唯一选择，只能要能有助于实现控制目标的量都可以定义为视觉特征。
    到此就解释了为什么视觉特征为什么可以定义为视觉伺服系统的输出量了，也弄清了什么是视觉特征。
    对于本文而言，我们暂且把这些p点定义为视觉伺服系统的特征点，注意该概念与机器视觉中的特征点的不同之处！

• 视觉伺服理论中的一些结论
  详细推导请参考：https://zhuanlan.zhihu.com/p/422634446 或 https://zhuanlan.zhihu.com/p/389903710

  • 在IBVS中，视觉特征一般用一系列合适的点的图像坐标系坐标来表示。

  • 以点坐标表征的视觉特征称为点视觉特征，被选作视觉特征的点称为特征点。

  • eye-to-hand 和 eye-in-hand
    eye-to-hand：相机固定在一个地方，机械手的运动不会带着相机一起移动。
    eye-in-hand：相机安装在机械手上，随着机械手一起移动。

  • 视觉特征向量，例如：
    假设定义了n个视觉特征（选取n个点），视觉特征向量 s 的表达式为：

    $$s=\left[\begin{array}{c}{p}_1\\ ⋮\\ p_n\end{array}\right]={\left[\begin{array}{c}{x}_1\\ y_1\\ \\ ⋮\\ x_n\\ y_n\end{array}\right]}_{2n\ast 1}(式1)$$x,y是特征点在图像坐标系下的坐标值，什么是图像坐标系请查阅：相机标定篇

  • 控制向量Vc
    $$V_c=\left[\begin{array}{c}{v}_c\\ w_c\end{array}\right]={\left[\begin{array}{c}{v}_x\\ v_y\\ v_z\\ w_x\\ w_y\\ w_z\end{array}\right]}_{6\ast 1}(式2)$$其中，vc, wc 分别是线速度和角速度（相机系）。

  • 控制率
    $$V_c=-\lambda L_s^{+}e(式3)$$其中：$$e=s_e={\left[\begin{array}{c}{x}_{e_1}\\ y_{e_1}\\ ⋮\\ x_{e_n}\\ y_{e_n}\end{array}\right]}_{2n\ast 1}(式4)$$
    $$L_s={\left[\begin{array}{c}L{1}_s\\ ⋮\\ L{n}_s\end{array}\right]}_{2n\ast 6}={\left[\begin{array}{c}\left[\begin{array}{cccccc}-1/Z_1& 0& x_1/Z_1& x_1{y}_1& -(1+x_1^2)& y_1\\ 0& -1/Z_1& y_1/Z_1& 1+y_1^2& -x_1{y}_1& -x_1\end{array}\right]\\ ⋮\\ \left[\begin{array}{cccccc}-1/Z_n& 0& x_n/Z_n& x_n{y}_n& -(1+x_n^2)& y_n\\ 0& -1/Z_n& y_n/Z_n& 1+y_n^2& -x_n{y}_n& -x_n\end{array}\right]\end{array}\right]}_{2n\ast 6}(式5)$$
    $$L_s^{+}={\left[\begin{array}{c}\dots \end{array}\right]}_{6\ast 2n}(式6)$$是 Lx 的广义逆矩阵。
    很多文献中也把 Ls 记作 Le 或 Lx .

  • 通常存在4个不同的相机姿态使得e=0，即存在4个不可能区分的全局最优点。一般地，选择多于3个点，作为视觉特征向量。

  • 获取Lx的常用方法
    

1.1.2 代码解析(tutorial-ibvs-4pts.cpp)：

对于tutorial-ibvs-4pts.cpp例子而言，定义了4个视觉特征，即定义了4个跟踪点（上一节分析中提到的p点，或称视觉特征点）。

• 定义相机的期望位置、初始位置。若以跟踪对象作为参考系(跟踪对象是不动的)，则这两个位置量可以用坐标转换矩阵cdMo，cMo表示：

  vpHomogeneousMatrix cdMo(0, 0, 0.75, 0, 0, 0);
  vpHomogeneousMatrix cMo(0.15, -0.1, 1., vpMath::rad(10), vpMath::rad(-10), vpMath::rad(50));
  

  cMo矩阵的含义请参考本章的第 1.4.1 小节。

• 以【相机检测一个20cm*20cm矩形，让保持相机在矩形中心位置】为例。
  在世界坐标系下，定义4个点，表示检测对象的四个角，在此例中，检测对象即20x20的矩形：

  vpPoint point[4] ;
  point[0].setWorldCoordinates(-0.1,-0.1, 0);
  point[1].setWorldCoordinates( 0.1,-0.1, 0);
  point[2].setWorldCoordinates( 0.1, 0.1, 0);
  point[3].setWorldCoordinates(-0.1, 0.1, 0);
  

• 定义视觉伺服器：
  vpServo task ;

• 配置视觉伺服方式：eye in hand 模式
  task.setServo(vpServo::EYEINHAND_CAMERA);

• 定义4个视觉特征
  根据上一小节的分析可知，视觉特征就是被跟踪的点的图像坐标系坐标值。本例中，将矩形的4个角点设定为被跟踪的点，那么就会有4个视觉特征。表征视觉特征的4个点的坐标被保存在变量vpFeaturePoint p[4], pd[4];中，其中 p[4] 保存当前视频帧(图像帧)的视觉特征，pd[4] 保存视觉特征期望值。

• 计算视觉特征
  根据上一小节的分析可知，视觉特征是用跟踪的点的图像坐标系坐标值来表征的。

  • point[i].track(cdMo);
    将点 point[i] 的坐标转换到相机坐标系下，结果保存在 point[i] 的祖父类 vpTracker 的 cP 中；
    将点 point[i] 的坐标转换到图像坐标系下，结果保存在 point[i] 的祖父类 vpTracker 的 p 中；
    其中 point 的类型为 vpPoint，而 vpPoint 继承了vpForwardProjection，vpForwardProjection 继承了vpTracker，因此point 可以直接访问到 cP 和 p 。可知，point[i] 保存了第i点的世界系坐标、相机系坐标、图像系坐标。
  • vpFeatureBuilder::create(pd[i], point[i]);
    将点point[i]在图像系下的坐标 p 赋值给视觉特征变量 pd[i]，create函数如下：

    void vpFeatureBuilder::create(vpFeaturePoint &s, const vpPoint &p)
    {
        s.set_x(p.get_x());
        s.set_y(p.get_y());
        s.set_Z(p.cP[2] / p.cP[3]);
    }
    

  • task.addFeature(p[i], pd[i]);
    将保存视觉特征的变量的地址记录到(拷贝到)task的 featureList 和 desiredFeatureList 中，addFeature函数如下：

    void vpServo::addFeature(vpBasicFeature &s_cur, vpBasicFeature &s_star, unsigned int select)
    {
      featureList.push_back(&s_cur);
      desiredFeatureList.push_back(&s_star);
      featureSelectionList.push_back(select);
    }
    

    因为拷贝的是地址，因此一次就够了，后续cMo更新后不需要再进行这步操作。
  • 更新 cMo
    在仿真中，是可以直接知道相机在世界坐标系中的坐标的，因此可以通过这个坐标算出cMo矩阵，如下。但在具体应用场景中cMo一般是通过目标检测算法得到的。

    robot.getPosition(wMc);
    cMo = wMc.inverse() * wMo;
    

  • 相机位置改变后 cMo 被更新，进而需要更新视觉特征：与上一步类似，这里不再赘叙。

    for (unsigned int i = 0 ; i < 4 ; i++) {
       point[i].track(cMo);
       vpFeatureBuilder::create(p[i], point[i]);
     }
    

    不需要再进行addFeature操作，因为featureList、desiredFeatureList 已经记录了视觉特征的引用。

• 根据控制律公式，计算控制量Vc：vpColVector v = task.computeControlLaw();
  
  
  注意：控制律中Lx右上角的符号是求伪逆矩阵的意思。伪逆矩阵即广义逆矩阵。
  computeControlLaw 重点代码分析如下：

  • (一) 计算误差：computeError()
    根据featureList、desiredFeatureList计算系统误差，结果存入类vpServo 的三个变量中：

    class vpServo  {
    public：
    	vpColVector error; //保存所有视觉特征（点）的所有维度的误差
    	vpColVector s; //保存所有视觉特征（点）的所有维度的值
    	vpColVector sStar; //保存所有视觉特征（点）的所有维度的期望值
    }
    

    例如，定义了4个视觉特征，分别用4个点的坐标 [x1, y1, z1] ~ [x4, y4, z4]表示，保存在featureList中。
    那么 s，sStar，error 的最终尺寸大小都被定义为 12x1，其中12=3*4（4个点，每个点的坐标都是3个维度）。
    已知，featureList中的元素类型是vpBasicFeature ，类型vpBasicFeature 中有一个 vpColVector 类型的元素 s;
    已知，类 vpServo 中有三个类型为vpColVector 的成员：s，sStar，error。
    误差计算可以归纳为：
    • （1）对于视觉特征、视觉特征期望值，赋值过程的伪代码可以表示为：
      vpServo :: vpColVector s = featureList.at(i):: vpColVector s ;
      vpServo :: vpColVector sStar = desiredFeatureList.at(i):: vpColVector s ;
      实质上可以看作是将类vpBasicFeature中的成员s赋给类vpServo中的成员s.
    • （2）对于误差，赋值过程的伪代码可以表示为：
      vpServo :: vpColVector error = vpServo :: s - vpServo :: sStar ;
    • 例如：有4个视觉特征，分别用4个点的坐标 [x1, y1, z1] ~ [x4, y4, z4]表示，保存在featureList中。那么 s，sStar，error 的最终尺寸大小都被定义为 12x1，其中12=3*4（4个点，每个点的坐标都是3个维度）。s，sStar，error 的伪代码赋值逻辑如下：
      • s的赋值：
        vpServo :: s[0] = featureList.at(i) :: s[0] = x1 ;
        vpServo :: s[1] = featureList.at(i) :: s[1] = y1 ;
        vpServo :: s[2] = featureList.at(i) :: s[2] = z1 ;
        …
        vpServo :: s[9] = featureList.at(i+3) :: s[0] = x4 ;
        vpServo :: s[10] = featureList.at(i+3) :: s[1] = y4 ;
        vpServo :: s[11] = featureList.at(i+3) :: s[2] = z4 ;
      • sStar 同理。
      • error 的赋值：
        vpServo :: error[0] = vpServo :: s[0] - vpServo :: sStar[0] ; // x1的误差
        vpServo :: error[1] = vpServo :: s[1] - vpServo :: sStar[1] ; // y1的误差
        vpServo :: error[2] = vpServo :: s[2] - vpServo :: sStar[2] ; // z1的误差
        …
        vpServo :: error[9] = vpServo :: s[9] - vpServo :: sStar[9] ; // x4的误差
        vpServo :: error[10] = vpServo :: s[10] - vpServo :: sStar[10] ; // y4的误差
        vpServo :: error[11] = vpServo :: s[11] - vpServo :: sStar[11] ; // z4的误差
    • vpServo.cpp 中计算误差vpServo::error的几个重要的函数盘点：
      (1) 从vpBasicFeature类中提取vpColVector s ：

      vectTmp = current_s -> get_s( ) 
      

      (2) 复制 vpBasicFeature :: s 到 vpServo :: s

      s[cursorS++] = vectTmp[k]; //其中s[0],s[1],s[2]分别是表征视觉特征的坐标值xyz
      sStar[cursorSStar++] = vectTmp[k];
      

      (3) 计算误差

      vectTmp = current_s->error(*desired_s, select);
      error[cursorError++] = vectTmp[k];
      

      //! Compute the error between two visual features from a subset of the
      //! possible features.
      vpColVector vpBasicFeature::error(const vpBasicFeature &s_star, unsigned int select)
      {
        vpColVector e(0), eLine(1);
        if (dim_s <= 31) {
          for (unsigned int i = 0; i < dim_s; ++i) {
            if (FEATURE_LINE[i] & select) {
              eLine[0] = s[i] - s_star[i];
              e.stack(eLine);
              // std::cout << "dim_s <= 31"<<std::endl;
            }
          }
        } else {
          e.resize(dim_s);
          vpColVector sd = s_star.get_s();
          e = s - sd;
        }
      
        return e;
      }
      

  • (二) 计算相互作用矩阵 Ls：task.computeInteractionMatrix()
    Jacobian矩阵Ls一般不能直接获取，因为表达式中需要深度 Z 值。一般 Ls 的获取方式有以下几种：
    
    vpServo类中对应以下几种计算Lx的类型：current, mean, desired, user，如：

      // Type of the interaction matrix (current, mean, desired, user)
      vpServoIteractionMatrixType interactionMatrixType;
    

    本例中的interactionMatrixType类型是CURRENT。
    计算 Ls 的伪代码逻辑如下：
    • 计算结果保存在vpServo::L中。
    • 理论公式如（式5）。
    • 对于视觉伺服中的 eye in hand 和 eye to hand 问题都有：
      cVa = cVe;
      aJe = eJe;
    • computeInteractionMatrix();
    • 对于用CURRENT方法计算Ls：

        for (it = featureList.begin() ... ) {
          matrixTmp = (*it)->interaction(*it_select);
          /* Copy the temporarily matrix into L. */
          for (unsigned int k = 0; k < rowMatrixTmp; ++k, ++cursorL) {
            for (unsigned int j = 0; j < colMatrixTmp; ++j) {
              L[cursorL][j] = matrixTmp[k][j];
            }
          }
        }
      

      vpMatrix vpFeaturePoint::interaction(unsigned int select)
      {
        vpMatrix L;
        double x_ = get_x();
        double y_ = get_y();
        double Z_ = get_Z();
        if (vpFeaturePoint::selectX() & select) {
          vpMatrix Lx(1, 6);
          Lx = 0;
          Lx[0][0] = -1 / Z_;
          Lx[0][1] = 0;
          Lx[0][2] = x_ / Z_;
          Lx[0][3] = x_ * y_;
          Lx[0][4] = -(1 + x_ * x_);
          Lx[0][5] = y_;
          L = vpMatrix::stack(L, Lx);
        }
        if (vpFeaturePoint::selectY() & select) {
          vpMatrix Ly(1, 6);
          Ly = 0;
          Ly[0][0] = 0;
          Ly[0][1] = -1 / Z_;
          Ly[0][2] = y_ / Z_;
          Ly[0][3] = 1 + y_ * y_;
          Ly[0][4] = -x_ * y_;
          Ly[0][5] = -x_;
          L = vpMatrix::stack(L, Ly);
        }
        return L;
      }
      

      由以上伪代码可知，matrixTmp的表达式即：
      $$L_s(i)={\left[\begin{array}{cccccc}-1/Z_i& 0& x_i/Z_i& x_i{y}_i& -(1+x_i^2)& y_i\\ 0& -1/Z_i& y_i/Z_i& 1+y_i^2& -x_i{y}_i& -x_i\end{array}\right]}_{2\ast 6}$$
      vpServo::L的最终形式即：
      $$L_s={\left[\begin{array}{c}{L}_s(1)\\ ⋮\\ L_s(n)\end{array}\right]}_{2n\ast 6}$$
  • (二) 计算执行器终端固连坐标系下的雅可比矩阵 J1
    原因：因为控制量是要作用在执行器终端固连坐标系中的，例如无人机视觉伺服则要把控制量作用在飞机机体坐标系，然而通过控制 Vc= - lambda * Ls * e 计算得到的控制量是相机坐标系下的控制量，因此要把控制量转换到执行器终端固连坐标系中，所以要计算J1.
    赋值伪代码：
    • 计算结果保存在vpServo::J1中。
    • 表达式：J1 = Ls * cVa * aJe
      cVa、aJe 矩阵的含义请查看下一小节。
    • 代码：

        // compute  task Jacobian
        if (iscJcIdentity)
          J1 = L * cVa * aJe;
        else
          J1 = L * cJc * cVa * aJe;
      

  • (三) 计算执行器终端固连坐标系的最终控制量 vpColVector v

    e1 = J1p * error; // primary task
    e = -lambda(e1) * e1;
    return e;
    

• 解析 vpServo 类

  • 什么是视觉特征？
    在IBVS中以图像系下某些点的坐标来定义视觉特征向量。
  • 视觉特征 向量
    vpColVector s;
    2D 矩阵，尺寸：2n x 1 ，n是特征点个数
    computeError()或computeControlLaw()执行后将s更新。
  • 视觉特征期望值 向量
    vpColVector sStar;
    2D 矩阵，尺寸：2n x 1 ，n是特征点个数
    computeError()或computeControlLaw()执行后将sStar更新。
  • 视觉特征误差 向量
    vpColVector error;
    2D 矩阵，尺寸：2n x 1 ，n是特征点个数
    computeError()或computeControlLaw()执行后将error更新。
  • 视觉特征列表
    std::list<vpBasicFeature *> featureList;
    所有视觉特征变量的地址列表，例如定义了4个点作为视觉特征则list就会有4个元素，一个元素就是一个视觉特征变量的地址。
    注意与s的区别。
  • 视觉特征期望值列表
    std::list<vpBasicFeature *> desiredFeatureList;
    与featureList的含义同理。
    元素数量与featureList同。
    注意与sStar的区别。
  • 视觉特征使能列表
    std::list featureSelectionList;
    例如定义了4个视觉特征只使用了其中3个，甚至可以使能每个视觉特征的某个维度，例如表征视觉特征的点坐标[x，y，z] 只让xy起作用参与有关视觉特征的运算。
    例如0xFFFF则启用，否则禁用。
    元素数量与featureList同。
  • 单位矩阵
    vpMatrix I;
  • 投影算子
    vpMatrix WpW;
  • 投影算子： I - WpW
    vpMatrix I_WpW;
  • 相互作用矩阵矩阵 Ls，也叫雅可比矩阵(Jacobian)
    vpMatrix L;
    表达式如（式5），尺寸：2n x 6
    注意与 vpServo::J1 的区别！！
  • Task Jacobian
    vpMatrix J1;
    具体情形下的雅可比矩阵坐标变换。
    注意与Jacobian矩阵( 即 L )的区别！
    表达式：
    $$J_1=L({}^c{V}_a)({}^a{J}_e)$$
    cVa，aJe的表达式往下看便知。在本例中cVa与cVe同，aJe与eJe同。
  • cJc 矩阵
    vpMatrix cJc;
    在相机系中vc、wc的哪些自由度是可控的。
    对角矩阵，若vc、wc所有维度都可控则cJc是单位阵。
  • cVe 矩阵
    vpVelocityTwistMatrix cVe;
    相机坐标系 与 执行器末端固连坐标系之间的转换关系矩阵。
  • cVf 矩阵
    vpVelocityTwistMatrix cVf;
    相机坐标系 与 robot base frame 之间的转换关系矩阵。
  • fVe 矩阵
    robot base frame 与 执行器末端固连坐标系之间的转换关系矩阵。
  • eJe 矩阵
    vpMatrix eJe;
    Jacobian矩阵在执行器末端固连坐标系中的表达式。
  • fJe 矩阵
    vpMatrix fJe;
    Jacobian矩阵在robot base frame 中的表达式。
  • 最终控制量
    vpColVector e;
    表达式：
    $$e=e_1+(I-{J_1}^{+}{J}_1)e_2$$
  • e1
    vpColVector e1;
    表达式：
    $$e_1={J_1}^{+}(s-s\ast )$$
  • 执行器终端坐标系、robot base frame 说明
    

• 结果分析
  
  左图：4个特征点在图像坐标系中的位置轨迹。
  右图：世界坐标系下，相机光心的移动轨迹。

1.2 基于图像处理的视觉伺服

在仿真例程tutorial-ibvs-4pts.cpp中，cMo是通过相机的世界坐标系坐标直接计算得到的，但在真实应用场景中，cMo应该是通过目标检测算法得到的。以Blob检测算法为例，使用Blob检测来得到特征点的cMo矩阵然后应用在视觉伺服控制系统中。
例程：tutorial-ibvs-4pts-image-tracking.cpp
参考：IBVS simulation with image processing
结果：

例程：tutorial-ibvs-4pts-image-tracking.cpp.
代码分析：