您现在的位置是：首页 >技术交流 >【机器人3】图像雅可比矩阵原理与推导网站首页技术交流
【机器人3】图像雅可比矩阵原理与推导
图像雅可比矩阵原理与推导
理想情况下，图像像素坐标系和图像物理坐标系无倾斜，则二者坐标转换关系如下，且两边求导：
[
u
v
1
]
=
[
1
d
x
0
u
0
0
1
d
y
v
0
0
0
1
]
[
x
y
1
]
(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
=
dx1000dy10u0v01
xy1
(1)
{
u
˙
=
1
d
x
x
˙
v
˙
=
1
d
y
y
˙
(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)由小孔成像原理，空间一点的相机坐标和图像物理坐标转换关系如下，且两边求导：
[
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)
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
=
Zcf000Zcf000Zc1
XcYcZc
(3)
{
x
˙
=
f
(
X
˙
c
Z
c
−
X
c
Z
˙
c
Z
c
2
)
=
f
X
˙
c
Z
c
−
x
Z
˙
c
Z
c
y
˙
=
f
(
Y
˙
c
Z
c
−
Y
c
Z
˙
c
Z
c
2
)
=
f
Y
˙
c
Z
c
−
y
Z
˙
c
Z
c
(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
=
c
v
p
+
c
ω
p
×
p
c
(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
=
−
c
v
c
−
c
ω
c
×
p
c
(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
,
y
Z
c
+
c
ω
c
,
z
Y
c
Y
˙
c
=
−
c
ν
c
,
y
−
c
ω
c
,
z
X
c
+
c
ω
c
,
x
Z
c
Z
˙
c
=
−
c
ν
c
,
z
−
c
ω
c
,
x
Y
c
+
c
ω
c
,
y
X
c
(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
˙
=
−
f
Z
c
c
v
c
,
x
+
x
Z
c
c
v
c
,
z
+
x
y
f
c
ω
c
,
x
−
f
2
+
x
2
f
c
ω
c
,
y
+
y
c
ω
c
,
z
y
˙
=
−
f
Z
c
c
v
c
,
y
+
y
Z
c
c
v
c
,
z
+
f
2
+
y
2
f
c
ω
c
,
x
−
x
y
f
c
ω
c
,
y
−
x
c
ω
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{x y}{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{x y}{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
˙
]
=
[
−
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)
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
=
d
x
(
u
−
u
0
)
x=d_{x}left(u-u_{0}
ight)
x=dx(u−u0)和
y
=
d
y
(
v
−
v
0
)
y=d_y(v-v_0)
y=dy(v−v0)代入(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[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
˙
]
=
J
i
m
g
[
c
v
c
c
u
c
]
(11)
egin{bmatrix}dot{u}\ dot{v}end{bmatrix}=J_{img}egin{bmatrix}^coldsymbol{v}_{c}\^c oldsymbol{u}_{c}end{bmatrix} ag{11}
[u˙v˙]=Jimg[cvccuc](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[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)如有不足之处欢迎指出~
站长推荐
- U8W/U8W-Mini使用与常见问题解决
U8W/U8W-Mini使用与常见问题解决 - stm32使用HAL库配置串口中断收发数据（保姆级教程）
stm32使用HAL库配置串口中断收发数据（保姆级教程） - SpringSecurity实现前后端分离认证授权
SpringSecurity实现前后端分离认证授权 - 【社区图书馆】伴我前行的一本书《The C Programming Language》
【社区图书馆】伴我前行的一本书《The C Programming Language》 - 分享几个国内免费的ChatGPT镜像网址(亲测有效)
分享几个国内免费的ChatGPT镜像网址(亲测有效)