CS336视觉伺服

笔记

动力学模型

机械臂动力学的研究方法：拉格朗日，牛顿-欧拉，高斯，凯恩方法。机械臂的动力学主要是两个问题：正向运动学和逆向运动学。

视觉伺服

视觉伺服的基本思想

基于视觉的伺服控制方法的目的是最小化一个图像误差，该误差可以定义为：
e(t)=s(m(t), a)-s^{*}

其中m(t)是一系列的图像特征，如图像点特征、形状特征、位姿特征或频域特征等。s(m(t),a)表示通过特征m(t)与一些额外的系统参数构成的映射函数。

将提取的特征m(t)映射至目标特征s^{*}的空间中，并与s^{*}进行比较，当两者差为零时，即系统误差e(t)为零，系统迗到稳定的收敛位置。

基于图像的视觉伺服方法通过从图像中获取的特征点直接控制机械臂的运动 ，而基于位置的伺服方法首先通过图像特征点计算目标的位姿，再根据机械臂与目标物体的相对位姿规划运动方案。

最筒单的控制方法是设计一个速度控制器来控制机械臂的移动。为了得到图像特征s随时间的变化率与摄像机运动速度的关系，设摄像机在摄像机坐标系下的瞬时速度为\mathbf{v}_{c}=\left(v_{c}, \omega_{c}\right)​

则图像特征随时间的变化率\dot{s}与摄像机速度\mathbf{v}_{c} 的关系可以表示为：
\dot{s}=\mathbf{J}_{s} \mathbf{v}_{c}

其中\mathbf{J}_{s} \in \mathbb{R}^{k \times 6}为图像雅克比矩阵。在未知相机参数和特征点深度信息条件下，定义图像雅克比矩阵为：

J(k)=\left[\begin{array}{ccc} \frac{\partial S_{1}(k)}{\partial r_{1}(k)} & \cdots & \frac{\partial S_{1}(k)}{\partial r_{n_{s}}(k)} \\ \vdots & \ddots & \vdots \\ \frac{\partial S_{m}(k)}{\partial r_{1}(k)} & \cdots & \frac{\partial S_{m}(k)}{\partial r_{n_{s}}(k)} \end{array}\right]_{m \times n_{s}}=\left[\begin{array}{ccc} j_{11} & \cdots & j_{1 n_{s}} \\ \vdots & \ddots & \vdots \\ j_{m 1} & \cdots & j_{m n_{s}} \end{array}\right]_{m \times n_{s}}

将式(2)代入(1)可得：
\dot{e}=\mathbf{J}_{e} \mathbf{v}_{c}

其中\mathbf{J}_{e}=\mathbf{J}_{s} ,考虑将\mathbf{v}_{c} 作为机械臂速度控制的输入，并保证图像误差的指数型下降，则可以通过对公式(3)中的雅克比矩阵求逆得到：
\mathbf{v}_{c}=-\lambda \mathbf{J}_{e}^{+} \mathbf{e}

在实际的视觉伺服系统中，在控制阶段的每一时刻获得精确的\mathbf{J}_{e}或者 \mathbf{J}_{e}^{+} 是十分困难的。因此需要对图像雅克比矩阵进行估计。因此实际的视觉伺服控制方程应改写为：

\mathbf{v}_{c}=-\lambda \hat{\mathbf{J}}_{e}^{+} \mathbf{e}

视觉伺服的基本架构

基于图像的视觉伺服

传统的基于图像的视觉伺服方式使用一系列特征点作为图像控制特征来定义s。对图像的测量m(t)通常是对应特征点对在图像上的像素距离，而系统参数a为摄像机的内参数。基于这祥的设定，可定义在摄像机坐标系下的空间点\mathbf{X}=\left \{ X ,Y,Z\right \}，它在摄像机二维成像平面上所映射的点为\mathbf{x}=\left \{ x,y\right \}。根据之前坐标概论中图像坐标与相机坐标的关系：

\left\{\begin{array}{c} u= (f \cdot X) / Z \\ v =(f \cdot Y) / Z \end{array}\right.

即

\left\{\begin{array}{c} x= (uZ) / f \\ y =(vZ) / f \end{array}\right. \tag{1}

求导

\left\{\begin{array}{l} \dot{u}=f \cdot \frac{z \dot{x}-x \dot{z}}{z^{2}} \\ \dot{v}=f \cdot \frac{z \dot{y}-y \dot{z}}{z^{2}} \end{array}\right. \tag{2}

根据速度的定义

\left\{\begin{array}{l} \dot{x}=z w_{y}-y w_{z}+v_{x} \\ \dot{y}=x w_{z}-z w_{x}+v_{y} \\ \dot{z}=y w_{x}-x w_{y}+v_{z} \end{array}\right. \tag{3}

将公式1代入公式3中可得

\left\{\begin{array}{l} \dot{x}=z w_{y}-\frac{v z}{f} w_{z}+v_{x} \\ \dot{y}=\frac{u z}{f} w_{z}-z w_{x}+v_{y} \\ \dot{z}=\frac{z}{f}\left(v w_{x}-u w_{y}\right)+v_{z} \end{array}\right. \tag{4}

将公式2代入公式4中可得
\left\{\begin{array}{l} \dot{u}=\frac{f}{z} v_{x}-\frac{u}{z} v_{z}-\frac{u v}{f} w_{x}+\frac{f^{2}+u^{2}}{f} w_{y}-v w_{z} \\ \dot{v}=\frac{f}{z} v_{y}-\frac{v}{z} v_{z}+\frac{u v}{f} w_{y}-\frac{f^{2}+u^{2}}{f} w_{x}+u w_{z} \end{array}\right.

用矩阵表示
\left[\begin{array}{c} \dot{u} \\ \dot{v} \end{array}\right]=\left[\begin{array}{cccccc} \frac{f}{z} & 0 & -\frac{u}{z} & -\frac{u v}{f} & \frac{f^{2}+u^{2}}{f} & -v \\ 0 & \frac{f}{z} & -\frac{v}{z} & -\frac{f^{2}+u^{2}}{f} & \frac{u v}{f} & u \end{array}\right]\left[\begin{array}{l} v_{x} \\ v_{y} \\ v_{z} \\ w_{x} \\ w_{y} \\ w_{z} \end{array}\right]

上式反映了某点在图像坐标系下与相机坐标系下运动之间的关系 。

图像雅克比矩阵的预估方法

对于图像雅克比矩阵的估计问题，传统方法采用Broyden估计器 进行估计，近年来还出现了KF算法 、PF算法 、神经网络 等方法。

基于位置的视觉伺服

基于位置的视觉伺服，其反馈信号在三维任务空间中以直角坐标形式定义。基本原理是通过对图像特征的提取，并结合已知的目标几何模型及摄像机模型，三维笛卡尔坐标系中对目标位姿进行估计，然后由机械手当前位姿与目标位姿（这里的目标位姿指的不是目标物体的位姿，而是下一步机械臂末端的期望位姿）之差，进行轨迹规划并计算出控制量，驱动机械手向目标运动，最终实现定位、抓取功能。

\mathbf{L}_{e}=\left[\begin{array}{cc} -\mathbf{I}_{3} & {\left[{ }^{c} \mathbf{t}_{o}\right]_{\mathrm{x}}} \\ 0 & \mathbf{L}_{\theta \mathbf{a}} \end{array}\right]

\mathbf{L}_{\theta_{u}}=I_{3}-\frac{\theta}{2}[\mathbf{u}]_{x}+\left(1-\frac{\sin c \theta}{\sin c^{2} \frac{\theta}{2}}\right)[\mathbf{u}]_{\times}^{2}

\hat{\mathbf{L}}_{e}^{-1}=\left[\begin{array}{cc} -\mathbf{I}_{3} & {\left[{ }^{c} \mathbf{t}_{o}\right]_{\times} \mathbf{L}_{\theta \mathbf{u}}^{-1}} \\ 0 & \mathbf{L}_{\theta \mathbf{u}}^{-1} \end{array}\right]

\boldsymbol{v}_{c}=-\lambda \hat{L}_{e}^{-1} \boldsymbol{e}

\left\{\begin{array}{c} v_{c}=-\lambda\left(\left(\dot{c} \mathbf{t}_{o}-{ }^{c} \mathbf{t}_{o}\right)+\left[{ }^{c} \mathbf{t}_{o}\right]_{\mathrm{x}} \theta \mathbf{u}\right) \\ \omega_{c}=-\lambda \theta \mathbf{u} \end{array}\right.

图的搜索算法

正向搜索和反向搜索，常见的正向搜索算法是A*，反向搜索算法RTT。

如何选择正向搜索和反向搜索算法？

现在我们这样来定义这个问题，f(v,d)表示一组节点的集合，其它节点距离节点v的距离都小于距离d。现在假设起始节点为s，目标节点为t。
那么，当f(s,d) < f(t,d) forward search更好，也就是起始节点附近满足距离d的节点数小于目标节点附近满足距离d的节点数；
当f(s,d) > f(t,d) backward search更好。