云台旋转角与中国摄影测量旋转角换算

一、不同定义下的旋转

中国使用的摄影测量公式，以 y 轴为主轴的公式如下：

\[\begin{aligned}R_{y^-xz}&=R_{\varphi^-} R_\omega R_\kappa \\&=\begin{bmatrix}\cos\varphi^- & 0&-\sin\varphi^-\\0&1&0\\\sin\varphi^- &0&\cos \varphi^-\end{bmatrix}\begin{bmatrix}1&0&0\\0&\cos\omega &-\sin\omega\\0&\sin\omega &\cos \omega\end{bmatrix}\begin{bmatrix}\cos\kappa&-\sin\kappa&0\\\sin\kappa&\cos \kappa&0\\0&0&1\end{bmatrix}\end{aligned}\]

\[\begin{aligned}R_{y^-xz}=\begin{bmatrix}\cos\varphi^-\cos\kappa-\sin\varphi^-\sin\omega\sin\kappa &-\cos\varphi^-\sin\kappa-\sin\varphi^-\sin\omega\cos\kappa &-\sin\varphi^-\cos\omega \\\cos\omega\sin\kappa &\cos\omega\cos\kappa &-\sin\omega \\\sin\varphi^-\cos\kappa+\cos\varphi^-\sin\omega\sin\kappa &-\sin\varphi^-\sin\kappa+\cos\varphi^-\sin\omega\cos\kappa &\cos\varphi^-\cos\omega\end{bmatrix}\end{aligned}\]

由于通常意义上的旋转角，都是符合右手定则的。但是，摄影测量中 y 轴旋转使用的是左手定则，其原因我认为是为了保持三个旋转矩阵形式上的“一致”（但是这样反而不符合轮换对称性）。

将 \(\varphi^-\) 改为 \(-\varphi\)，可转化为计算机视觉角度下 y-x-z 顺规的旋转矩阵：

\[\begin{aligned}R_{yxz}&=R_\varphi R_\omega R_\kappa \\&=\begin{bmatrix}\cos\varphi & 0&\sin\varphi\\0&1&0\\-\sin\varphi &0&\cos \varphi\end{bmatrix}\begin{bmatrix}1&0&0\\0&\cos\omega &-\sin\omega\\0&\sin\omega &\cos \omega\end{bmatrix}\begin{bmatrix}\cos\kappa&-\sin\kappa&0\\\sin\kappa&\cos \kappa&0\\0&0&1\end{bmatrix}\end{aligned}\]

\[\begin{aligned}R_{yxz}=\begin{bmatrix}\cos\varphi\cos\kappa+\sin\varphi\sin\omega\sin\kappa &-\cos\varphi\sin\kappa+\sin\varphi\sin\omega\cos\kappa &\sin\varphi\cos\omega \\\cos\omega\sin\kappa &\cos\omega\cos\kappa &-\sin\omega \\-\sin\varphi\cos\kappa+\cos\varphi\sin\omega\sin\kappa &\sin\varphi\sin\kappa+\cos\varphi\sin\omega\cos\kappa &\cos\varphi\cos\omega\end{bmatrix}\end{aligned}\]

以 yaw、pitch 和 roll 旋转角表示，对应 z-y-x 顺规，可得到计算机视觉常用的旋转矩阵：

\[\begin{aligned}R_{zyx}&=R_ Y R_ P R_ R \\&=\begin{bmatrix}\cos Y&-\sin Y&0\\\sin Y&\cos Y&0\\0&0&1\end{bmatrix}\begin{bmatrix}\cos P & 0&\sin P\\0&1&0\\-\sin P &0&\cos P\end{bmatrix}\begin{bmatrix}1&0&0\\0&\cos R &-\sin R\\0&\sin R &\cos R\end{bmatrix}\end{aligned}\]

\[\begin{aligned}R_{zyx}=\begin{bmatrix}\cos Y\cos P &-\sin Y\cos R+\cos Y\sin P\sin R&\sin Y\sin R+\cos Y\sin P\cos R \\\sin Y\cos P&\cos Y\cos R+\sin Y\sin P\sin R&-\cos Y\sin R+\sin Y\sin P\cos R \\-\sin P &\cos P\sin R&\cos P\cos R\end{bmatrix}\end{aligned}\]

云台旋转角的旋转矩阵就如上式 \(R_{zyx}\) 所示。

二、摄影测量坐标系与云台坐标系的不同

云台旋转角是独立于无人机进行计算的，不需要考虑无人机的姿态。

云台坐标系的旋转初始状态（三个旋转角均为 0 ）是向北的（朝向北东地坐标系的 x 轴），而中国使用的摄影测量坐标系的旋转初始状态是向下的（东北天坐标 z 轴负方向）。

三、旋转角转换

如果已知 \(R_{y^-xz}\) ，就可通过若干矩阵元素计算出中国摄影测量常使用的旋转角（以下简称 nPOK）。

也就是如果能计算得到摄影测量像空间坐标 \((x_p,y_p,z_p)^\mathrm{T}\) 与像空间辅助坐标系 \((X_P,Y_P,Z_P)^\mathrm{T}\)（根据我的直觉应是东北天坐标）的换算关系，就能计算出 nPOK。

为使用已有的云台旋转角，需要将像空间下的初始状态坐标转为云台下的初始状态坐标：

\[\begin{aligned}\begin{bmatrix}x_{g} \\y_{g}\\z_{g} \end{bmatrix}&= R_{z=\pi/2}R_{x=-\pi/2}\begin{bmatrix}x \\y\\z \end{bmatrix}= \begin{bmatrix}0&-1&0 \\1&0&0\\0&0&1 \end{bmatrix}\begin{bmatrix}1&0&0 \\0&0&1\\0&-1&0 \end{bmatrix}\begin{bmatrix}x_p \\y_p\\z_p \end{bmatrix}\\&= \begin{bmatrix}0&0&-1 \\1&0&0\\0&-1&0 \end{bmatrix}\begin{bmatrix}x_p \\y_p\\z_p \end{bmatrix} \\&= R_p^g\begin{bmatrix}x_p \\y_p\\z_p \end{bmatrix}\end{aligned}\]

\(R_p^g\) 就是转换矩阵。

再由 \(R_{zyx}\)，得计算机视觉下定义的世界坐标：

\[\begin{aligned}\begin{bmatrix}X_{G} \\Y_{G}\\Z_{G} \end{bmatrix}= R_{zyx}\begin{bmatrix}x_g \\y_g\\z_g \end{bmatrix}\end{aligned}\]

再将世界坐标转为摄影测量坐标：

\[\begin{aligned}\begin{bmatrix}X_P \\Y_P\\Z_P \end{bmatrix}= R_G^P\begin{bmatrix}X_{G} \\Y_{G}\\Z_{G} \end{bmatrix}\end{aligned}\]

其中

\[\begin{aligned}R_G^P=\begin{bmatrix}0 &1 &0\\1&0&0\\0&0&-1 \end{bmatrix}\end{aligned}\]

也即：

\[\begin{aligned}\begin{bmatrix}X_P \\Y_P\\Z_P \end{bmatrix}= R_G^PR_{zyx}R_p^g\begin{bmatrix}x_p \\y_p\\z_p \end{bmatrix}\end{aligned}\]

则 \(R_{y^-xz}\) 即为我们需要的摄影测量中的旋转矩阵：

\[\begin{aligned}R_{y^-xz}= R_G^PR_{zyx}R_p^g=\begin{bmatrix}\cos Y\cos R + \sin Y\sin P\sin R&\cos Y\sin R - \sin Y\sin P\cos R&-\sin Y\cos P\\ - \sin Y\cos R + \cos Y\sin P\sin R & - \sin Y\sin R - \cos Y\sin P\cos R &-\cos Y\cos P\\-\cos P\sin R& \cos P\cos R&-\sin P\end{bmatrix}\end{aligned}\]

显然 \(R_{y^-xz}\) 是一个正交矩阵，因而它是一个旋转矩阵。它完全从摄影测量像平面坐标系出发，变换得到了摄影测量像空间辅助坐标系，故可由 \(R_{y^-xz}\) 计算 nPOK