关注即可了解更多相关知识。
欢迎转发、收藏、友善交流。
文章目录
旋转矩阵
Givens rotation 逆时针
[
c
−
s
s
c
]
\begin{bmatrix}c &-s\\s &c\end{bmatrix}
[cs−sc]
Jacobi rotation 顺时针
[
c
s
−
s
c
]
\begin{bmatrix}c &s\\-s &c\end{bmatrix}
[c−ssc]
箭头朝里朝外，顺时针、逆时针，旋转角的正负
高中物理 磁场方向 右手法则
左手系、右手系
左乘、右乘
左乘： 坐标系不动，点动，则左乘。
右乘： 点不动，坐标系动，则右乘。
【可以说，如果一个旋转矩阵左乘表示逆时针旋转 theta 角，那么将此矩阵右乘的话则表示顺时针旋转 theta 角】
左乘与右乘是可以变换的。也即是说：
R
l
e
f
t
(
θ
)
=
R
r
i
g
h
t
(
θ
)
,
R
l
e
f
t
⋅
R
r
i
g
h
t
=
I
R_{left}(\theta)=R_{right}(\theta),R_{left}\cdot R_{right}=I
Rleft(θ)=Rright(θ),Rleft⋅Rright=I
特殊欧氏群
S
E
(
n
)
SE(n)
SE(n)
n
n
n维欧氏变换
特殊正交群
S
O
(
n
)
SO(n)
SO(n)旋转矩阵群
相似变换
S
i
m
(
3
)
Sim(3)
Sim(3)
S
O
(
3
)
SO(3)
SO(3)三维空间的旋转
罗德里格斯公式 （Rodrigues’s Formula）
R
T
(
α
)
=
R
−
1
(
α
)
=
R
(
−
α
)
R^T(\alpha)=R^{-1}(\alpha)=R(-\alpha)
RT(α)=R−1(α)=R(−α)
三角恒等式 Trigonometric identities
sin
θ
=
−
sin
(
−
θ
)
=
−
cos
(
θ
+
9
0
∘
)
=
cos
(
θ
−
9
0
∘
)
\sin \theta=-\sin (-\theta)=-\cos(\theta+90^\circ)=\cos(\theta-90^\circ)
sinθ=−sin(−θ)=−cos(θ+90∘)=cos(θ−90∘)
cos
θ
=
cos
(
−
θ
)
=
sin
(
θ
+
9
0
∘
)
=
−
sin
(
θ
−
9
0
∘
)
\cos \theta=\cos (-\theta)=\sin(\theta+90^\circ)=-\sin(\theta-90^\circ)
cosθ=cos(−θ)=sin(θ+90∘)=−sin(θ−90∘)
sin
(
θ
1
+
θ
2
)
=
s
1
c
2
+
c
1
s
2
=
s
12
\sin(\theta_1+\theta_2)=s_1c_2+c_1s_2=s_{12}
sin(θ1+θ2)=s1c2+c1s2=s12
sin
(
θ
1
−
θ
2
)
=
s
1
c
2
−
c
1
s
2
\sin(\theta_1-\theta_2)=s_1c_2-c_1s_2
sin(θ1−θ2)=s1c2−c1s2
cos
(
θ
1
+
θ
2
)
=
c
1
c
2
−
s
1
s
2
=
c
12
\cos(\theta_1+\theta_2)=c_1c_2-s_1s_2=c_{12}
cos(θ1+θ2)=c1c2−s1s2=c12
cos
(
θ
1
−
θ
2
)
=
c
1
c
2
+
s
1
s
2
\cos(\theta_1-\theta_2)=c_1c_2+s_1s_2
cos(θ1−θ2)=c1c2+s1s2
二维旋转矩阵
R ( θ ) = [ cos θ − sin θ sin θ cos θ ] = cos θ [ 1 0 0 1 ] + sin θ [ 0 − 1 1 0 ] = exp ( θ [ 0 − 1 1 0 ] ) R(\theta)={\begin{bmatrix}\cos \theta &-\sin \theta \\\sin \theta &\cos \theta \\\end{bmatrix}}=\cos \theta \begin{bmatrix}1 &0\\0 &1\\\end{bmatrix}+\sin \theta \begin{bmatrix}0 &-1\\1 &0\\\end{bmatrix}=\exp(\theta \begin{bmatrix}0 &-1\\1 &0\\\end{bmatrix}) R(θ)=[cosθsinθ−sinθcosθ]=cosθ[1001]+sinθ[01−10]=exp(θ[01−10])
三维旋转矩阵 Euler Rotations
R x ( θ x ) = [ 1 0 0 0 cos θ x − sin θ x 0 sin θ x cos θ x ] = exp ( [ 0 0 0 0 0 − θ x 0 θ x 0 ] ) = r o l l R_{x}(\theta_x)={\begin{bmatrix}1 &0 &0\\0 &\cos \theta_x &-\sin \theta_x \\0 &\sin \theta_x &\cos \theta_x \\\end{bmatrix}}=\exp(\begin{bmatrix}0 &0 &0\\0 &0 &-\theta_x\\0 &\theta_x &0\end{bmatrix})=roll Rx(θx)=⎣⎡1000cosθxsinθx0−sinθxcosθx⎦⎤=exp(⎣⎡00000θx0−θx0⎦⎤)=roll
R y ( θ y ) = [ cos θ y 0 sin θ y 0 1 0 − sin θ y 0 cos θ y ] = exp ( [ 0 0 θ y 0 0 0 − θ y 0 0 ] ) = p i t c h R_{y}(\theta_y )={\begin{bmatrix}\cos \theta_y &0&\sin \theta_y \\0&1&0\\-\sin \theta_y &0&\cos \theta_y \\\end{bmatrix}}=\exp(\begin{bmatrix}0 &0 &\theta_y\\0 &0 &0\\-\theta_y &0 &0\end{bmatrix})=pitch Ry(θy)=⎣⎡cosθy0−sinθy010sinθy0cosθy⎦⎤=exp(⎣⎡00−θy000θy00⎦⎤)=pitch
R z ( θ z ) = [ cos θ z − sin θ z 0 sin θ z cos θ z 0 0 0 1 ] = exp ( [ 0 − θ z 0 θ z 0 0 0 0 0 ] ) = y a w R_{z}(\theta_z )={\begin{bmatrix}\cos \theta_z &-\sin \theta_z &0\\\sin \theta_z &\cos \theta_z &0\\0&0&1\\\end{bmatrix}}=\exp(\begin{bmatrix}0 &-\theta_z &0\\\theta_z &0 &0\\0 &0 &0\end{bmatrix})=yaw Rz(θz)=⎣⎡cosθzsinθz0−sinθzcosθz0001⎦⎤=exp(⎣⎡0θz0−θz00000⎦⎤)=yaw
M = R z ( θ z ) R y ( θ y ) R x ( θ x ) = exp ( [ 0 − θ z θ y θ z 0 − θ x − θ y θ x 0 ] ) M=R_z(\theta_z)R_y(\theta_y)R_x(\theta_x)=\exp(\begin{bmatrix}0 &-\theta_z &\theta_y\\\theta_z &0 &-\theta_x\\-\theta_y &\theta_x &0\end{bmatrix}) M=Rz(θz)Ry(θy)Rx(θx)=exp(⎣⎡0θz−θy−θz0θxθy−θx0⎦⎤)
matlab
eul = [0 pi/2 0]; % z y x
rotmZYX = eul2rotm(eul)
微分旋转矩阵
M 0 = [ 1 − θ z θ y θ z 1 − θ x − θ y θ x 1 ] M_0=\begin{bmatrix}1 &-\theta_z &\theta_y\\\theta_z &1 &-\theta_x\\-\theta_y &\theta_x &1\end{bmatrix} M0=⎣⎡1θz−θy−θz1θxθy−θx1⎦⎤
“偏航-俯仰-滚转”（yaw-pitch-roll）
1.绕物体的Z 轴旋转， 得到偏航角yaw；
2.绕旋转之后 的Y 轴旋转， 得到俯仰角pitch；
3.绕旋转之后 的X 轴旋转， 得到滚转角roll。
东北天、站心坐标系
Local east, north, up (ENU) coordinates
R
z
(
+
(
90
+
L
)
)
R
x
(
+
(
90
−
B
)
)
R_z(+(90+L))R_x(+(90-B))
Rz(+(90+L))Rx(+(90−B)) 逆
R
x
(
−
(
90
−
B
)
)
R
z
(
−
(
90
+
L
)
)
R_x(-(90-B))R_z(-(90+L))
Rx(−(90−B))Rz(−(90+L)) 正 右乘
R = [ − sin λ − sin ϕ cos λ cos ϕ cos λ cos λ − sin ϕ sin λ cos ϕ sin λ 0 cos ϕ sin ϕ ] {\displaystyle R={\begin{bmatrix}-\sin \lambda &-\sin \phi \cos \lambda &\cos \phi \cos \lambda \\\cos \lambda &-\sin \phi \sin \lambda &\cos \phi \sin \lambda \\0&\cos \phi &\sin \phi \end{bmatrix}}} R=⎣⎡−sinλcosλ0−sinϕcosλ−sinϕsinλcosϕcosϕcosλcosϕsinλsinϕ⎦⎤
Local north, east, down (NED) coordinates
R
z
(
+
L
)
R
y
(
−
(
90
+
B
)
)
R_z(+L)R_y(-(90+B))
Rz(+L)Ry(−(90+B)) 逆
R
y
(
+
(
90
+
B
)
)
R
z
(
−
L
)
R_y(+(90+B))R_z(-L)
Ry(+(90+B))Rz(−L) 正 右乘
R = [ − sin ( ϕ ) cos ( λ ) − sin ( λ ) − cos ( ϕ ) cos ( λ ) − sin ( ϕ ) sin ( λ ) cos ( λ ) − cos ( ϕ ) sin ( λ ) cos ( ϕ ) 0 − sin ( ϕ ) ] {\displaystyle R={\begin{bmatrix}-\sin(\phi )\cos(\lambda )&-\sin(\lambda )&-\cos(\phi )\cos(\lambda )\\-\sin(\phi )\sin(\lambda )&\cos(\lambda )&-\cos(\phi )\sin(\lambda )\\\cos(\phi )&0&-\sin(\phi )\end{bmatrix}}} R=⎣⎡−sin(ϕ)cos(λ)−sin(ϕ)sin(λ)cos(ϕ)−sin(λ)cos(λ)0−cos(ϕ)cos(λ)−cos(ϕ)sin(λ)−sin(ϕ)⎦⎤
Matlab
3-D Coordinate and Vector Transformations
geodetic2enu
enu2geodetic
Geodetic and ECEF Coordinate Systems
BLH2XYZ
Geodetic coordinates (latitude
ϕ
{\displaystyle\ \phi }
ϕ, longitude
λ
{\displaystyle \ \lambda }
λ, height
h
{\displaystyle h}
h) can be converted into ECEF coordinates using the following equation:
X
=
(
N
(
ϕ
)
+
h
)
cos
ϕ
cos
λ
Y
=
(
N
(
ϕ
)
+
h
)
cos
ϕ
sin
λ
Z
=
(
b
2
a
2
N
(
ϕ
)
+
h
)
sin
ϕ
{\displaystyle {\begin{aligned}X&=\left(N(\phi )+h\right)\cos {\phi }\cos {\lambda }\\Y&=\left(N(\phi )+h\right)\cos {\phi }\sin {\lambda }\\Z&=\left({\frac {b^{2}}{a^{2}}}N(\phi )+h\right)\sin {\phi }\end{aligned}}}
XYZ=(N(ϕ)+h)cosϕcosλ=(N(ϕ)+h)cosϕsinλ=(a2b2N(ϕ)+h)sinϕ
where
N
(
ϕ
)
=
a
2
a
2
cos
2
ϕ
+
b
2
sin
2
ϕ
=
a
1
−
e
2
sin
2
ϕ
,
{\displaystyle N(\phi )={\frac {a^{2}}{\sqrt {a^{2}\cos ^{2}\phi +b^{2}\sin ^{2}\phi }}}={\frac {a}{\sqrt {1-e^{2}\sin ^{2}\phi }}},}
N(ϕ)=a2cos2ϕ+b2sin2ϕa2=1−e2sin2ϕa,
其中
e
2
=
1
−
b
2
a
2
{\displaystyle e^{2}=1-{\frac {b^{2}}{a^{2}}}}
e2=1−a2b2
{ B = arctan [ z e + ( e ′ ) 2 ⋅ b ⋅ sin 3 U x e 2 + y e 2 − a ⋅ e 2 ⋅ cos 3 U ] L = arctan y e x e H = x e 2 + y e 2 cos B − N \left\{\begin{array}{l}B=\arctan \left[\frac{z_{e}+\left(e^{\prime}\right)^{2} \cdot b \cdot \sin ^{3} U}{\sqrt{x_{e}^{2}+y_{e}^{2}}-a \cdot e^{2} \cdot \cos ^{3} U}\right]\\\\L=\arctan \frac{y_e}{x_e}\\\\H=\frac{\sqrt{x_{e}^{2}+y_{e}^{2}}}{\cos B}-N\\\end{array}\right. ⎩⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎧B=arctan[xe2+ye2−a⋅e2⋅cos3Uze+(e′)2⋅b⋅sin3U]L=arctanxeyeH=cosBxe2+ye2−N
当大地高 H < 1000 k m H<1000km H<1000km 时，上式的计算精度可达厘米级。但当大地高过大时，纬度计算精度将下降，且大地高H的计算稳定性也会下降。为此 Bowring于1985年又给出下列改进公式:
[ bowring1985 ] THE ACCURACY OF GEODETIC LATITUDE AND HEIGHT EQUATIONS
用上式计算时纬度的精度可达
1
′
′
×
1
0
−
7
1''\times10^{-7}
1′′×10−7 ，大地高的误差小于
1
0
−
6
c
m
10^{-6}cm
10−6cm ，可满足
各种用户的要求。注意上式主要用于高精度的大地测量计算等应用场合，而对于导航来讲之前的公式一般即可满足要求。
经纬度 对应 圆弧距 arc distance
Δ lat 1 = π a ( 1 − e 2 ) 18 0 ∘ ( 1 − e 2 sin 2 ϕ ) 3 2 {\displaystyle \Delta _{\text{lat}}^{1}={\frac {\pi a\left(1-e^{2}\right)}{180^{\circ }\left(1-e^{2}\sin ^{2}\phi \right)^{\frac {3}{2}}}}} Δlat1=180∘(1−e2sin2ϕ)23πa(1−e2)
WGS84 spheroid
Δ lat 1 = 111 132.954 − 559.822 cos 2 ϕ + 1.175 cos 4 ϕ {\displaystyle \Delta _{\text{lat}}^{1}=111\,132.954-559.822\cos 2\phi +1.175\cos 4\phi } Δlat1=111132.954−559.822cos2ϕ+1.175cos4ϕ
Δ long 1 = π a cos ϕ 18 0 ∘ 1 − e 2 sin 2 ϕ {\displaystyle \Delta _{\text{long}}^{1}={\frac {\pi a\cos \phi }{180^{\circ }{\sqrt {1-e^{2}\sin ^{2}\phi }}}}\,} Δlong1=180∘1−e2sin2ϕπacosϕ
| ϕ \phi ϕ | Δ lat 1 \Delta _{\text{lat}}^{1} Δlat1 | Δ long 1 \Delta _{\text{long}}^{1} Δlong1 |
|---|---|---|
| 0° | 110.574 km | 111.320 km |
| 15° | 110.649 km | 107.550 km |
| 30° | 110.852 km | 96.486 km |
| 45° | 111.132 km | 78.847 km |
| 60° | 111.412 km | 55.800 km |
| 75° | 111.618 km | 28.902 km |
| 90° | 111.694 km | 0.000 km |
( d X d Y d Z ) = ( − sin λ − sin ϕ cos λ cos ϕ cos λ cos λ − sin ϕ sin λ cos ϕ sin λ 0 cos ϕ sin ϕ ) ( d E d N d U ) , ( d E d N d U ) = ( ( N ( ϕ ) + h ) cos ϕ 0 0 0 M ( ϕ ) + h 0 0 0 1 ) ( d λ d ϕ d h ) , {\displaystyle {\begin{aligned}{\begin{pmatrix}dX\\dY\\dZ\end{pmatrix}}&={\begin{pmatrix}-\sin \lambda &-\sin \phi \cos \lambda &\cos \phi \cos \lambda \\\cos \lambda &-\sin \phi \sin \lambda &\cos \phi \sin \lambda \\0&\cos \phi &\sin \phi \\\end{pmatrix}}{\begin{pmatrix}dE\\dN\\dU\end{pmatrix}},\\[3pt]{\begin{pmatrix}dE\\dN\\dU\end{pmatrix}}&={\begin{pmatrix}\left(N(\phi )+h\right)\cos \phi &0&0\\0&M(\phi )+h&0\\0&0&1\\\end{pmatrix}}{\begin{pmatrix}d\lambda \\d\phi \\dh\end{pmatrix}},\end{aligned}}} ⎝⎛dXdYdZ⎠⎞⎝⎛dEdNdU⎠⎞=⎝⎛−sinλcosλ0−sinϕcosλ−sinϕsinλcosϕcosϕcosλcosϕsinλsinϕ⎠⎞⎝⎛dEdNdU⎠⎞,=⎝⎛(N(ϕ)+h)cosϕ000M(ϕ)+h0001⎠⎞⎝⎛dλdϕdh⎠⎞,
M ( ϕ ) = a ( 1 − e 2 ) ( 1 − e 2 sin 2 ϕ ) 3 2 {\displaystyle M(\phi )={\frac {a\left(1-e^{2}\right)}{\left(1-e^{2}\sin ^{2}\phi \right)^{\frac {3}{2}}}}} M(ϕ)=(1−e2sin2ϕ)23a(1−e2)
[ x y z ] = [ E N U ] \begin{bmatrix}x\\y\\z\end{bmatrix}=\begin{bmatrix}E\\N\\U\end{bmatrix} ⎣⎡xyz⎦⎤=⎣⎡ENU⎦⎤
地心坐标系
大地坐标系（φ,λ,h）纬度、经度、高度，geodetic
地心地固坐标系（Earth-Centered, Earth-Fixed，简称ECEF）简称地心坐标系
WGS-84椭球模型
From ECEF to ENU
先绕z轴正向 逆时针 旋转 90+L, 再绕x轴 顺时针 旋转 90-B. -号表示为右乘：点不动，坐标系动
R
x
(
−
(
90
−
B
)
)
R
z
(
−
(
90
+
L
)
)
=
[
−
sin
L
cos
L
0
−
sin
B
cos
L
−
sin
B
sin
L
cos
B
cos
B
cos
L
cos
B
sin
L
sin
B
]
R_x(-(90-B))R_z(-(90+L))={\begin{bmatrix}-\sin L&\cos L&0\\-\sin B\cos L&-\sin B\sin L&\cos B\\\cos B\cos L&\cos B\sin L&\sin B\end{bmatrix}}
Rx(−(90−B))Rz(−(90+L))=⎣⎡−sinL−sinBcosLcosBcosLcosL−sinBsinLcosBsinL0cosBsinB⎦⎤ 正 右乘
R x ( − ( 90 − B ) ) = [ 1 0 0 0 cos ( B − 90 ) − sin ( B − 90 ) 0 sin ( B − 90 ) cos ( B − 90 ) ] = [ 1 0 0 0 sin B cos B 0 − cos B sin B ] {\displaystyle R_x(-(90-B))= {\begin{bmatrix}1 &0 &0\\0 &\cos(B-90) &-\sin(B-90) \\0 &\sin(B-90) &\cos(B-90)\end{bmatrix}}= {\begin{bmatrix}1 &0 &0\\0 &\sin B &\cos B \\0 &-\cos B &\sin B\end{bmatrix}}} Rx(−(90−B))=⎣⎡1000cos(B−90)sin(B−90)0−sin(B−90)cos(B−90)⎦⎤=⎣⎡1000sinB−cosB0cosBsinB⎦⎤
R z ( − ( 90 + L ) ) = [ cos ( L + 90 ) sin ( L + 90 ) 0 − sin ( L + 90 ) cos ( L + 90 ) 0 0 0 1 ] = [ − sin L cos L 0 − cos L − sin L 0 0 0 1 ] {\displaystyle R_z(-(90+L))={\begin{bmatrix}\cos(L+90) &\sin(L+90) &0 \\ -\sin(L+90) &\cos(L+90) &0\\ 0 &0 &1\end{bmatrix}}={\begin{bmatrix}-\sin L &\cos L &0 \\ -\cos L &-\sin L &0\\ 0 &0 &1\end{bmatrix}}} Rz(−(90+L))=⎣⎡cos(L+90)−sin(L+90)0sin(L+90)cos(L+90)0001⎦⎤=⎣⎡−sinL−cosL0cosL−sinL0001⎦⎤
[ x y z ] = [ − sin λ r cos λ r 0 − sin ϕ r cos λ r − sin ϕ r sin λ r cos ϕ r cos ϕ r cos λ r cos ϕ r sin λ r sin ϕ r ] [ X p − X r Y p − Y r Z p − Z r ] {\displaystyle {\begin{bmatrix}x\\y\\z\end{bmatrix}}={\begin{bmatrix}-\sin \lambda _{r}&\cos \lambda _{r}&0\\-\sin \phi _{r}\cos \lambda _{r}&-\sin \phi _{r}\sin \lambda _{r}&\cos \phi _{r}\\\cos \phi _{r}\cos \lambda _{r}&\cos \phi _{r}\sin \lambda _{r}&\sin \phi _{r}\end{bmatrix}}{\begin{bmatrix}X_{p}-X_{r}\\Y_{p}-Y_{r}\\Z_{p}-Z_{r}\end{bmatrix}}} ⎣⎡xyz⎦⎤=⎣⎡−sinλr−sinϕrcosλrcosϕrcosλrcosλr−sinϕrsinλrcosϕrsinλr0cosϕrsinϕr⎦⎤⎣⎡Xp−XrYp−YrZp−Zr⎦⎤
From ENU to ECEF
R
z
(
+
(
90
+
L
)
)
R
x
(
+
(
90
−
B
)
)
R_z(+(90+L))R_x(+(90-B))
Rz(+(90+L))Rx(+(90−B)) 逆
[
X
Y
Z
]
=
[
−
sin
λ
−
sin
ϕ
cos
λ
cos
ϕ
cos
λ
cos
λ
−
sin
ϕ
sin
λ
cos
ϕ
sin
λ
0
cos
ϕ
sin
ϕ
]
[
x
y
z
]
+
[
X
r
Y
r
Z
r
]
{\displaystyle {\begin{bmatrix}X\\Y\\Z\end{bmatrix}}={\begin{bmatrix}-\sin \lambda &-\sin \phi \cos \lambda &\cos \phi \cos \lambda \\\cos \lambda &-\sin \phi \sin \lambda &\cos \phi \sin \lambda \\0&\cos \phi &\sin \phi \end{bmatrix}}{\begin{bmatrix}x\\y\\z\end{bmatrix}}+{\begin{bmatrix}X_{r}\\Y_{r}\\Z_{r}\end{bmatrix}}}
⎣⎡XYZ⎦⎤=⎣⎡−sinλcosλ0−sinϕcosλ−sinϕsinλcosϕcosϕcosλcosϕsinλsinϕ⎦⎤⎣⎡xyz⎦⎤+⎣⎡XrYrZr⎦⎤
可根据BLH、DAE求BLH,
[lat,lon,h] = aer2geodetic(az,elev,slantRange,lat0,lon0,h0,spheroid)
参考文献
- GIS Fundamentals A First Text on Geographic Information Systems by Paul Bolstad
- Small Unmanned Aircraft Theory and Practice by Randal W. Beard, Timothy W. McLain 小型无人机理论与应用
- Unmanned Rotorcraft Systems by Guowei Cai, Ben M. Chen, Tong Heng Lee
- [ bowring1985 ] THE ACCURACY OF GEODETIC LATITUDE AND HEIGHT EQUATIONS
赞赏
如果你觉得以上内容对你有帮助，欢迎赞赏、转发！
扫一扫
8119
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
