【机器人3】图像雅可比矩阵原理与推导

理想情况下，图像像素坐标系和图像物理坐标系无倾斜，则二者坐标转换关系如下，且两边求导：

[ u v 1 ] = [ 1 d x 0 u 0 0 1 d y v 0 0 0 1 ] [ x y 1 ] (1) \begin{bmatrix}u\\v\\1\end{bmatrix}=\begin{bmatrix}\frac{1}{d_x}&0&u_0\\0&\frac{1}{d_y}&v_0\\0&0&1\end{bmatrix}\begin{bmatrix}x\\y\\1\end{bmatrix} \tag{1} ​uv1​ ​= ​dx​1​00​0dy​1​0​u0​v0​1​ ​ ​xy1​ ​(1)$$\{\begin{array}{l}\dot{u}=\frac{1}{d_x}\dot{x}\\ \dot{v}=\frac{1}{d_y}\dot{y}\end{array}\text{\hspace{1em}(2)}$$由小孔成像原理，空间一点的相机坐标和图像物理坐标转换关系如下，且两边求导： [ x y 1 ] = [ f Z c 0 0 0 f Z c 0 0 0 1 Z c ] [ X c Y c Z c ] (3) \begin{bmatrix}x\\ y\\ 1\end{bmatrix}=\begin{bmatrix}\frac{f}{Z_c}&0&0\\ 0&\frac{f}{Z_c}&0\\ 0&0&\frac{1}{Z_c}\end{bmatrix}\begin{bmatrix}X_c\\ Y_c\\ Z_c\end{bmatrix} \tag{3} ​xy1​ ​= ​Zc​f​00​0Zc​f​0​00Zc​1​​ ​ ​Xc​Yc​Zc​​ ​(3)$$\{\begin{array}{l}\dot{x}=f(\frac{{\dot{X}}_c}{Z_c}-\frac{X_c{\dot{Z}}_c}{Z_c^2})=\frac{f{\dot{X}}_c}{Z_c}-\frac{x{\dot{Z}}_c}{Z_c}\\ \dot{y}=f(\frac{{\dot{Y}}_c}{Z_c}-\frac{Y_c{\dot{Z}}_c}{Z_c^2})=\frac{f{\dot{Y}}_c}{Z_c}-\frac{y{\dot{Z}}_c}{Z_c}\end{array}\text{\hspace{1em}(4)}$$固定相机，移动空间点时，速度关系为：$${\dot{\mathbf{p}}}_c{=}^c{\mathbf{v}}_p{+}^c{\mathbf{\omega }}_p\times {\mathbf{p}}_c\text{\hspace{1em}(5)}$$固定空间点，移动相机时，速度关系为：$${\dot{\mathbf{p}}}_c={-}^c{\mathbf{v}}_c{-}^c{\mathbf{\omega }}_c\times {\mathbf{p}}_c\text{\hspace{1em}(6)}$$$$\{\begin{array}{l}{\dot{X}}_c=-{}^c{\nu }_{c,x}-{}^c{\omega }_{c,y}{Z}_c+{}^c{\omega }_{c,z}{Y}_c\\ {\dot{Y}}_c=-{}^c{\nu }_{c,y}-{}^c{\omega }_{c,z}{X}_c+{}^c{\omega }_{c,x}{Z}_c\\ {\dot{Z}}_c=-{}^c{\nu }_{c,z}-{}^c{\omega }_{c,x}{Y}_c+{}^c{\omega }_{c,y}{X}_c\end{array}\text{\hspace{1em}(7)}$$将(7)代入(4)，得：$$\{\begin{array}{l}\dot{x}=-\frac{f}{Z_c}{}^c{v}_{c,x}+\frac{x}{Z_c}{}^c{v}_{c,z}+\frac{xy}{f}{}^c{\omega }_{c,x}-\frac{f^2+x^2}{f}{}^c{\omega }_{c,y}+y^c{\omega }_{c,z}\\ \dot{y}=-\frac{f}{Z_c}{}^c{v}_{c,y}+\frac{y}{Z_c}{}^c{v}_{c,z}+\frac{f^2+y^2}{f}{}^c{\omega }_{c,x}-\frac{xy}{f}{}^c{\omega }_{c,y}-x^c{\omega }_{c,z}\end{array}\text{\hspace{1em}(8)}$$即： [ x ˙ y ˙ ] = [ − f Z c 0 x Z c x y f − f 2 + x 2 f y 0 − f Z c y Z c f 2 + y 2 f − x y f − x ] [ c v c , x c v c , y c v c , z c ω c , x c ω c , y c ω c , z ] (9) \begin{bmatrix}\dot{x}\\ \dot{y}\end{bmatrix}=\begin{bmatrix}-\frac{f}{Z_c}&0&\frac{x}{Z_c}&\frac{xy}{f}&-\frac{f^2+x^2}{f}&y\\ 0&-\frac{f}{Z_c}&\frac{y}{Z_c}&\frac{f^2+y^2}{f}&-\frac{xy}{f}&-x\end{bmatrix}\left[\begin{array}{l} { }^{c} v_{c, x} \\ { }^{c} v_{c, y} \\ { }^{c} v_{c, z} \\ { }^{c} \omega_{c, x} \\ { }^{c} \omega_{c, y} \\ { }^{c} \omega_{c, z} \end{array}\right]\tag{9} [x˙y˙​​]=[−Zc​f​0​0−Zc​f​​Zc​x​Zc​y​​fxy​ff2+y2​​−ff2+x2​−fxy​​y−x​] ​cvc,x​cvc,y​cvc,z​cωc,x​cωc,y​cωc,z​​ ​(9)将(9)以及$x=d_x\left(u-u_0\right)$和$y=d_y(v-v_0)$代入(2)： [ u ˙ v ˙ ] = [ − f d x Z c 0 ( u − u 0 ) Z c ( u − u 0 ) d y ( v − v 0 ) f − f 2 + d x 2 ( u − u 0 ) 2 d x f d y ( v − v 0 ) d x 0 − f d y Z c ( v − v 0 ) Z c f 2 + d y 2 ( v − v 0 ) 2 d y f − d x ( u − u 0 ) ( v − v 0 ) f − d x ( u − u 0 ) d y ] [ c v c , x c v c , y c v c , z c ω c , x c ω c , y c ω c , z ] (10) \left[\begin{array}{c} \dot{u} \\ \dot{v} \end{array}\right]=\left[\begin{array}{cccccc} -\frac{f}{d_{x} Z_{c}} & 0 & \frac{\left(u-u_{0}\right)}{Z_{c}} & \frac{\left(u-u_{0}\right) d_{y}\left(v-v_{0}\right)}{f} & -\frac{f^{2}+d_{x}^{2}\left(u-u_{0}\right)^{2}}{d_{x} f} & \frac{d_{y}\left(v-v_{0}\right)}{d_{x}} \\ 0 & -\frac{f}{d_{y} Z_{c}} & \frac{\left(v-v_{0}\right)}{Z_{c}} & \frac{f^{2}+d_{y}^{2}\left(v-v_{0}\right)^{2}}{d_{y} f} & -\frac{d_{x}\left(u-u_{0}\right)\left(v-v_{0}\right)}{f} & -\frac{d_{x}\left(u-u_{0}\right)}{d_{y}} \end{array}\right]\left[\begin{array}{l} { }^{c} v_{c, x} \\ { }^{c} v_{c, y} \\ { }^{c} v_{c, z} \\ { }^{c} \omega_{c, x} \\ { }^{c} \omega_{c, y} \\ { }^{c} \omega_{c, z} \end{array}\right] \tag{10} [u˙v˙​]= ​−dx​Zc​f​0​0−dy​Zc​f​​Zc​(u−u0​)​Zc​(v−v0​)​​f(u−u0​)dy​(v−v0​)​dy​ff2+dy2​(v−v0​)2​​−dx​ff2+dx2​(u−u0​)2​−fdx​(u−u0​)(v−v0​)​​dx​dy​(v−v0​)​−dy​dx​(u−u0​)​​ ​ ​cvc,x​cvc,y​cvc,z​cωc,x​cωc,y​cωc,z​​ ​(10)即：$$\left[\begin{array}{c}\dot{u}\\ \dot{v}\end{array}\right]=J_{img}\left[\begin{array}{c}{}^c{\mathbf{v}}_c\\ {}^c{\mathbf{u}}_c\end{array}\right]\text{\hspace{1em}(11)}$$可得图像雅可比矩阵： J i m g = [ − f d x Z c 0 ( u − u 0 ) Z c ( u − u 0 ) d y ( v − v 0 ) f − f 2 + d x 2 ( u − u 0 ) 2 d x f d y ( v − v 0 ) d x 0 − f d y Z c ( v − v 0 ) Z c f 2 + d y 2 ( v − v 0 ) 2 d y f − d x ( u − u 0 ) ( v − v 0 ) f − d x ( u − u 0 ) d y ] (12) J_{img}=\left[\begin{array}{cccccc} -\frac{f}{d_{x} Z_{c}} & 0 & \frac{\left(u-u_{0}\right)}{Z_{c}} & \frac{\left(u-u_{0}\right) d_{y}\left(v-v_{0}\right)}{f} & -\frac{f^{2}+d_{x}^{2}\left(u-u_{0}\right)^{2}}{d_{x} f} & \frac{d_{y}\left(v-v_{0}\right)}{d_{x}} \\ 0 & -\frac{f}{d_{y} Z_{c}} & \frac{\left(v-v_{0}\right)}{Z_{c}} & \frac{f^{2}+d_{y}^{2}\left(v-v_{0}\right)^{2}}{d_{y} f} & -\frac{d_{x}\left(u-u_{0}\right)\left(v-v_{0}\right)}{f} & -\frac{d_{x}\left(u-u_{0}\right)}{d_{y}} \end{array}\right]\tag{12} Jimg​= ​−dx​Zc​f​0​0−dy​Zc​f​​Zc​(u−u0​)​Zc​(v−v0​)​​f(u−u0​)dy​(v−v0​)​dy​ff2+dy2​(v−v0​)2​​−dx​ff2+dx2​(u−u0​)2​−fdx​(u−u0​)(v−v0​)​​dx​dy​(v−v0​)​−dy​dx​(u−u0​)​​ ​(12)如有不足之处欢迎指出~