【机器人3】图像雅可比矩阵原理与推导
图像雅可比矩阵原理与推导理想情况下，图像像素坐标系和图像物理坐标系无倾斜，则二者坐标转换关系如下，且两边求导：[uv1]=[1dx0u001dyv0001][xy1](1)egin{bmatrix}u\v\1end{bmatrix}=egin{bmatrix}frac{1}{d_x}&0&u_0\0&frac{1}{d_y}&v_0\0&0&1end{bmatrix}egin{bmatrix}x\y\1end{bmatrix} ag{1}uv1=dx1000dy10u0v01xy1(1){u˙=1dxx˙v˙=1dyy˙(2)egin{cases}dot{u}=frac{1}{d_x}dot{x}\dot{v}=frac{1}{d_y}dot{y}end{cases} ag{2}{u˙=dx1x˙v˙=dy1y˙(2)由小孔成像原理，空间一点的相机坐标和图像物理坐标转换关系如下，且两边求导：[xy1]=[fZc000fZc0001Zc][XcYcZc](3)egin{bmatrix}x\y\1end{bmatrix}=egin{bmatrix}frac{f}{Z_c}&0&0\0&frac{f}{Z_c}&0\0&0&frac{1}{Z_c}end{bmatrix}egin{bmatrix}X_c\Y_c\Z_cend{bmatrix} ag{3}xy1=Zcf000Zcf000Zc1XcYcZc(3){x˙=f(X˙cZc−XcZ˙cZc2)=fX˙cZc−xZ˙cZcy˙=f(Y˙cZc−YcZ˙cZc2)=fY˙cZc−yZ˙cZc(4)egin{cases}dot{x}=f(frac{dot{X}_c}{Z_c}-frac{X_cdot{Z}_c}{Z_c^2})=frac{fdot{X}_c}{Z_c}-frac{xdot{Z}_c}{Z_c}\dot{y}=f(frac{dot{Y}_c}{Z_c}-frac{Y_cdot{Z}_c}{Z_c^2})=frac{fdot{Y}_c}{Z_c}-frac{ydot{Z}_c}{Z_c}end{cases} ag{4}{x˙=f(ZcX˙c−Zc2XcZ˙c)=ZcfX˙c−ZcxZ˙cy˙=f(ZcY˙c−Zc2YcZ˙c)=ZcfY˙c−ZcyZ˙c(4)固定相机，移动空间点时，速度关系为：p˙c=cvp+cωp×pc(5)dot{oldsymbol{p}}_c=^coldsymbol{v}_p+^coldsymbol{omega}_p imesoldsymbol{p}_c ag{5}p˙c=cvp+cωp×pc(5)固定空间点，移动相机时，速度关系为：p˙c=−cvc−cωc×pc(6)dot{oldsymbol{p}}_c=-^coldsymbol{v}_c-^coldsymbol{omega}_c imesoldsymbol{p}_c ag{6}p˙c=−cvc−cωc×pc(6){X˙c=−cνc,x−cωc,yZc+cωc,zYcY˙c=−cνc,y−cωc,zXc+cωc,xZcZ˙c=−cνc,z−cωc,xYc+cωc,yXc(7)egin{cases}dot{X}_c=-{}^c u_{c,x}-{}^comega_{c,y}Z_c+{}^comega_{c,z}Y_c\dot{Y}_c=-{}^c u_{c,y}-{}^comega_{c,z}X_c+{}^comega_{c,x}Z_c\dot{Z}_c=-{}^c u_{c,z}-{}^comega_{c,x}Y_c+{}^comega_{c,y}X_cend{cases} ag{7}⎩⎨⎧X˙c=−cνc,x−cωc,yZc+cωc,zYcY˙c=−cνc,y−cωc,zXc+cωc,xZcZ˙c=−cνc,z−cωc,xYc+cωc,yXc(7)将(7)代入(4)，得：{x˙=−fZccvc,x+xZccvc,z+xyfcωc,x−f2+x2fcωc,y+ycωc,zy˙=−fZccvc,y+yZccvc,z+f2+y2fcωc,x−xyfcωc,y−xcωc,z(8)left{egin{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} ight. ag{8}{x˙=−Zcfcvc,x+Zcxcvc,z+fxycωc,x−ff2+x2cωc,y+ycωc,zy˙=−Zcfcvc,y+Zcycvc,z+ff2+y2cωc,x−fxycωc,y−xcωc,z(8)即：[x˙y˙]=[−fZc0xZcxyf−f2+x2fy0−fZcyZcf2+y2f−xyf−x][cvc,xcvc,ycvc,zcωc,xcωc,ycωc,z](9)egin{bmatrix}dot{x}\dot{y}end{bmatrix}=egin{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}&-xend{bmatrix}left[egin{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} ight] ag{9}[x˙y˙]=[−Zcf00−ZcfZcxZcyfxyff2+y2−ff2+x2−fxyy−x]cvc,xcvc,ycvc,zcωc,xcωc,ycωc,z(9)将(9)以及x=dx(u−u0)x=d_{x}left(u-u_{0} ight)x=dx(u−u0)和y=dy(v−v0)y=d_y(v-v_0)y=dy(v−v0)代入(2)：[u˙v˙]=[−fdxZc0(u−u0)Zc(u−u0)dy(v−v0)f−f2+dx2(u−u0)2dxfdy(v−v0)dx0−fdyZc(v−v0)Zcf2+dy2(v−v0)2dyf−dx(u−u0)(v−v0)f−dx(u−u0)dy][cvc,xcvc,ycvc,zcωc,xcωc,ycωc,z](10)left[egin{array}{c}dot{u}\dot{v}end{array} ight]=left[egin{array}{cccccc}-frac{f}{d_{x}Z_{c}}&0&frac{left(u-u_{0} ight)}{Z_{c}}&frac{left(u-u_{0} ight)d_{y}left(v-v_{0} ight)}{f}&-frac{f^{2}+d_{x}^{2}left(u-u_{0} ight)^{2}}{d_{x}f}&frac{d_{y}left(v-v_{0} ight)}{d_{x}}\0&-frac{f}{d_{y}Z_{c}}&frac{left(v-v_{0} ight)}{Z_{c}}&frac{f^{2}+d_{y}^{2}left(v-v_{0} ight)^{2}}{d_{y}f}&-frac{d_{x}left(u-u_{0} ight)left(v-v_{0} ight)}{f}&-frac{d_{x}left(u-u_{0} ight)}{d_{y}}end{array} ight]left[egin{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} ight] ag{10}[u˙v˙]=−dxZcf00−dyZcfZc(u−u0)Zc(v−v0)f(u−u0)dy(v−v0)dyff2+dy2(v−v0)2−dxff2+dx2(u−u0)2−fdx(u−u0)(v−v0)dxdy(v−v0)−dydx(u−u0)cvc,xcvc,ycvc,zcωc,xcωc,ycωc,z(10)即：[u˙v˙]=Jimg[cvccuc](11)egin{bmatrix}dot{u}\dot{v}end{bmatrix}=J_{img}egin{bmatrix}^coldsymbol{v}_{c}\^coldsymbol{u}_{c}end{bmatrix} ag{11}[u˙v˙]=Jimg[cvccuc](11)可得图像雅可比矩阵：Jimg=[−fdxZc0(u−u0)Zc(u−u0)dy(v−v0)f−f2+dx2(u−u0)2dxfdy(v−v0)dx0−fdyZc(v−v0)Zcf2+dy2(v−v0)2dyf−dx(u−u0)(v−v0)f−dx(u−u0)dy](12)J_{img}=left[egin{array}{cccccc}-frac{f}{d_{x}Z_{c}}&0&frac{left(u-u_{0} ight)}{Z_{c}}&frac{left(u-u_{0} ight)d_{y}left(v-v_{0} ight)}{f}&-frac{f^{2}+d_{x}^{2}left(u-u_{0} ight)^{2}}{d_{x}f}&frac{d_{y}left(v-v_{0} ight)}{d_{x}}\0&-frac{f}{d_{y}Z_{c}}&frac{left(v-v_{0} ight)}{Z_{c}}&frac{f^{2}+d_{y}^{2}left(v-v_{0} ight)^{2}}{d_{y}f}&-frac{d_{x}left(u-u_{0} ight)left(v-v_{0} ight)}{f}&-frac{d_{x}left(u-u_{0} ight)}{d_{y}}end{array} ight] ag{12}Jimg=−dxZcf00−dyZcfZc(u−u0)Zc(v−v0)f(u−u0)dy(v−v0)dyff2+dy2(v−v0)2−dxff2+dx2(u−u0)2−fdx(u−u0)(v−v0)dxdy(v−v0)−dydx(u−u0)(12)如有不足之处欢迎指出~