基于图像雅可比的目标跟踪控制
基于图像雅可比的目标跟踪控制
本文从李群李代数的方式对二轴云台的目标跟踪控制(视觉伺服)问题进行了推导。基于图像的视觉伺服(IBVS)问题多使用基于解析几何的方法进行推到，但是似乎很少有人使用李群李代数进行推到。本文使用李代数完成对基于图像雅可比的视觉伺服问题推导，相比于解析几何的方法，我认为在面对更多更复杂关节的情况下，李代数的方法更具有通用性。下面将介绍系统模型，并基于此模型开始推导。
系统建模
系统模型如上图所示，该系统由两个相机和一个二关节云台构成，其中C1相机为广角相机，C2相机为长焦相机。该系统将使用C1相机检测目标，并控制C2相机跟踪目标。该系统的目标是使C2相机的视野中始终包含目标，即在C1相机能看到目标的情况下使目标在C2相机视野中移动。{C1},{C2}坐标系为前文所述两相机的坐标系; {G1},{G2}为电机法兰坐标系; {M1}{M2}为电机基座坐标系，{M}系与{G}系的z轴同方向共线。
目标
目标是使C2相机的视野中始终包含目标，即在C1相机能看到目标的情况下使目标在C2相机视野中移动。为了实现上述目标，我们需要获得长焦相机中目标像素坐标与相机光心坐标的误差与云台电机旋转之间的关系，即：
$$
\vec{e}=[\begin{matrix} u \ v \ \end{matrix}] - [\begin{matrix} u_0 \ v_0 \ \end{matrix}]= {}^{2}T_{1} P_1+\frac{\partial e}{\partial \phi_1}\delta\phi_1+\frac{\partial e}{\partial \phi_2}\delta\phi_2
$$
在获得雅可比矩阵后即可根据误差雅可比进行视觉伺服控制。
公式推导
预备定理与公式
SE(3)群的伴随性质
$$
\boldsymbol{T} Exp(\xi) \boldsymbol{T}^{-1}=Exp(Ad(\boldsymbol{T})\xi)
$$
稍加变形后可得：
$$
Exp(\xi)\boldsymbol{T}=\boldsymbol{T}Exp(Ad(T^{-1})\xi)
$$
其中：
$$
Exp(\xi)=exp(\xi^\wedge)
$$
$$
Ad(\boldsymbol{T})=[\begin{matrix} R & t^\wedge R \ 0 & R \ \end{matrix}]
$$
SE(3)群的BCH近似公式
$$
Exp(\xi+\delta\xi)\approx Exp(\xi)Exp(J_r(\xi)\delta\xi)
$$
$$
Exp(\xi)Exp(\delta\xi)=Exp(\xi+J_r^{-1}(\xi)\delta\xi)
$$
其中$J_r，J_r^{-1}$为其右乘雅可比以及它的逆，由于我们只用到了右乘，这里仅展示右乘，左乘有类似形式。当$\delta\xi$为小量时，雅可比矩阵约等于单位阵，即：
$$
J_r(\delta\xi)=J_r^{-1}(\delta\xi)=I_(6×6)
$$
SO(3)群中的向量叉积有以下性质
$$
a^\wedge b=-b^\wedge a
$$
se(3)李代数的升运算
$$
\xi^\wedge=\left[ \begin{matrix} \rho \ \phi \ \end{matrix} \right]^\wedge = \left[ \begin{matrix} \phi^\wedge & J\rho \ \boldsymbol{0} & \boldsymbol{0} \ \end{matrix} \right]
$$
推导
$$
e=[u^2,v^2]^T-[u_0^2,v_0^2]^T=K_2 {}^{C2}T_{G2} rot(\omega_2) {}^{M2}T_{G1} rot(\omega_1) {}^{M1}T_{C1}P_1-[u_0^2,v_0^2]^T (1)
$$
我们对(1)式加入右扰动
$$
\hat{e}=[u^2,v^2]^T-[u_0^2,v_0^2]^T=K_2 {}^{C2}T_{G2} rot(\omega_2) Exp(\delta\xi_2) {}^{M2}T_{G1} rot(\omega_1) Exp(\delta\xi_1){}^{M1}T_{C1}P_1-[u_0^2,v_0^2]^T (2)
$$
利用伴随性质将扰动项移至末尾
$$
\hat{e}=[u^2,v^2]^T-[u_0^2,v_0^2]^T=K_2 {}^{C2}T_{G2} rot(\omega_2) Exp(\delta\xi_2) {}^{M2}T_{G1} rot(\omega_1) {}^{M1}T_{C1} Exp(Ad({}^{M1}T_{C1}^{-1})\delta\xi_1) P_1-[u_0^2,v_0^2]^T (3)
$$
$$
\hat{e}=[u^2,v^2]^T-[u_0^2,v_0^2]^T=K_2 {}^{C2}T_{G2} rot(\omega_2) {}^{M2}T_{G1} rot(\omega_1) {}^{M1}T_{C1} Exp(Ad(({}^{M2}T_{G1} rot(\omega_1) {}^{M1}T_{C1})^{-1})\delta\xi_2) Exp(Ad({}^{M1}T_{C1}^{-1})\delta\xi_1) P_1-[u_0^2,v_0^2]^T
$$
令
$$
{}^{C2}T_{C1}=k_2 {}^{C2}T_{G2} rot(\omega_2) {}^{M2}T_{G1} rot(\omega_1) {}^{M1}T_{C1}
$$
$$
{}^{C1}T_{M2}=({}^{M2}T_{G1} rot(\omega_1) {}^{M1}T_{C1})^{-1}
$$
可得
$$
\hat{e}=K_2{}^{C2}T_{C1} Exp(Ad({}^{C1}T_{M2})\delta\xi_2) Exp(Ad({}^{M1}T_{C1}^{-1})\delta\xi_1) P_1-[u_0^2,v_0^2]^T
$$
利用BCH近似可得
$$
\hat{e}=K_2{}^{C2}T_{C1} Exp(Ad({}^{C1}T_{M2})\delta\xi_2+J_r^{-1}(Ad({}^{C1}T_{M2})\delta\xi_2) Ad({}^{M1}T_{C1}^{-1})\delta\xi_1) P_1-[u_0^2,v_0^2]^T
$$
由于$Ad( ^{C1}T_{M2} )\delta\xi_2$是小量，其右雅可比约等于单位阵，可得：
$$
\hat{e}\approx K_2{}^{C2}T_{C1} Exp(Ad({}^{C1}T_{M2})\delta\xi_2+Ad({}^{C1}T_{M1})\delta\xi_1) P_1-[u_0^2,v_0^2]^T
$$
$$
\hat{e}\approx K_2{}^{C2}T_{C1} (I + (Ad({}^{C1}T_{M2})\delta\xi_2)^\wedge+(Ad({}^{C1}T_{M1})\delta\xi_1)^\wedge) P_1-[u_0^2,v_0^2]^T
$$
$$
\hat{e}\approx K_2{}^{C2}T_{C1} P_1-[u_0^2,v_0^2]^T +K_2((Ad({}^{C1}T_{M2})\delta\xi_2)^\wedge+(Ad({}^{C1}T_{M1})\delta\xi_1)^\wedge)P_1
$$
将旋转平移矩阵进行拆分，由于转轴的平移分量为0，因此 $\rho=0$.
$$
\hat{e}\approx K_2{}^{C2}T_{C1}P_1-[u_0^2,v_0^2]^T+K_2 [\begin{matrix} ({}^{C1}R_{M2}\delta\phi_2)^\wedge & 0 \ 0 & 0\\end{matrix}][\begin{matrix}P_1 \ 1\ \end{matrix}]+K_2[\begin{matrix} ({}^{C1}R_{M1}\delta\phi_1)^\wedge & 0 \ 0 & 0\ \end{matrix}][\begin{matrix}P_1 \ 1\ \end{matrix}]
$$
$$
\hat{e}\approx K_2{}^{C2}T_{C1}P_1-[u_0^2,v_0^2]^T+K_2 [\begin{matrix} ({}^{C1}R_{M2}\delta\phi_2)^\wedge P_1 \ 0\\end{matrix}]+K_2[\begin{matrix} ({}^{C1}R_{M1}\delta\phi_1)^\wedge P_1 \ 0\ \end{matrix}]
$$
利用SO(3)群中的向量叉积的性质可得
$$
\hat{e}\approx K_2{}^{C2}T_{C1}P_1-[u_0^2,v_0^2]^T+K_2 [\begin{matrix} -P_1^\wedge ({}^{C1}R_{M2}\delta\phi_2) \ 0\ \end{matrix}]+K_2[\begin{matrix} - P_1^\wedge({}^{C1}R_{M1}\delta\phi_1) \ 0\ \end{matrix}]
$$
$$
\hat{e}\approx K_2{}^{C2}T_{C1}P_1-[u_0^2,v_0^2]^T+K_2 [\begin{matrix} -P_1^\wedge {}^{C1}R_{M2} & 0 \ 0 & 0\ \end{matrix}][\begin{matrix} \delta\phi_2 \ 0 \ \end{matrix}]+K_2[\begin{matrix} - P_1^\wedge{}^{C1}R_{M1} & 0 \ 0& 0\ \end{matrix}][\begin{matrix} \delta\phi_1 \ 0 \ \end{matrix}]
$$
$$
\hat{e}=e+\frac{\partial e}{\partial \phi_2}\delta\phi_2+\frac{\partial e}{\partial \phi_1}\delta\phi_1
$$
最后我们可以得到误差关于电机旋转的偏导数，由于电机只能饶z轴旋转，因此需要将x, y分量置零
$$
\frac{\partial e}{\partial \phi_2}=-K_2 diag(0,0,1) P_1^\wedge {}^{C1}R_{M2}
$$
$$
\frac{\partial e}{\partial \phi_1}=-K_2 diag(0,0,1) P_1^\wedge {}^{C1}R_{M1}
$$
$$
\hat{e}=e+ [\begin{matrix} \frac{\partial e}{\partial \phi_1}& \frac{\partial e}{\partial \phi_2}\\end{matrix}][\begin{matrix} \delta\phi_1 \ \delta\phi_2 \ \end{matrix}]
$$
$$
\delta e=\hat{e}-e=J\phi
$$
最后对雅可比矩阵求伪逆即可得到控制增量$\delta\phi$
$$
\phi=(J^TJ)^{-1}(J^T(\delta e))
$$
电机新的目标位置即为$\phi+\delta\phi$.
EOF
wechat
