三维旋转矩阵；东北天坐标系(ENU)；地心地固坐标系(ECEF)；大地坐标系(Geodetic)；经纬度对应圆弧距离

关注即可了解更多相关知识。
欢迎转发、收藏、友善交流。

文章目录

旋转矩阵
- 三角恒等式 Trigonometric identities
- 二维旋转矩阵
- 三维旋转矩阵 Euler Rotations
- matlab
- 微分旋转矩阵
- “偏航-俯仰-滚转”（yaw-pitch-roll）
东北天、站心坐标系
- Matlab
- Geodetic and ECEF Coordinate Systems
- 经纬度对应圆弧距 arc distance
地心坐标系
- From ECEF to ENU
- From ENU to ECEF
参考文献
赞赏

旋转矩阵

Givens rotation 逆时针
[c−ssc]\begin{bmatrix}c &-s\\s &c\end{bmatrix}[cs−sc]
Jacobi rotation 顺时针
[cs−sc]\begin{bmatrix}c &s\\-s &c\end{bmatrix}[c−ssc]
箭头朝里朝外，顺时针、逆时针，旋转角的正负
高中物理磁场方向右手法则
左手系、右手系
左乘、右乘

左乘：坐标系不动，点动，则左乘。
右乘：点不动，坐标系动，则右乘。
【可以说，如果一个旋转矩阵左乘表示逆时针旋转 theta 角，那么将此矩阵右乘的话则表示顺时针旋转 theta 角】
左乘与右乘是可以变换的。也即是说：Rleft(θ)=Rright(θ),Rleft⋅Rright=IR_{left}(\theta)=R_{right}(\theta),R_{left}\cdot R_{right}=IRleft(θ)=Rright(θ),Rleft⋅Rright=I

特殊欧氏群SE(n)SE(n)SE(n) nnn维欧氏变换
特殊正交群SO(n)SO(n)SO(n)旋转矩阵群
相似变换Sim(3)Sim(3)Sim(3)
SO(3)SO(3)SO(3)三维空间的旋转
罗德里格斯公式（Rodrigues’s Formula）
RT(α)=R−1(α)=R(−α)R^T(\alpha)=R^{-1}(\alpha)=R(-\alpha)RT(α)=R−1(α)=R(−α)

三角恒等式 Trigonometric identities

sin⁡θ=−sin⁡(−θ)=−cos⁡(θ+90∘)=cos⁡(θ−90∘)\sin \theta=-\sin (-\theta)=-\cos(\theta+90^\circ)=\cos(\theta-90^\circ)sinθ=−sin(−θ)=−cos(θ+90∘)=cos(θ−90∘)
cos⁡θ=cos⁡(−θ)=sin⁡(θ+90∘)=−sin⁡(θ−90∘)\cos \theta=\cos (-\theta)=\sin(\theta+90^\circ)=-\sin(\theta-90^\circ)cosθ=cos(−θ)=sin(θ+90∘)=−sin(θ−90∘)

sin⁡(θ1+θ2)=s1c2+c1s2=s12\sin(\theta_1+\theta_2)=s_1c_2+c_1s_2=s_{12}sin(θ1+θ2)=s1c2+c1s2=s12
sin⁡(θ1−θ2)=s1c2−c1s2\sin(\theta_1-\theta_2)=s_1c_2-c_1s_2sin(θ1−θ2)=s1c2−c1s2
cos⁡(θ1+θ2)=c1c2−s1s2=c12\cos(\theta_1+\theta_2)=c_1c_2-s_1s_2=c_{12}cos(θ1+θ2)=c1c2−s1s2=c12
cos⁡(θ1−θ2)=c1c2+s1s2\cos(\theta_1-\theta_2)=c_1c_2+s_1s_2cos(θ1−θ2)=c1c2+s1s2

二维旋转矩阵

R(θ)=[cos⁡θ−sin⁡θsin⁡θcos⁡θ]=cos⁡θ[1001]+sin⁡θ[0−110]=exp⁡(θ[0−110])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

Rx(θx)=[1000cos⁡θx−sin⁡θx0sin⁡θxcos⁡θx]=exp⁡([00000−θx0θx0])=rollR_{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})=rollRx(θx)=⎣⎡1000cosθxsinθx0−sinθxcosθx⎦⎤=exp(⎣⎡00000θx0−θx0⎦⎤)=roll

Ry(θy)=[cos⁡θy0sin⁡θy010−sin⁡θy0cos⁡θy]=exp⁡([00θy000−θy00])=pitchR_{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})=pitchRy(θy)=⎣⎡cosθy0−sinθy010sinθy0cosθy⎦⎤=exp(⎣⎡00−θy000θy00⎦⎤)=pitch

Rz(θz)=[cos⁡θz−sin⁡θz0sin⁡θzcos⁡θz0001]=exp⁡([0−θz0θz00000])=yawR_{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})=yawRz(θz)=⎣⎡cosθzsinθz0−sinθzcosθz0001⎦⎤=exp(⎣⎡0θz0−θz00000⎦⎤)=yaw

M=Rz(θz)Ry(θy)Rx(θx)=exp⁡([0−θzθyθz0−θx−θyθx0])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)

微分旋转矩阵

M0=[1−θzθyθz1−θx−θyθx1]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

Rz(+(90+L))Rx(+(90−B))R_z(+(90+L))R_x(+(90-B))Rz(+(90+L))Rx(+(90−B)) 逆
Rx(−(90−B))Rz(−(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⁡λ0cos⁡ϕ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

Rz(+L)Ry(−(90+B))R_z(+L)R_y(-(90+B))Rz(+L)Ry(−(90+B)) 逆
Ry(+(90+B))Rz(−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=(b2a2N(ϕ)+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(ϕ)=a2a2cos⁡2ϕ+b2sin⁡2ϕ=a1−e2sin⁡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,
其中
e2=1−b2a2{\displaystyle e^{2}=1-{\frac {b^{2}}{a^{2}}}}e2=1−a2b2

{B=arctan⁡[ze+(e′)2⋅b⋅sin⁡3Uxe2+ye2−a⋅e2⋅cos⁡3U]L=arctan⁡yexeH=xe2+ye2cos⁡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<1000kmH<1000kmH<1000km 时，上式的计算精度可达厘米级。但当大地高过大时，纬度计算精度将下降，且大地高H的计算稳定性也会下降。为此 Bowring于1985年又给出下列改进公式:

[ bowring1985 ] THE ACCURACY OF GEODETIC LATITUDE AND HEIGHT EQUATIONS

用上式计算时纬度的精度可达 1′′×10−71''\times10^{-7}1′′×10−7 ，大地高的误差小于 10−6cm10^{-6}cm10−6cm ，可满足
各种用户的要求。注意上式主要用于高精度的大地测量计算等应用场合，而对于导航来讲之前的公式一般即可满足要求。

经纬度对应圆弧距 arc distance

Δlat1=πa(1−e2)180∘(1−e2sin⁡2ϕ)32{\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

Δlat1=111132.954−559.822cos⁡2ϕ+1.175cos⁡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ϕ

Δlong1=πacos⁡ϕ180∘1−e2sin⁡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ϕ	Δlat1\Delta _{\text{lat}}^{1}Δlat1	Δlong1\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

(dXdYdZ)=(−sin⁡λ−sin⁡ϕcos⁡λcos⁡ϕcos⁡λcos⁡λ−sin⁡ϕsin⁡λcos⁡ϕsin⁡λ0cos⁡ϕsin⁡ϕ)(dEdNdU),(dEdNdU)=((N(ϕ)+h)cos⁡ϕ000M(ϕ)+h0001)(dλdϕdh),{\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−e2)(1−e2sin⁡2ϕ)32{\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)

[xyz]=[ENU]\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. -号表示为右乘：点不动，坐标系动
Rx(−(90−B))Rz(−(90+L))=[−sin⁡Lcos⁡L0−sin⁡Bcos⁡L−sin⁡Bsin⁡Lcos⁡Bcos⁡Bcos⁡Lcos⁡Bsin⁡Lsin⁡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⎦⎤ 正右乘

Rx(−(90−B))=[1000cos⁡(B−90)−sin⁡(B−90)0sin⁡(B−90)cos⁡(B−90)]=[1000sin⁡Bcos⁡B0−cos⁡Bsin⁡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⎦⎤

Rz(−(90+L))=[cos⁡(L+90)sin⁡(L+90)0−sin⁡(L+90)cos⁡(L+90)0001]=[−sin⁡Lcos⁡L0−cos⁡L−sin⁡L0001]{\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⎦⎤

[xyz]=[−sin⁡λrcos⁡λr0−sin⁡ϕrcos⁡λr−sin⁡ϕrsin⁡λrcos⁡ϕrcos⁡ϕrcos⁡λrcos⁡ϕrsin⁡λrsin⁡ϕr][Xp−XrYp−YrZp−Zr]{\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

Rz(+(90+L))Rx(+(90−B))R_z(+(90+L))R_x(+(90-B))Rz(+(90+L))Rx(+(90−B)) 逆
[XYZ]=[−sin⁡λ−sin⁡ϕcos⁡λcos⁡ϕcos⁡λcos⁡λ−sin⁡ϕsin⁡λcos⁡ϕsin⁡λ0cos⁡ϕsin⁡ϕ][xyz]+[XrYrZr]{\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

赞赏

如果你觉得以上内容对你有帮助，欢迎赞赏、转发！