BA雅可比矩阵详细推导

 已知空间点(世界坐标系下) p = [ X Y Z ] T \bf{p}=\begin{bmatrix}\it{X} & \it{Y} & \it{Z}\end{bmatrix}^\mathrm{T} p=[XYZ]T,记其重投影位置为 x ′ = [ u ′ v ′ ] T \bf{x'}=\begin{bmatrix}\it{u'}&v'\end{bmatrix}^\mathrm{T} x=[uv]T以及其对应实际像素坐标点 x = [ u v ] T \bf{x}=\begin{bmatrix}\it{u}&v\end{bmatrix}^\mathrm{T} x=[uv]T,相机焦距为 f x f_x fx f y f_y fy,平移量为 c x c_x cx c y c_y cy,径向畸变系数 k 1 k_1 k1 k 2 k_2 k2,旋转矩阵 R R R对应的旋转向量 w = [ w 0 w 1 w 2 ] T \bf{w}=\begin{bmatrix}\it{w_0}&w_1&w_2\end{bmatrix}^\mathrm{T} w=[w0w1w2]T,平移向量 t = [ t 0 t 1 t 2 ] T \bf{t}=\begin{bmatrix}\it{t_0}&t_1&t_2\end{bmatrix}^\mathrm{T} t=[t0t1t2]T,则重投影误差 e = [ u − u ′ v − v ′ ] T \bf{e}=\begin{bmatrix}\it{u-u'}&\it{v-v'}\end{bmatrix}^\mathrm{T} e=[uuvv]T关于空间点位置,相机内参,相机外参的雅可比矩阵推导如下:

 首先,写出重投影误差 e \bf{e} e的具体形式。记 p \bf{p} p点在相机坐标系下的坐标为 [ x ′ y ′ z ′ ] T \begin{bmatrix}x'&y'&z'\end{bmatrix}^\mathrm{T} [xyz]T,有:
[ x ′ y ′ z ′ ] = R [ X Y Z ] + [ t 0 t 1 t 2 ] \begin{bmatrix}x'\\y'\\z'\end{bmatrix}=R\begin{bmatrix}X\\Y\\Z\end{bmatrix}+\begin{bmatrix}t_0\\t_1\\t_2\end{bmatrix} xyz=RXYZ+t0t1t2
记归一化平面坐标 [ x y ] T \begin{bmatrix}x&y\end{bmatrix}^\mathrm{T} [xy]T,有:
[ x y ] = [ x ′ z ′ y ′ z ′ ] \begin{bmatrix}x\\y\end{bmatrix}=\begin{bmatrix}\displaystyle\frac{x'}{z'}\\\displaystyle\frac{y'}{z'}\end{bmatrix} [xy]=zxzy
记畸变后对应坐标为 [ x d i s t o r t e d y d i s t o r t e d ] T \begin{bmatrix}x_{distorted}&y_{distorted}\end{bmatrix}^\mathrm{T} [xdistortedydistorted]T r 2 = x 2 + y 2 r^2=x^2+y^2 r2=x2+y2,有:
[ x d i s t o r t e d y d i s t o r t e d ] = [ x ( 1 + k 1 r 2 + k 2 r 4 ) y ( 1 + k 1 r 2 + k 2 r 4 ) ] \begin{bmatrix}x_{distorted}\\y_{distorted}\end{bmatrix}=\begin{bmatrix}x(1+k_1r^2+k_2r^4)\\y(1+k_1r^2+k_2r^4)\end{bmatrix} [xdistortedydistorted]=[x(1+k1r2+k2r4)y(1+k1r2+k2r4)]
那么其重投影坐标 x ′ \bf{x'} x为:
[ u ′ v ′ ] = [ f x ∗ x d i s t o r t e d + c x f y ∗ y d i s t o r t e d + c y ] \begin{bmatrix}u'\\v'\end{bmatrix}=\begin{bmatrix}f_x*x_{distorted}+c_x\\f_y*y_{distorted}+c_y\end{bmatrix} [uv]=[fxxdistorted+cxfyydistorted+cy]
故重投影误差 e \bf{e} e为:
e = [ e 0 e 1 ] = [ u − u ′ v − v ′ ] = [ u − f x ∗ x ∗ ( 1 + k 1 r 2 + k 2 r 4 ) − c x v − f y ∗ y ∗ ( 1 + k 1 r 2 + k 2 r 4 ) − c y ] \bf{e}=\begin{bmatrix}e_0\\e_1\end{bmatrix}=\begin{bmatrix}u-u'\\v-v'\end{bmatrix}=\begin{bmatrix}u-f_x*x*(1+k_1r^2+k_2r^4)-c_x\\v-f_y*y*(1+k_1r^2+k_2r^4)-c_y\end{bmatrix} e=[e0e1]=[uuvv]=[ufxx(1+k1r2+k2r4)cxvfyy(1+k1r2+k2r4)cy]

故雅克比矩阵可写作( R R R w \bf{w} w表示):
∂ e ∂ [ f x , f y , k 1 , k 2 , w 0 , w 1 , w 2 , t 0 , t 1 , t 2 , X , Y , Z ] = [ ∂ e 0 ∂ f x , ∂ e 0 ∂ f y , ∂ e 0 ∂ k 1 , ∂ e 0 ∂ k 2 , ∂ e 0 ∂ w 0 , ∂ e 0 ∂ w 1 , ∂ e 0 ∂ w 2 , ∂ e 0 ∂ t 0 , ∂ e 0 ∂ t 1 , ∂ e 0 ∂ t 2 , ∂ e 0 ∂ X , ∂ e 0 ∂ Y , ∂ e 0 ∂ Z ∂ e 1 ∂ f x , ∂ e 1 ∂ f y , ∂ e 1 ∂ k 1 , ∂ e 1 ∂ k 2 , ∂ e 1 ∂ w 0 , ∂ e 1 ∂ w 1 , ∂ e 1 ∂ w 2 , ∂ e 1 ∂ t 0 , ∂ e 1 ∂ t 1 , ∂ e 1 ∂ t 2 , ∂ e 1 ∂ X , ∂ e 1 ∂ Y , ∂ e 1 ∂ Z ] \displaystyle\frac{\partial\bf{e}}{\partial\begin{bmatrix}f_x,f_y,k_1,k_2,w_0,w_1,w_2,t_0,t_1,t_2,X,Y,Z\end{bmatrix}}=\\\begin{bmatrix}\displaystyle\frac{\partial e_0}{\partial f_x},\displaystyle\frac{\partial e_0}{\partial f_y},\displaystyle\frac{\partial e_0}{\partial k_1},\displaystyle\frac{\partial e_0}{\partial k_2},\displaystyle\frac{\partial e_0}{\partial w_0},\displaystyle\frac{\partial e_0}{\partial w_1},\displaystyle\frac{\partial e_0}{\partial w_2},\displaystyle\frac{\partial e_0}{\partial t_0},\displaystyle\frac{\partial e_0}{\partial t_1},\displaystyle\frac{\partial e_0}{\partial t_2},\displaystyle\frac{\partial e_0}{\partial X},\displaystyle\frac{\partial e_0}{\partial Y},\displaystyle\frac{\partial e_0}{\partial Z}\\\displaystyle\frac{\partial e_1}{\partial f_x},\displaystyle\frac{\partial e_1}{\partial f_y},\displaystyle\frac{\partial e_1}{\partial k_1},\displaystyle\frac{\partial e_1}{\partial k_2},\displaystyle\frac{\partial e_1}{\partial w_0},\displaystyle\frac{\partial e_1}{\partial w_1},\displaystyle\frac{\partial e_1}{\partial w_2},\displaystyle\frac{\partial e_1}{\partial t_0},\displaystyle\frac{\partial e_1}{\partial t_1},\displaystyle\frac{\partial e_1}{\partial t_2},\displaystyle\frac{\partial e_1}{\partial X},\displaystyle\frac{\partial e_1}{\partial Y},\displaystyle\frac{\partial e_1}{\partial Z}\end{bmatrix} [fx,fy,k1,k2,w0,w1,w2,t0,t1,t2,X,Y,Z]e=fxe0,fye0,k1e0,k2e0,w0e0,w1e0,w2e0,t0e0,t1e0,t2e0,Xe0,Ye0,Ze0fxe1,fye1,k1e1,k2e1,w0e1,w1e1,w2e1,t0e1,t1e1,t2e1,Xe1,Ye1,Ze1
下面求每项偏导数的具体形式。

1.关于相机内参的偏导数

∂ e 0 ∂ f x = − x ( 1 + k 1 r 2 + k 2 r 4 ) \displaystyle\frac{\partial e_0}{\partial f_x}=-x(1+k_1r^2+k_2r^4) fxe0=x(1+k1r2+k2r4)

∂ e 1 ∂ f x = 0 \displaystyle\frac{\partial e_1}{\partial f_x}=0 fxe1=0

∂ e 0 ∂ f y = 0 \displaystyle\frac{\partial e_0}{\partial f_y}=0 fye0=0

∂ e 1 ∂ f y = − y ( 1 + k 1 r 2 + k 2 r 4 ) \displaystyle\frac{\partial e_1}{\partial f_y}=-y(1+k_1r^2+k_2r^4) fye1=y(1+k1r2+k2r4)

∂ e 0 ∂ k 1 = − f x x r 2 \displaystyle\frac{\partial e_0}{\partial k_1}=-f_xxr^2 k1e0=fxxr2

∂ e 1 ∂ k 1 = − f y y r 2 \displaystyle\frac{\partial e_1}{\partial k_1}=-f_yyr^2 k1e1=fyyr2

∂ e 0 ∂ k 2 = − f x x r 4 \displaystyle\frac{\partial e_0}{\partial k_2}=-f_xxr^4 k2e0=fxxr4

∂ e 1 ∂ k 2 = − f y y r 4 \displaystyle\frac{\partial e_1}{\partial k_2}=-f_yyr^4 k2e1=fyyr4

2.关于旋转的偏导数

 考虑畸变时, e \bf{e} e关于旋转的偏导数形式较为复杂,故在推导之前,预先求出部分中间变量(后面的结果均会用到这几个量):
∂ x ∂ x ′ = 1 z ′ , ∂ x ∂ y ′ = 0 , ∂ x ∂ z ′ = − x ′ z ′ 2 ∂ y ∂ x ′ = 0 , ∂ y ∂ y ′ = 1 z ′ , ∂ y ∂ z ′ = − y ′ z ′ 2 ∂ e 0 ∂ x = − f x ( 1 + k 1 r 2 + k 2 r 4 ) − f x x ( 2 k 1 x + 4 k 2 x r 2 ) ∂ e 0 ∂ y = − f x x ( 2 k 1 y + 4 k 2 y r 2 ) ∂ e 1 ∂ x = − f y y ( 2 k 1 x + 4 k 2 x r 2 ) ∂ e 1 ∂ y = − f y ( 1 + k 1 r 2 + k 2 r 4 ) − f y y ( 2 k 1 y + 4 k 2 y r 2 ) \displaystyle\frac{\partial x}{\partial x'}=\frac{1}{z'},\displaystyle\frac{\partial x}{\partial y'}=0,\displaystyle\frac{\partial x}{\partial z'}=-\frac{x'}{z'^2}\\ \displaystyle\frac{\partial y}{\partial x'}=0,\displaystyle\frac{\partial y}{\partial y'}=\frac{1}{z'},\displaystyle\frac{\partial y}{\partial z'}=-\frac{y'}{z'^2}\\ \displaystyle\frac{\partial e_0}{\partial x}=-f_x(1+k_1r^2+k_2r^4)-f_xx(2k_1x+4k_2xr^2)\\ \displaystyle\frac{\partial e_0}{\partial y}=-f_xx(2k_1y+4k_2yr^2)\\ \displaystyle\frac{\partial e_1}{\partial x}=-f_yy(2k_1x+4k_2xr^2)\\ \displaystyle\frac{\partial e_1}{\partial y}=-f_y(1+k_1r^2+k_2r^4)-f_yy(2k_1y+4k_2yr^2) xx=z1,yx=0,zx=z2xxy=0,yy=z1,zy=z2yxe0=fx(1+k1r2+k2r4)fxx(2k1x+4k2xr2)ye0=fxx(2k1y+4k2yr2)xe1=fyy(2k1x+4k2xr2)ye1=fy(1+k1r2+k2r4)fyy(2k1y+4k2yr2)

 旋转向量 w = θ n {\bf{w}}=\theta n w=θn,其中 θ \theta θ代表旋转角度, n n n代表旋转轴,由罗德里格斯公式有:
R = cos ⁡ θ I + ( 1 − cos ⁡ θ ) n n T + sin ⁡ θ n ∧ R=\cos\theta {\boldsymbol{I}}+(1-\cos\theta)nn^\mathrm{T}+\sin\theta n^\wedge R=cosθI+(1cosθ)nnT+sinθn
当旋转一个很小的角度时, θ → 0 \theta\to0 θ0,有:
Δ R = I + θ n ∧ = I + w ∧ = [ 1 0 0 0 1 0 0 0 1 ] + [ 0 − w 2 w 1 w 2 0 − w 0 − w 1 w 0 0 ] \Delta R={\boldsymbol{I}}+\theta n^\wedge={\boldsymbol{I}}+{\bf{w}}^\wedge=\begin{bmatrix}1&0&0\\0&1&0\\0&0&1\end{bmatrix}+\begin{bmatrix}0&-w_2&w_1\\w_2&0&-w_0\\-w_1&w_0&0\end{bmatrix} ΔR=I+θn=I+w=100010001+0w2w1w20w0w1w00
记世界坐标系下空间点 p \bf{p} p在相机坐标系下为 p ′ \bf{p'} p,则 p ′ = R p + t {\bf{p'}}=R{\bf{p}}+t p=Rp+t,则基于左扰动模型有:
∂ p ′ ∂ w = lim ⁡ w → 0 Δ R ∗ R p + t − R p − t w = lim ⁡ w → 0 ( I + w ∧ ) R p − R p w = lim ⁡ w → 0 w ∧ R p w = lim ⁡ w → 0 − ( R p ) ∧ w w = − ( R p ) ∧ \begin{aligned}\displaystyle\frac{\partial\bf{p'}}{\partial \bf{w}}&=\lim\limits_{\bf{w}\to0}\displaystyle\frac{{\Delta R*R{\bf{p}}+t}-R{\bf{p}}-t}{\bf{w}} \\&=\lim\limits_{\bf{w}\to0}\displaystyle\frac{{(\boldsymbol{I}+{\bf{w}}^\wedge)R{\bf{p}}}-R{\bf{p}}}{\bf{w}} \\&=\lim\limits_{\bf{w}\to0}\displaystyle\frac{{\bf{w}}^\wedge R{\bf{p}}}{\bf{w}} =\lim\limits_{\bf{w}\to0}\displaystyle\frac{-(R{\bf{p}})^\wedge\bf{w}}{\bf{w}}=-(R\bf{p})^\wedge \end{aligned} wp=w0limwΔRRp+tRpt=w0limw(I+w)RpRp=w0limwwRp=w0limw(Rp)w=(Rp)

p ′ = [ x ′ y ′ z ′ ] T \bf{p'}=\begin{bmatrix}x'&y'&z'\end{bmatrix}^\mathrm{T} p=[xyz]T w = [ w 0 w 1 w 2 ] T \bf{w}=\begin{bmatrix}\it{w_0}&w_1&w_2\end{bmatrix}^\mathrm{T} w=[w0w1w2]T,记 R R R的行向量为 r 0 , r 1 , r 2 \boldsymbol{r_0},\boldsymbol{r_1},\boldsymbol{r_2} r0,r1,r2则上式写作:
∂ [ x ′ y ′ z ′ ] T ∂ [ w 0 w 1 w 2 ] T = [ ∂ x ′ ∂ w 0 ∂ x ′ ∂ w 1 ∂ x ′ ∂ w 2 ∂ y ′ ∂ w 0 ∂ y ′ ∂ w 1 ∂ y ′ ∂ w 2 ∂ z ′ ∂ w 0 ∂ z ′ ∂ w 1 ∂ z ′ ∂ w 2 ] = [ 0 r 2 p − r 1 p − r 2 p 0 r 0 p r 1 p − r 0 p 0 ] \displaystyle\frac{\partial \begin{bmatrix}x'&y'&z'\end{bmatrix}^\mathrm{T}}{\partial \begin{bmatrix}\it{w_0}&w_1&w_2\end{bmatrix}^\mathrm{T}} =\begin{bmatrix}\displaystyle\frac{\partial x'}{\partial w_0}&\displaystyle\frac{\partial x'}{\partial w_1}&\displaystyle\frac{\partial x'}{\partial w_2}\\\displaystyle\frac{\partial y'}{\partial w_0}&\displaystyle\frac{\partial y'}{\partial w_1}&\displaystyle\frac{\partial y'}{\partial w_2}\\\displaystyle\frac{\partial z'}{\partial w_0}&\displaystyle\frac{\partial z'}{\partial w_1}&\displaystyle\frac{\partial z'}{\partial w_2}\end{bmatrix} =\begin{bmatrix}0&\boldsymbol{r_2}{\bf{p}}&-\boldsymbol{r_1}{\bf{p}}\\-\boldsymbol{r_2}{\bf{p}}&0&\boldsymbol{r_0}{\bf{p}}\\\boldsymbol{r_1}{\bf{p}}&-\boldsymbol{r_0}{\bf{p}}&0\end{bmatrix} [w0w1w2]T[xyz]T=w0xw0yw0zw1xw1yw1zw2xw2yw2z=0r2pr1pr2p0r0pr1pr0p0
归一化平面坐标 [ x y ] T \begin{bmatrix}x&y\end{bmatrix}^\mathrm{T} [xy]T是关于 p ′ \bf{p'} p的函数,由链式求导有:
∂ e 0 ∂ w 0 = ∂ e 0 ∂ x ( ∂ x ∂ x ′ ∂ x ′ ∂ w 0 + ∂ x ∂ y ′ ∂ y ′ ∂ w 0 + ∂ x ∂ z ′ ∂ z ′ ∂ w 0 ) + ∂ e 0 ∂ y ( ∂ y ∂ x ′ ∂ x ′ ∂ w 0 + ∂ y ∂ y ′ ∂ y ′ ∂ w 0 + ∂ y ∂ z ′ ∂ z ′ ∂ w 0 ) = ∂ e 0 ∂ x ∂ x ∂ z ′ ∂ z ′ ∂ w 0 + ∂ e 0 ∂ y ( ∂ y ∂ y ′ ∂ y ′ ∂ w 0 + ∂ y ∂ z ′ ∂ z ′ ∂ w 0 ) \begin{aligned} \displaystyle\frac{\partial e_0}{\partial w_0}&=\displaystyle\frac{\partial e_0}{\partial x}(\displaystyle\frac{\partial x}{\partial x'}\frac{\partial x'}{\partial w_0}+\displaystyle\frac{\partial x}{\partial y'}\frac{\partial y'}{\partial w_0}+\displaystyle\frac{\partial x}{\partial z'}\frac{\partial z'}{\partial w_0})+ \displaystyle\frac{\partial e_0}{\partial y}(\displaystyle\frac{\partial y}{\partial x'}\frac{\partial x'}{\partial w_0}+\displaystyle\frac{\partial y}{\partial y'}\frac{\partial y'}{\partial w_0}+\displaystyle\frac{\partial y}{\partial z'}\frac{\partial z'}{\partial w_0}) \\&=\displaystyle\frac{\partial e_0}{\partial x}\displaystyle\frac{\partial x}{\partial z'}\frac{\partial z'}{\partial w_0}+ \displaystyle\frac{\partial e_0}{\partial y}(\displaystyle\frac{\partial y}{\partial y'}\frac{\partial y'}{\partial w_0}+\displaystyle\frac{\partial y}{\partial z'}\frac{\partial z'}{\partial w_0}) \end{aligned} w0e0=xe0(xxw0x+yxw0y+zxw0z)+ye0(xyw0x+yyw0y+zyw0z)=xe0zxw0z+ye0(yyw0y+zyw0z)

∂ e 0 ∂ w 1 = ∂ e 0 ∂ x ( ∂ x ∂ x ′ ∂ x ′ ∂ w 1 + ∂ x ∂ y ′ ∂ y ′ ∂ w 1 + ∂ x ∂ z ′ ∂ z ′ ∂ w 1 ) + ∂ e 0 ∂ y ( ∂ y ∂ x ′ ∂ x ′ ∂ w 1 + ∂ y ∂ y ′ ∂ y ′ ∂ w 1 + ∂ y ∂ z ′ ∂ z ′ ∂ w 1 ) = ∂ e 0 ∂ x ( ∂ x ∂ x ′ ∂ x ′ ∂ w 1 + ∂ x ∂ z ′ ∂ z ′ ∂ w 1 ) + ∂ e 0 ∂ y ∂ y ∂ z ′ ∂ z ′ ∂ w 1 \begin{aligned} \displaystyle\frac{\partial e_0}{\partial w_1}&=\displaystyle\frac{\partial e_0}{\partial x}(\displaystyle\frac{\partial x}{\partial x'}\frac{\partial x'}{\partial w_1}+\displaystyle\frac{\partial x}{\partial y'}\frac{\partial y'}{\partial w_1}+\displaystyle\frac{\partial x}{\partial z'}\frac{\partial z'}{\partial w_1})+ \displaystyle\frac{\partial e_0}{\partial y}(\displaystyle\frac{\partial y}{\partial x'}\frac{\partial x'}{\partial w_1}+\displaystyle\frac{\partial y}{\partial y'}\frac{\partial y'}{\partial w_1}+\displaystyle\frac{\partial y}{\partial z'}\frac{\partial z'}{\partial w_1}) \\&=\displaystyle\frac{\partial e_0}{\partial x}(\displaystyle\frac{\partial x}{\partial x'}\frac{\partial x'}{\partial w_1}+\displaystyle\frac{\partial x}{\partial z'}\frac{\partial z'}{\partial w_1})+ \displaystyle\frac{\partial e_0}{\partial y}\displaystyle\frac{\partial y}{\partial z'}\frac{\partial z'}{\partial w_1} \end{aligned} w1e0=xe0(xxw1x+yxw1y+zxw1z)+ye0(xyw1x+yyw1y+zyw1z)=xe0(xxw1x+zxw1z)+ye0zyw1z

∂ e 0 ∂ w 2 = ∂ e 0 ∂ x ( ∂ x ∂ x ′ ∂ x ′ ∂ w 2 + ∂ x ∂ y ′ ∂ y ′ ∂ w 2 + ∂ x ∂ z ′ ∂ z ′ ∂ w 2 ) + ∂ e 0 ∂ y ( ∂ y ∂ x ′ ∂ x ′ ∂ w 2 + ∂ y ∂ y ′ ∂ y ′ ∂ w 2 + ∂ y ∂ z ′ ∂ z ′ ∂ w 2 ) = ∂ e 0 ∂ x ∂ x ∂ x ′ ∂ x ′ ∂ w 2 + ∂ e 0 ∂ y ∂ y ∂ y ′ ∂ y ′ ∂ w 2 \begin{aligned} \displaystyle\frac{\partial e_0}{\partial w_2}&=\displaystyle\frac{\partial e_0}{\partial x}(\displaystyle\frac{\partial x}{\partial x'}\frac{\partial x'}{\partial w_2}+\displaystyle\frac{\partial x}{\partial y'}\frac{\partial y'}{\partial w_2}+\displaystyle\frac{\partial x}{\partial z'}\frac{\partial z'}{\partial w_2})+ \displaystyle\frac{\partial e_0}{\partial y}(\displaystyle\frac{\partial y}{\partial x'}\frac{\partial x'}{\partial w_2}+\displaystyle\frac{\partial y}{\partial y'}\frac{\partial y'}{\partial w_2}+\displaystyle\frac{\partial y}{\partial z'}\frac{\partial z'}{\partial w_2}) \\&=\displaystyle\frac{\partial e_0}{\partial x}\displaystyle\frac{\partial x}{\partial x'}\frac{\partial x'}{\partial w_2}+ \displaystyle\frac{\partial e_0}{\partial y}\displaystyle\frac{\partial y}{\partial y'}\frac{\partial y'}{\partial w_2} \end{aligned} w2e0=xe0(xxw2x+yxw2y+zxw2z)+ye0(xyw2x+yyw2y+zyw2z)=xe0xxw2x+ye0yyw2y

∂ e 1 ∂ w 0 = ∂ e 1 ∂ x ( ∂ x ∂ x ′ ∂ x ′ ∂ w 0 + ∂ x ∂ y ′ ∂ y ′ ∂ w 0 + ∂ x ∂ z ′ ∂ z ′ ∂ w 0 ) + ∂ e 1 ∂ y ( ∂ y ∂ x ′ ∂ x ′ ∂ w 0 + ∂ y ∂ y ′ ∂ y ′ ∂ w 0 + ∂ y ∂ z ′ ∂ z ′ ∂ w 0 ) = ∂ e 1 ∂ x ∂ x ∂ z ′ ∂ z ′ ∂ w 0 + ∂ e 1 ∂ y ( ∂ y ∂ y ′ ∂ y ′ ∂ w 0 + ∂ y ∂ z ′ ∂ z ′ ∂ w 0 ) \begin{aligned} \displaystyle\frac{\partial e_1}{\partial w_0}&=\displaystyle\frac{\partial e_1}{\partial x}(\displaystyle\frac{\partial x}{\partial x'}\frac{\partial x'}{\partial w_0}+\displaystyle\frac{\partial x}{\partial y'}\frac{\partial y'}{\partial w_0}+\displaystyle\frac{\partial x}{\partial z'}\frac{\partial z'}{\partial w_0})+ \displaystyle\frac{\partial e_1}{\partial y}(\displaystyle\frac{\partial y}{\partial x'}\frac{\partial x'}{\partial w_0}+\displaystyle\frac{\partial y}{\partial y'}\frac{\partial y'}{\partial w_0}+\displaystyle\frac{\partial y}{\partial z'}\frac{\partial z'}{\partial w_0}) \\&=\displaystyle\frac{\partial e_1}{\partial x}\displaystyle\frac{\partial x}{\partial z'}\frac{\partial z'}{\partial w_0}+ \displaystyle\frac{\partial e_1}{\partial y}(\displaystyle\frac{\partial y}{\partial y'}\frac{\partial y'}{\partial w_0}+\displaystyle\frac{\partial y}{\partial z'}\frac{\partial z'}{\partial w_0}) \end{aligned} w0e1=xe1(xxw0x+yxw0y+zxw0z)+ye1(xyw0x+yyw0y+zyw0z)=xe1zxw0z+ye1(yyw0y+zyw0z)

∂ e 1 ∂ w 1 = ∂ e 1 ∂ x ( ∂ x ∂ x ′ ∂ x ′ ∂ w 1 + ∂ x ∂ y ′ ∂ y ′ ∂ w 1 + ∂ x ∂ z ′ ∂ z ′ ∂ w 1 ) + ∂ e 1 ∂ y ( ∂ y ∂ x ′ ∂ x ′ ∂ w 1 + ∂ y ∂ y ′ ∂ y ′ ∂ w 1 + ∂ y ∂ z ′ ∂ z ′ ∂ w 1 ) = ∂ e 1 ∂ x ( ∂ x ∂ x ′ ∂ x ′ ∂ w 1 + ∂ x ∂ z ′ ∂ z ′ ∂ w 1 ) + ∂ e 1 ∂ y ∂ y ∂ z ′ ∂ z ′ ∂ w 1 \begin{aligned} \displaystyle\frac{\partial e_1}{\partial w_1}&=\displaystyle\frac{\partial e_1}{\partial x}(\displaystyle\frac{\partial x}{\partial x'}\frac{\partial x'}{\partial w_1}+\displaystyle\frac{\partial x}{\partial y'}\frac{\partial y'}{\partial w_1}+\displaystyle\frac{\partial x}{\partial z'}\frac{\partial z'}{\partial w_1})+ \displaystyle\frac{\partial e_1}{\partial y}(\displaystyle\frac{\partial y}{\partial x'}\frac{\partial x'}{\partial w_1}+\displaystyle\frac{\partial y}{\partial y'}\frac{\partial y'}{\partial w_1}+\displaystyle\frac{\partial y}{\partial z'}\frac{\partial z'}{\partial w_1}) \\&=\displaystyle\frac{\partial e_1}{\partial x}(\displaystyle\frac{\partial x}{\partial x'}\frac{\partial x'}{\partial w_1}+\displaystyle\frac{\partial x}{\partial z'}\frac{\partial z'}{\partial w_1})+ \displaystyle\frac{\partial e_1}{\partial y}\displaystyle\frac{\partial y}{\partial z'}\frac{\partial z'}{\partial w_1} \end{aligned} w1e1=xe1(xxw1x+yxw1y+zxw1z)+ye1(xyw1x+yyw1y+zyw1z)=xe1(xxw1x+zxw1z)+ye1zyw1z

∂ e 1 ∂ w 2 = ∂ e 1 ∂ x ( ∂ x ∂ x ′ ∂ x ′ ∂ w 2 + ∂ x ∂ y ′ ∂ y ′ ∂ w 2 + ∂ x ∂ z ′ ∂ z ′ ∂ w 2 ) + ∂ e 1 ∂ y ( ∂ y ∂ x ′ ∂ x ′ ∂ w 2 + ∂ y ∂ y ′ ∂ y ′ ∂ w 2 + ∂ y ∂ z ′ ∂ z ′ ∂ w 2 ) = ∂ e 1 ∂ x ∂ x ∂ x ′ ∂ x ′ ∂ w 2 + ∂ e 1 ∂ y ∂ y ∂ y ′ ∂ y ′ ∂ w 2 \begin{aligned} \displaystyle\frac{\partial e_1}{\partial w_2}&=\displaystyle\frac{\partial e_1}{\partial x}(\displaystyle\frac{\partial x}{\partial x'}\frac{\partial x'}{\partial w_2}+\displaystyle\frac{\partial x}{\partial y'}\frac{\partial y'}{\partial w_2}+\displaystyle\frac{\partial x}{\partial z'}\frac{\partial z'}{\partial w_2})+ \displaystyle\frac{\partial e_1}{\partial y}(\displaystyle\frac{\partial y}{\partial x'}\frac{\partial x'}{\partial w_2}+\displaystyle\frac{\partial y}{\partial y'}\frac{\partial y'}{\partial w_2}+\displaystyle\frac{\partial y}{\partial z'}\frac{\partial z'}{\partial w_2}) \\&=\displaystyle\frac{\partial e_1}{\partial x}\displaystyle\frac{\partial x}{\partial x'}\frac{\partial x'}{\partial w_2}+ \displaystyle\frac{\partial e_1}{\partial y}\displaystyle\frac{\partial y}{\partial y'}\frac{\partial y'}{\partial w_2} \end{aligned} w2e1=xe1(xxw2x+yxw2y+zxw2z)+ye1(xyw2x+yyw2y+zyw2z)=xe1xxw2x+ye1yyw2y

3.关于平移的偏导数

 由 p ′ = R p + t {\bf{p'}}=R{\bf{p}}+t p=Rp+t,即:
[ x ′ y ′ z ′ ] = [ r 0 p + t 0 r 1 p + t 1 r 2 p + t 2 ] \begin{bmatrix} x'\\y'\\z'\end{bmatrix}=\begin{bmatrix}\boldsymbol{r_0}{\bf{p}}+t_0\\\boldsymbol{r_1}{\bf{p}}+t_1\\\boldsymbol{r_2}{\bf{p}}+t_2\end{bmatrix} xyz=r0p+t0r1p+t1r2p+t2

故有:
∂ x ′ ∂ t 0 = 1 , ∂ y ′ ∂ t 1 = 1 , ∂ z ′ ∂ t 2 = 1 \displaystyle\frac{\partial x'}{\partial t_0}=1,\displaystyle\frac{\partial y'}{\partial t_1}=1,\displaystyle\frac{\partial z'}{\partial t_2}=1 t0x=1,t1y=1,t2z=1

同样地,由链式求导有:
∂ e 0 ∂ t 0 = ∂ e 0 ∂ x ( ∂ x ∂ x ′ ∂ x ′ ∂ t 0 + ∂ x ∂ y ′ ∂ y ′ ∂ t 0 + ∂ x ∂ z ′ ∂ z ′ ∂ t 0 ) + ∂ e 0 ∂ y ( ∂ y ∂ x ′ ∂ x ′ ∂ t 0 + ∂ y ∂ y ′ ∂ y ′ ∂ t 0 + ∂ y ∂ z ′ ∂ z ′ ∂ t 0 ) = ∂ e 0 ∂ x ∂ x ∂ x ′ \begin{aligned} \displaystyle\frac{\partial e_0}{\partial t_0}&=\displaystyle\frac{\partial e_0}{\partial x}(\displaystyle\frac{\partial x}{\partial x'}\frac{\partial x'}{\partial t_0}+\displaystyle\frac{\partial x}{\partial y'}\frac{\partial y'}{\partial t_0}+\displaystyle\frac{\partial x}{\partial z'}\frac{\partial z'}{\partial t_0})+ \displaystyle\frac{\partial e_0}{\partial y}(\displaystyle\frac{\partial y}{\partial x'}\frac{\partial x'}{\partial t_0}+\displaystyle\frac{\partial y}{\partial y'}\frac{\partial y'}{\partial t_0}+\displaystyle\frac{\partial y}{\partial z'}\frac{\partial z'}{\partial t_0}) \\&=\displaystyle\frac{\partial e_0}{\partial x}\displaystyle\frac{\partial x}{\partial x'} \end{aligned} t0e0=xe0(xxt0x+yxt0y+zxt0z)+ye0(xyt0x+yyt0y+zyt0z)=xe0xx

∂ e 0 ∂ t 1 = ∂ e 0 ∂ x ( ∂ x ∂ x ′ ∂ x ′ ∂ t 1 + ∂ x ∂ y ′ ∂ y ′ ∂ t 1 + ∂ x ∂ z ′ ∂ z ′ ∂ t 1 ) + ∂ e 0 ∂ y ( ∂ y ∂ x ′ ∂ x ′ ∂ t 1 + ∂ y ∂ y ′ ∂ y ′ ∂ t 1 + ∂ y ∂ z ′ ∂ z ′ ∂ t 1 ) = ∂ e 0 ∂ y ∂ y ∂ y ′ \begin{aligned} \displaystyle\frac{\partial e_0}{\partial t_1}&=\displaystyle\frac{\partial e_0}{\partial x}(\displaystyle\frac{\partial x}{\partial x'}\frac{\partial x'}{\partial t_1}+\displaystyle\frac{\partial x}{\partial y'}\frac{\partial y'}{\partial t_1}+\displaystyle\frac{\partial x}{\partial z'}\frac{\partial z'}{\partial t_1})+ \displaystyle\frac{\partial e_0}{\partial y}(\displaystyle\frac{\partial y}{\partial x'}\frac{\partial x'}{\partial t_1}+\displaystyle\frac{\partial y}{\partial y'}\frac{\partial y'}{\partial t_1}+\displaystyle\frac{\partial y}{\partial z'}\frac{\partial z'}{\partial t_1}) \\&=\displaystyle\frac{\partial e_0}{\partial y}\displaystyle\frac{\partial y}{\partial y'} \end{aligned} t1e0=xe0(xxt1x+yxt1y+zxt1z)+ye0(xyt1x+yyt1y+zyt1z)=ye0yy

∂ e 0 ∂ t 2 = ∂ e 0 ∂ x ( ∂ x ∂ x ′ ∂ x ′ ∂ t 2 + ∂ x ∂ y ′ ∂ y ′ ∂ t 2 + ∂ x ∂ z ′ ∂ z ′ ∂ t 2 ) + ∂ e 0 ∂ y ( ∂ y ∂ x ′ ∂ x ′ ∂ t 2 + ∂ y ∂ y ′ ∂ y ′ ∂ t 2 + ∂ y ∂ z ′ ∂ z ′ ∂ t 2 ) = ∂ e 0 ∂ x ∂ x ∂ z ′ + ∂ e 0 ∂ y ∂ y ∂ z ′ \begin{aligned} \displaystyle\frac{\partial e_0}{\partial t_2}&=\displaystyle\frac{\partial e_0}{\partial x}(\displaystyle\frac{\partial x}{\partial x'}\frac{\partial x'}{\partial t_2}+\displaystyle\frac{\partial x}{\partial y'}\frac{\partial y'}{\partial t_2}+\displaystyle\frac{\partial x}{\partial z'}\frac{\partial z'}{\partial t_2})+ \displaystyle\frac{\partial e_0}{\partial y}(\displaystyle\frac{\partial y}{\partial x'}\frac{\partial x'}{\partial t_2}+\displaystyle\frac{\partial y}{\partial y'}\frac{\partial y'}{\partial t_2}+\displaystyle\frac{\partial y}{\partial z'}\frac{\partial z'}{\partial t_2}) \\&=\displaystyle\frac{\partial e_0}{\partial x}\displaystyle\frac{\partial x}{\partial z'}+\displaystyle\frac{\partial e_0}{\partial y}\displaystyle\frac{\partial y}{\partial z'} \end{aligned} t2e0=xe0(xxt2x+yxt2y+zxt2z)+ye0(xyt2x+yyt2y+zyt2z)=xe0zx+ye0zy

∂ e 1 ∂ t 0 = ∂ e 1 ∂ x ( ∂ x ∂ x ′ ∂ x ′ ∂ t 0 + ∂ x ∂ y ′ ∂ y ′ ∂ t 0 + ∂ x ∂ z ′ ∂ z ′ ∂ t 0 ) + ∂ e 1 ∂ y ( ∂ y ∂ x ′ ∂ x ′ ∂ t 0 + ∂ y ∂ y ′ ∂ y ′ ∂ t 0 + ∂ y ∂ z ′ ∂ z ′ ∂ t 0 ) = ∂ e 1 ∂ x ∂ x ∂ x ′ \begin{aligned} \displaystyle\frac{\partial e_1}{\partial t_0}&=\displaystyle\frac{\partial e_1}{\partial x}(\displaystyle\frac{\partial x}{\partial x'}\frac{\partial x'}{\partial t_0}+\displaystyle\frac{\partial x}{\partial y'}\frac{\partial y'}{\partial t_0}+\displaystyle\frac{\partial x}{\partial z'}\frac{\partial z'}{\partial t_0})+ \displaystyle\frac{\partial e_1}{\partial y}(\displaystyle\frac{\partial y}{\partial x'}\frac{\partial x'}{\partial t_0}+\displaystyle\frac{\partial y}{\partial y'}\frac{\partial y'}{\partial t_0}+\displaystyle\frac{\partial y}{\partial z'}\frac{\partial z'}{\partial t_0}) \\&=\displaystyle\frac{\partial e_1}{\partial x}\displaystyle\frac{\partial x}{\partial x'} \end{aligned} t0e1=xe1(xxt0x+yxt0y+zxt0z)+ye1(xyt0x+yyt0y+zyt0z)=xe1xx

∂ e 1 ∂ t 1 = ∂ e 1 ∂ x ( ∂ x ∂ x ′ ∂ x ′ ∂ t 1 + ∂ x ∂ y ′ ∂ y ′ ∂ t 1 + ∂ x ∂ z ′ ∂ z ′ ∂ t 1 ) + ∂ e 1 ∂ y ( ∂ y ∂ x ′ ∂ x ′ ∂ t 1 + ∂ y ∂ y ′ ∂ y ′ ∂ t 1 + ∂ y ∂ z ′ ∂ z ′ ∂ t 1 ) = ∂ e 0 ∂ y ∂ y ∂ y ′ \begin{aligned} \displaystyle\frac{\partial e_1}{\partial t_1}&=\displaystyle\frac{\partial e_1}{\partial x}(\displaystyle\frac{\partial x}{\partial x'}\frac{\partial x'}{\partial t_1}+\displaystyle\frac{\partial x}{\partial y'}\frac{\partial y'}{\partial t_1}+\displaystyle\frac{\partial x}{\partial z'}\frac{\partial z'}{\partial t_1})+ \displaystyle\frac{\partial e_1}{\partial y}(\displaystyle\frac{\partial y}{\partial x'}\frac{\partial x'}{\partial t_1}+\displaystyle\frac{\partial y}{\partial y'}\frac{\partial y'}{\partial t_1}+\displaystyle\frac{\partial y}{\partial z'}\frac{\partial z'}{\partial t_1}) \\&=\displaystyle\frac{\partial e_0}{\partial y}\displaystyle\frac{\partial y}{\partial y'} \end{aligned} t1e1=xe1(xxt1x+yxt1y+zxt1z)+ye1(xyt1x+yyt1y+zyt1z)=ye0yy

∂ e 1 ∂ t 2 = ∂ e 1 ∂ x ( ∂ x ∂ x ′ ∂ x ′ ∂ t 2 + ∂ x ∂ y ′ ∂ y ′ ∂ t 2 + ∂ x ∂ z ′ ∂ z ′ ∂ t 2 ) + ∂ e 1 ∂ y ( ∂ y ∂ x ′ ∂ x ′ ∂ t 2 + ∂ y ∂ y ′ ∂ y ′ ∂ t 2 + ∂ y ∂ z ′ ∂ z ′ ∂ t 2 ) = ∂ e 1 ∂ x ∂ x ∂ z ′ + ∂ e 1 ∂ y ∂ y ∂ z ′ \begin{aligned} \displaystyle\frac{\partial e_1}{\partial t_2}&=\displaystyle\frac{\partial e_1}{\partial x}(\displaystyle\frac{\partial x}{\partial x'}\frac{\partial x'}{\partial t_2}+\displaystyle\frac{\partial x}{\partial y'}\frac{\partial y'}{\partial t_2}+\displaystyle\frac{\partial x}{\partial z'}\frac{\partial z'}{\partial t_2})+ \displaystyle\frac{\partial e_1}{\partial y}(\displaystyle\frac{\partial y}{\partial x'}\frac{\partial x'}{\partial t_2}+\displaystyle\frac{\partial y}{\partial y'}\frac{\partial y'}{\partial t_2}+\displaystyle\frac{\partial y}{\partial z'}\frac{\partial z'}{\partial t_2}) \\&=\displaystyle\frac{\partial e_1}{\partial x}\displaystyle\frac{\partial x}{\partial z'}+\displaystyle\frac{\partial e_1}{\partial y}\displaystyle\frac{\partial y}{\partial z'} \end{aligned} t2e1=xe1(xxt2x+yxt2y+zxt2z)+ye1(xyt2x+yyt2y+zyt2z)=xe1zx+ye1zy

4.关于空间点的偏导数

 由 p ′ = R p + t {\bf{p'}}=R{\bf{p}}+t p=Rp+t,即:
[ x ′ y ′ z ′ ] = [ R 00 R 01 R 02 R 10 R 11 R 12 R 20 R 21 R 22 ] [ X Y Z ] + [ t 0 t 1 t 2 ] \begin{bmatrix}x'\\y'\\z'\end{bmatrix}=\begin{bmatrix}R_{00}&R_{01}&R_{02}\\R_{10}&R_{11}&R_{12}\\R_{20}&R_{21}&R_{22}\end{bmatrix}\begin{bmatrix}X\\Y\\Z\end{bmatrix}+\begin{bmatrix}t_0\\t_1\\t_2\end{bmatrix} xyz=R00R10R20R01R11R21R02R12R22XYZ+t0t1t2
故有:
∂ x ′ ∂ X = R 00 , ∂ x ′ ∂ Y = R 01 , ∂ x ′ ∂ Z = R 02 ∂ y ′ ∂ X = R 10 , ∂ y ′ ∂ Y = R 11 , ∂ y ′ ∂ Z = R 12 ∂ z ′ ∂ X = R 20 , ∂ z ′ ∂ Y = R 21 , ∂ z ′ ∂ Z = R 22 \displaystyle\frac{\partial x'}{\partial X}=R_{00},\displaystyle\frac{\partial x'}{\partial Y}=R_{01},\displaystyle\frac{\partial x'}{\partial Z}=R_{02}\\ \displaystyle\frac{\partial y'}{\partial X}=R_{10},\displaystyle\frac{\partial y'}{\partial Y}=R_{11},\displaystyle\frac{\partial y'}{\partial Z}=R_{12}\\ \displaystyle\frac{\partial z'}{\partial X}=R_{20},\displaystyle\frac{\partial z'}{\partial Y}=R_{21},\displaystyle\frac{\partial z'}{\partial Z}=R_{22} Xx=R00,Yx=R01,Zx=R02Xy=R10,Yy=R11,Zy=R12Xz=R20,Yz=R21,Zz=R22
同样地,由链式求导有:
∂ e 0 ∂ X = ∂ e 0 ∂ x ( ∂ x ∂ x ′ ∂ x ′ ∂ X + ∂ x ∂ y ′ ∂ y ′ ∂ X + ∂ x ∂ z ′ ∂ z ′ ∂ X ) + ∂ e 0 ∂ y ( ∂ y ∂ x ′ ∂ x ′ ∂ X + ∂ y ∂ y ′ ∂ y ′ ∂ X + ∂ y ∂ z ′ ∂ z ′ ∂ X ) = ∂ e 0 ∂ x ( ∂ x ∂ x ′ ∂ x ′ ∂ X + ∂ x ∂ z ′ ∂ z ′ ∂ X ) + ∂ e 0 ∂ y ( ∂ y ∂ y ′ ∂ y ′ ∂ X + ∂ y ∂ z ′ ∂ z ′ ∂ X ) \begin{aligned} \displaystyle\frac{\partial e_0}{\partial X}&=\displaystyle\frac{\partial e_0}{\partial x}(\displaystyle\frac{\partial x}{\partial x'}\frac{\partial x'}{\partial X}+\displaystyle\frac{\partial x}{\partial y'}\frac{\partial y'}{\partial X}+\displaystyle\frac{\partial x}{\partial z'}\frac{\partial z'}{\partial X})+ \displaystyle\frac{\partial e_0}{\partial y}(\displaystyle\frac{\partial y}{\partial x'}\frac{\partial x'}{\partial X}+\displaystyle\frac{\partial y}{\partial y'}\frac{\partial y'}{\partial X}+\displaystyle\frac{\partial y}{\partial z'}\frac{\partial z'}{\partial X}) \\&=\displaystyle\frac{\partial e_0}{\partial x}(\displaystyle\frac{\partial x}{\partial x'}\frac{\partial x'}{\partial X}+\displaystyle\frac{\partial x}{\partial z'}\frac{\partial z'}{\partial X})+ \displaystyle\frac{\partial e_0}{\partial y}(\displaystyle\frac{\partial y}{\partial y'}\frac{\partial y'}{\partial X}+\displaystyle\frac{\partial y}{\partial z'}\frac{\partial z'}{\partial X}) \end{aligned} Xe0=xe0(xxXx+yxXy+zxXz)+ye0(xyXx+yyXy+zyXz)=xe0(xxXx+zxXz)+ye0(yyXy+zyXz)

∂ e 0 ∂ Y = ∂ e 0 ∂ x ( ∂ x ∂ x ′ ∂ x ′ ∂ Y + ∂ x ∂ y ′ ∂ y ′ ∂ Y + ∂ x ∂ z ′ ∂ z ′ ∂ Y ) + ∂ e 0 ∂ y ( ∂ y ∂ x ′ ∂ x ′ ∂ Y + ∂ y ∂ y ′ ∂ y ′ ∂ Y + ∂ y ∂ z ′ ∂ z ′ ∂ Y ) = ∂ e 0 ∂ x ( ∂ x ∂ x ′ ∂ x ′ ∂ Y + ∂ x ∂ z ′ ∂ z ′ ∂ Y ) + ∂ e 0 ∂ y ( ∂ y ∂ y ′ ∂ y ′ ∂ Y + ∂ y ∂ z ′ ∂ z ′ ∂ Y ) \begin{aligned} \displaystyle\frac{\partial e_0}{\partial Y}&=\displaystyle\frac{\partial e_0}{\partial x}(\displaystyle\frac{\partial x}{\partial x'}\frac{\partial x'}{\partial Y}+\displaystyle\frac{\partial x}{\partial y'}\frac{\partial y'}{\partial Y}+\displaystyle\frac{\partial x}{\partial z'}\frac{\partial z'}{\partial Y})+ \displaystyle\frac{\partial e_0}{\partial y}(\displaystyle\frac{\partial y}{\partial x'}\frac{\partial x'}{\partial Y}+\displaystyle\frac{\partial y}{\partial y'}\frac{\partial y'}{\partial Y}+\displaystyle\frac{\partial y}{\partial z'}\frac{\partial z'}{\partial Y}) \\&=\displaystyle\frac{\partial e_0}{\partial x}(\displaystyle\frac{\partial x}{\partial x'}\frac{\partial x'}{\partial Y}+\displaystyle\frac{\partial x}{\partial z'}\frac{\partial z'}{\partial Y})+ \displaystyle\frac{\partial e_0}{\partial y}(\displaystyle\frac{\partial y}{\partial y'}\frac{\partial y'}{\partial Y}+\displaystyle\frac{\partial y}{\partial z'}\frac{\partial z'}{\partial Y}) \end{aligned} Ye0=xe0(xxYx+yxYy+zxYz)+ye0(xyYx+yyYy+zyYz)=xe0(xxYx+zxYz)+ye0(yyYy+zyYz)

∂ e 0 ∂ Z = ∂ e 0 ∂ x ( ∂ x ∂ x ′ ∂ x ′ ∂ Z + ∂ x ∂ y ′ ∂ y ′ ∂ Z + ∂ x ∂ z ′ ∂ z ′ ∂ Z ) + ∂ e 0 ∂ y ( ∂ y ∂ x ′ ∂ x ′ ∂ Z + ∂ y ∂ y ′ ∂ y ′ ∂ Z + ∂ y ∂ z ′ ∂ z ′ ∂ Z ) = ∂ e 0 ∂ x ( ∂ x ∂ x ′ ∂ x ′ ∂ Z + ∂ x ∂ z ′ ∂ z ′ ∂ Z ) + ∂ e 0 ∂ y ( ∂ y ∂ y ′ ∂ y ′ ∂ Z + ∂ y ∂ z ′ ∂ z ′ ∂ Z ) \begin{aligned} \displaystyle\frac{\partial e_0}{\partial Z}&=\displaystyle\frac{\partial e_0}{\partial x}(\displaystyle\frac{\partial x}{\partial x'}\frac{\partial x'}{\partial Z}+\displaystyle\frac{\partial x}{\partial y'}\frac{\partial y'}{\partial Z}+\displaystyle\frac{\partial x}{\partial z'}\frac{\partial z'}{\partial Z})+ \displaystyle\frac{\partial e_0}{\partial y}(\displaystyle\frac{\partial y}{\partial x'}\frac{\partial x'}{\partial Z}+\displaystyle\frac{\partial y}{\partial y'}\frac{\partial y'}{\partial Z}+\displaystyle\frac{\partial y}{\partial z'}\frac{\partial z'}{\partial Z}) \\&=\displaystyle\frac{\partial e_0}{\partial x}(\displaystyle\frac{\partial x}{\partial x'}\frac{\partial x'}{\partial Z}+\displaystyle\frac{\partial x}{\partial z'}\frac{\partial z'}{\partial Z})+ \displaystyle\frac{\partial e_0}{\partial y}(\displaystyle\frac{\partial y}{\partial y'}\frac{\partial y'}{\partial Z}+\displaystyle\frac{\partial y}{\partial z'}\frac{\partial z'}{\partial Z}) \end{aligned} Ze0=xe0(xxZx+yxZy+zxZz)+ye0(xyZx+yyZy+zyZz)=xe0(xxZx+zxZz)+ye0(yyZy+zyZz)

∂ e 1 ∂ X = ∂ e 1 ∂ x ( ∂ x ∂ x ′ ∂ x ′ ∂ X + ∂ x ∂ y ′ ∂ y ′ ∂ X + ∂ x ∂ z ′ ∂ z ′ ∂ X ) + ∂ e 1 ∂ y ( ∂ y ∂ x ′ ∂ x ′ ∂ X + ∂ y ∂ y ′ ∂ y ′ ∂ X + ∂ y ∂ z ′ ∂ z ′ ∂ X ) = ∂ e 1 ∂ x ( ∂ x ∂ x ′ ∂ x ′ ∂ X + ∂ x ∂ z ′ ∂ z ′ ∂ X ) + ∂ e 1 ∂ y ( ∂ y ∂ y ′ ∂ y ′ ∂ X + ∂ y ∂ z ′ ∂ z ′ ∂ X ) \begin{aligned} \displaystyle\frac{\partial e_1}{\partial X}&=\displaystyle\frac{\partial e_1}{\partial x}(\displaystyle\frac{\partial x}{\partial x'}\frac{\partial x'}{\partial X}+\displaystyle\frac{\partial x}{\partial y'}\frac{\partial y'}{\partial X}+\displaystyle\frac{\partial x}{\partial z'}\frac{\partial z'}{\partial X})+ \displaystyle\frac{\partial e_1}{\partial y}(\displaystyle\frac{\partial y}{\partial x'}\frac{\partial x'}{\partial X}+\displaystyle\frac{\partial y}{\partial y'}\frac{\partial y'}{\partial X}+\displaystyle\frac{\partial y}{\partial z'}\frac{\partial z'}{\partial X}) \\&=\displaystyle\frac{\partial e_1}{\partial x}(\displaystyle\frac{\partial x}{\partial x'}\frac{\partial x'}{\partial X}+\displaystyle\frac{\partial x}{\partial z'}\frac{\partial z'}{\partial X})+ \displaystyle\frac{\partial e_1}{\partial y}(\displaystyle\frac{\partial y}{\partial y'}\frac{\partial y'}{\partial X}+\displaystyle\frac{\partial y}{\partial z'}\frac{\partial z'}{\partial X}) \end{aligned} Xe1=xe1(xxXx+yxXy+zxXz)+ye1(xyXx+yyXy+zyXz)=xe1(xxXx+zxXz)+ye1(yyXy+zyXz)

∂ e 1 ∂ Y = ∂ e 1 ∂ x ( ∂ x ∂ x ′ ∂ x ′ ∂ Y + ∂ x ∂ y ′ ∂ y ′ ∂ Y + ∂ x ∂ z ′ ∂ z ′ ∂ Y ) + ∂ e 1 ∂ y ( ∂ y ∂ x ′ ∂ x ′ ∂ Y + ∂ y ∂ y ′ ∂ y ′ ∂ Y + ∂ y ∂ z ′ ∂ z ′ ∂ Y ) = ∂ e 1 ∂ x ( ∂ x ∂ x ′ ∂ x ′ ∂ Y + ∂ x ∂ z ′ ∂ z ′ ∂ Y ) + ∂ e 1 ∂ y ( ∂ y ∂ y ′ ∂ y ′ ∂ Y + ∂ y ∂ z ′ ∂ z ′ ∂ Y ) \begin{aligned} \displaystyle\frac{\partial e_1}{\partial Y}&=\displaystyle\frac{\partial e_1}{\partial x}(\displaystyle\frac{\partial x}{\partial x'}\frac{\partial x'}{\partial Y}+\displaystyle\frac{\partial x}{\partial y'}\frac{\partial y'}{\partial Y}+\displaystyle\frac{\partial x}{\partial z'}\frac{\partial z'}{\partial Y})+ \displaystyle\frac{\partial e_1}{\partial y}(\displaystyle\frac{\partial y}{\partial x'}\frac{\partial x'}{\partial Y}+\displaystyle\frac{\partial y}{\partial y'}\frac{\partial y'}{\partial Y}+\displaystyle\frac{\partial y}{\partial z'}\frac{\partial z'}{\partial Y}) \\&=\displaystyle\frac{\partial e_1}{\partial x}(\displaystyle\frac{\partial x}{\partial x'}\frac{\partial x'}{\partial Y}+\displaystyle\frac{\partial x}{\partial z'}\frac{\partial z'}{\partial Y})+ \displaystyle\frac{\partial e_1}{\partial y}(\displaystyle\frac{\partial y}{\partial y'}\frac{\partial y'}{\partial Y}+\displaystyle\frac{\partial y}{\partial z'}\frac{\partial z'}{\partial Y}) \end{aligned} Ye1=xe1(xxYx+yxYy+zxYz)+ye1(xyYx+yyYy+zyYz)=xe1(xxYx+zxYz)+ye1(yyYy+zyYz)

∂ e 1 ∂ Z = ∂ e 1 ∂ x ( ∂ x ∂ x ′ ∂ x ′ ∂ Z + ∂ x ∂ y ′ ∂ y ′ ∂ Z + ∂ x ∂ z ′ ∂ z ′ ∂ Z ) + ∂ e 1 ∂ y ( ∂ y ∂ x ′ ∂ x ′ ∂ Z + ∂ y ∂ y ′ ∂ y ′ ∂ Z + ∂ y ∂ z ′ ∂ z ′ ∂ Z ) = ∂ e 1 ∂ x ( ∂ x ∂ x ′ ∂ x ′ ∂ Z + ∂ x ∂ z ′ ∂ z ′ ∂ Z ) + ∂ e 1 ∂ y ( ∂ y ∂ y ′ ∂ y ′ ∂ Z + ∂ y ∂ z ′ ∂ z ′ ∂ Z ) \begin{aligned} \displaystyle\frac{\partial e_1}{\partial Z}&=\displaystyle\frac{\partial e_1}{\partial x}(\displaystyle\frac{\partial x}{\partial x'}\frac{\partial x'}{\partial Z}+\displaystyle\frac{\partial x}{\partial y'}\frac{\partial y'}{\partial Z}+\displaystyle\frac{\partial x}{\partial z'}\frac{\partial z'}{\partial Z})+ \displaystyle\frac{\partial e_1}{\partial y}(\displaystyle\frac{\partial y}{\partial x'}\frac{\partial x'}{\partial Z}+\displaystyle\frac{\partial y}{\partial y'}\frac{\partial y'}{\partial Z}+\displaystyle\frac{\partial y}{\partial z'}\frac{\partial z'}{\partial Z}) \\&=\displaystyle\frac{\partial e_1}{\partial x}(\displaystyle\frac{\partial x}{\partial x'}\frac{\partial x'}{\partial Z}+\displaystyle\frac{\partial x}{\partial z'}\frac{\partial z'}{\partial Z})+ \displaystyle\frac{\partial e_1}{\partial y}(\displaystyle\frac{\partial y}{\partial y'}\frac{\partial y'}{\partial Z}+\displaystyle\frac{\partial y}{\partial z'}\frac{\partial z'}{\partial Z}) \end{aligned} Ze1=xe1(xxZx+yxZy+zxZz)+ye1(xyZx+yyZy+zyZz)=xe1(xxZx+zxZz)+ye1(yyZy+zyZz)

至此,雅可比矩阵中各项已给出。

参考:https://zhuanlan.zhihu.com/p/44651602

  • 6
    点赞
  • 9
    收藏
    觉得还不错? 一键收藏
  • 1
    评论
rpr三自由度机械臂的运动学模型可以通过雅可比矩阵来描述,其推导过程如下: 首先,我们需要确定机械臂末端执行器的位置和姿态。假设机械臂的基座坐标系为 $O_0$,末端执行器的坐标系为 $O_3$,则末端执行器的位置可以用三维向量表示为: $$ \boldsymbol{p} = \begin{bmatrix} x \\ y \\ z \end{bmatrix} $$ 末端执行器的姿态可以用欧拉角表示为 $\boldsymbol{\theta} = [\theta_1, \theta_2, \theta_3]$,其中 $\theta_1$、$\theta_2$、$\theta_3$ 分别表示绕 $x$、$y$、$z$ 轴的旋转角度。 接下来,我们需要确定机械臂各关节的坐标系和相对运动关系。假设机械臂的三个关节分别为 $J_1$、$J_2$、$J_3$,它们的坐标系分别为 $O_1$、$O_2$、$O_3$,则它们之间的相对位置和姿态关系可以用以下变换矩阵表示: $$ \boldsymbol{T}_{01} = \begin{bmatrix} \cos\theta_1 & -\sin\theta_1 & 0 & 0 \\ \sin\theta_1 & \cos\theta_1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}, \quad \boldsymbol{T}_{12} = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & \cos\theta_2 & -\sin\theta_2 & 0 \\ 0 & \sin\theta_2 & \cos\theta_2 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}, \quad \boldsymbol{T}_{23} = \begin{bmatrix} \cos\theta_3 & -\sin\theta_3 & 0 & 0 \\ \sin\theta_3 & \cos\theta_3 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} $$ 其中,$\boldsymbol{T}_{01}$ 表示从 $O_1$ 到 $O_0$ 的变换矩阵,$\boldsymbol{T}_{12}$ 表示从 $O_2$ 到 $O_1$ 的变换矩阵,$\boldsymbol{T}_{23}$ 表示从 $O_3$ 到 $O_2$ 的变换矩阵。 有了以上变换矩阵,我们可以通过正向运动学计算出末端执行器的位置和姿态。但是,反向运动学需要求解关节变量 $\theta_1$、$\theta_2$、$\theta_3$ 对末端执行器位置和姿态的导数,因此需要求出雅可比矩阵雅可比矩阵可以用以下公式计算: $$ \boldsymbol{J} = \begin{bmatrix} \frac{\partial p_x}{\partial \theta_1} & \frac{\partial p_x}{\partial \theta_2} & \frac{\partial p_x}{\partial \theta_3} \\ \frac{\partial p_y}{\partial \theta_1} & \frac{\partial p_y}{\partial \theta_2} & \frac{\partial p_y}{\partial \theta_3} \\ \frac{\partial p_z}{\partial \theta_1} & \frac{\partial p_z}{\partial \theta_2} & \frac{\partial p_z}{\partial \theta_3} \\ \frac{\partial \theta_1}{\partial t} & \frac{\partial \theta_2}{\partial t} & \frac{\partial \theta_3}{\partial t} \end{bmatrix} $$ 其中,$\frac{\partial p}{\partial \theta}$ 表示末端执行器位置对关节变量的导数,$\frac{\partial \theta}{\partial t}$ 表示关节变量对时间的导数,即关节速度。 根据链式法则,我们可以将 $\frac{\partial p}{\partial \theta}$ 分解为以下三个部分的乘积: $$ \frac{\partial p}{\partial \theta} = \frac{\partial p}{\partial O_3} \cdot \frac{\partial O_3}{\partial O_2} \cdot \frac{\partial O_2}{\partial \theta} $$ 其中,$\frac{\partial p}{\partial O_3}$ 表示末端执行器位置对 $O_3$ 坐标系的变换矩阵的导数,$\frac{\partial O_3}{\partial O_2}$ 表示 $O_3$ 坐标系对 $O_2$ 坐标系的变换矩阵的导数,$\frac{\partial O_2}{\partial \theta}$ 表示 $O_2$ 坐标系对关节变量的导数。 根据变换矩阵的求导公式,可以得到: $$ \frac{\partial p}{\partial O_3} = \begin{bmatrix} \frac{\partial x}{\partial O_{3_{11}}} & \frac{\partial x}{\partial O_{3_{12}}} & \frac{\partial x}{\partial O_{3_{13}}} \\ \frac{\partial y}{\partial O_{3_{21}}} & \frac{\partial y}{\partial O_{3_{22}}} & \frac{\partial y}{\partial O_{3_{23}}} \\ \frac{\partial z}{\partial O_{3_{31}}} & \frac{\partial z}{\partial O_{3_{32}}} & \frac{\partial z}{\partial O_{3_{33}}} \end{bmatrix} $$ $$ \frac{\partial O_3}{\partial O_2} = \begin{bmatrix} \frac{\partial O_{3_{11}}}{\partial O_{2_{11}}} & \frac{\partial O_{3_{11}}}{\partial O_{2_{12}}} & \frac{\partial O_{3_{11}}}{\partial O_{2_{13}}} & 0 & 0 & 0 \\ \frac{\partial O_{3_{21}}}{\partial O_{2_{11}}} & \frac{\partial O_{3_{21}}}{\partial O_{2_{12}}} & \frac{\partial O_{3_{21}}}{\partial O_{2_{13}}} & 0 & 0 & 0 \\ \frac{\partial O_{3_{31}}}{\partial O_{2_{11}}} & \frac{\partial O_{3_{31}}}{\partial O_{2_{12}}} & \frac{\partial O_{3_{31}}}{\partial O_{2_{13}}} & 0 & 0 & 0 \\ 0 & 0 & 0 & \frac{\partial O_{3_{11}}}{\partial O_{2_{21}}} & \frac{\partial O_{3_{11}}}{\partial O_{2_{22}}} & \frac{\partial O_{3_{11}}}{\partial O_{2_{23}}} \\ 0 & 0 & 0 & \frac{\partial O_{3_{21}}}{\partial O_{2_{21}}} & \frac{\partial O_{3_{21}}}{\partial O_{2_{22}}} & \frac{\partial O_{3_{21}}}{\partial O_{2_{23}}} \\ 0 & 0 & 0 & \frac{\partial O_{3_{31}}}{\partial O_{2_{21}}} & \frac{\partial O_{3_{31}}}{\partial O_{2_{22}}} & \frac{\partial O_{3_{31}}}{\partial O_{2_{23}}} \end{bmatrix} $$ $$ \frac{\partial O_2}{\partial \theta} = \begin{bmatrix} \frac{\partial O_{2_{11}}}{\partial \theta_1} & \frac{\partial O_{2_{11}}}{\partial \theta_2} & \frac{\partial O_{2_{11}}}{\partial \theta_3} \\ \frac{\partial O_{2_{21}}}{\partial \theta_1} & \frac{\partial O_{2_{21}}}{\partial \theta_2} & \frac{\partial O_{2_{21}}}{\partial \theta_3} \\ \frac{\partial O_{2_{31}}}{\partial \theta_1} & \frac{\partial O_{2_{31}}}{\partial \theta_2} & \frac{\partial O_{2_{31}}}{\partial \theta_3} \\ 0 & 0 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & 0 \end{bmatrix} $$ 将以上三个矩阵相乘,得到 $\frac{\partial p}{\partial \theta}$。 同理,我们可以计算出关节变量对时间的导数 $\frac{\partial \theta}{\partial t}$,即关节速度。最终,将 $\frac{\partial p}{\partial \theta}$ 和 $\frac{\partial \theta}{\partial t}$ 组合成雅可比矩阵 $\boldsymbol{J}$,即可完成反向运动学的求解。

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值