三维空间坐标转换早期笔记

在工业测量和测绘数据处理中，常常涉及三维空间直角坐标系转换问题。本文是上学时刚接触三维测量时的实现方法，现在估计都不会有人这么干了，有现成的优化库和包，调用一下就搞定了。这里算法实现的主要思路就是：最小二乘法+泰勒展开+牛顿法迭代，这里直接用了旋转矩阵中9个参数求解的，实际就是个带二次约束的多元非线性方程组的求解问题。当然，如果不用旋转矩阵中的9个参数和约束条件，直接用3个欧拉角不断迭代也能得到相同的结果，就是那个求偏导公式实在太长了。值得注意的是，本文中初值的选取比较粗暴，实际上有很多有效的方法可以取得更好的初值，让模型收敛的更快。

1 空间坐标转换模型

1.1 空间坐标转换模型的构建

对空间任意两坐标系之间实现空间坐标系之间的转换通常通过平移、旋转、缩放来实现的 。

假设有两空间直角坐标系 O−XYZO-XYZO−XYZ 和 o−xyzo-xyzo−xyz，对空间任一点 AAA，在 O−XYZO-XYZO−XYZ 中的坐标为 X,Y,ZX, Y, ZX,Y,Z，在 o−xyzo-xyzo−xyz 中的坐标为 x,y,zx, y, zx,y,z。以 ooo 为原点建立一个与空间直角坐标系 O−XYZO-XYZO−XYZ 平行的空间直角坐标系 o−x′y′z′o-x'y'z'o−x′y′z′，AAA 点在该坐标系中的坐标为 x′,y′,z′x', y', z'x′,y′,z′。

1.1.1 绕坐标轴旋转公式的详细推导

（1）绕 x 轴顺时针旋转角度 α\alphaα

当坐标系绕 x 轴旋转时，x 坐标保持不变，只有 y 和 z 坐标发生变化。在 yz 平面内，设点 PPP 的原始坐标为 (y,z)(y, z)(y,z)，旋转 α\alphaα 角后的坐标为 (y′,z′)(y', z')(y′,z′)。

设点 PPP 到原点的距离为 rrr，点 PPP 与 y 轴的夹角为 θ\thetaθ（从 y 轴逆时针量测），则有：
y=rcos⁡θz=rsin⁡θ \begin{aligned} y &= r\cos\theta \\ z &= r\sin\theta \end{aligned} yz=rcosθ=rsinθ

当坐标系绕 x 轴顺时针 旋转 α\alphaα 角后，y 轴向 z 轴负方向转动，点 PPP 相对于新坐标系 y' 轴的夹角变为 θ+α\theta + \alphaθ+α（坐标系转离了点，夹角变大）。因此新坐标为：
y′=rcos⁡(θ+α)=r(cos⁡θcos⁡α−sin⁡θsin⁡α)=ycos⁡α−zsin⁡αz′=rsin⁡(θ+α)=r(sin⁡θcos⁡α+cos⁡θsin⁡α)=zcos⁡α+ysin⁡α \begin{aligned} y' &= r\cos(\theta + \alpha) = r(\cos\theta\cos\alpha - \sin\theta\sin\alpha) = y\cos\alpha - z\sin\alpha \\ z' &= r\sin(\theta + \alpha) = r(\sin\theta\cos\alpha + \cos\theta\sin\alpha) = z\cos\alpha + y\sin\alpha \end{aligned} y′z′=rcos(θ+α)=r(cosθcosα−sinθsinα)=ycosα−zsinα=rsin(θ+α)=r(sinθcosα+cosθsinα)=zcosα+ysinα

因此，绕 x 轴旋转的变换矩阵为：
Rx(α)=1000cos⁡α−sin⁡α0sin⁡αcos⁡α R_x(\alpha) = \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos\alpha & -\sin\alpha \\ 0 & \sin\alpha & \cos\alpha \end{bmatrix} Rx(α)= 1000cosαsinα0−sinαcosα

变换公式为：
x′y′z′=1000cos⁡α−sin⁡α0sin⁡αcos⁡αxyz \begin{bmatrix} x' \\ y' \\ z' \end{bmatrix} = \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos\alpha & -\sin\alpha \\ 0 & \sin\alpha & \cos\alpha \end{bmatrix} \begin{bmatrix} x \\ y \\ z \end{bmatrix} x′y′z′ = 1000cosαsinα0−sinαcosα xyz

（2）绕 y 轴顺时针旋转角度 β\betaβ

当坐标系绕 y 轴旋转时，y 坐标保持不变，只有 x 和 z 坐标发生变化。在 xz 平面内，设点 PPP 的原始坐标为 (x,z)(x, z)(x,z)，旋转 β\betaβ 角后的坐标为 (x′,z′)(x', z')(x′,z′)。

设点 PPP 到原点的距离为 rrr，点 PPP 与 x 轴的夹角为 θ\thetaθ（从 x 轴逆时针量测），则有：
x=rcos⁡θz=rsin⁡θ \begin{aligned} x &= r\cos\theta \\ z &= r\sin\theta \end{aligned} xz=rcosθ=rsinθ

当坐标系绕 y 轴顺时针 旋转 β\betaβ 角后（从 y 轴正方向看），x 轴向 z 轴正方向转动，点 PPP 相对于新坐标系 x' 轴的夹角变为 θ−β\theta - \betaθ−β（坐标系转向了点，夹角变小）。因此新坐标为：
x′=rcos⁡(θ−β)=r(cos⁡θcos⁡β+sin⁡θsin⁡β)=xcos⁡β+zsin⁡βz′=rsin⁡(θ−β)=r(sin⁡θcos⁡β−cos⁡θsin⁡β)=zcos⁡β−xsin⁡β \begin{aligned} x' &= r\cos(\theta - \beta) = r(\cos\theta\cos\beta + \sin\theta\sin\beta) = x\cos\beta + z\sin\beta \\ z' &= r\sin(\theta - \beta) = r(\sin\theta\cos\beta - \cos\theta\sin\beta) = z\cos\beta - x\sin\beta \end{aligned} x′z′=rcos(θ−β)=r(cosθcosβ+sinθsinβ)=xcosβ+zsinβ=rsin(θ−β)=r(sinθcosβ−cosθsinβ)=zcosβ−xsinβ

因此，绕 y 轴旋转的变换矩阵为：
Ry(β)=cos⁡β0sin⁡β010−sin⁡β0cos⁡β R_y(\beta) = \begin{bmatrix} \cos\beta & 0 & \sin\beta \\ 0 & 1 & 0 \\ -\sin\beta & 0 & \cos\beta \end{bmatrix} Ry(β)= cosβ0−sinβ010sinβ0cosβ

变换公式为：
x′y′z′=cos⁡β0sin⁡β010−sin⁡β0cos⁡βxyz \begin{bmatrix} x' \\ y' \\ z' \end{bmatrix} = \begin{bmatrix} \cos\beta & 0 & \sin\beta \\ 0 & 1 & 0 \\ -\sin\beta & 0 & \cos\beta \end{bmatrix} \begin{bmatrix} x \\ y \\ z \end{bmatrix} x′y′z′ = cosβ0−sinβ010sinβ0cosβ xyz

（3）绕 z 轴顺时针旋转角度 γ\gammaγ

当坐标系绕 z 轴旋转时，z 坐标保持不变，只有 x 和 y 坐标发生变化。在 xy 平面内，设点 PPP 的原始坐标为 (x,y)(x, y)(x,y)，旋转 γ\gammaγ 角后的坐标为 (x′,y′)(x', y')(x′,y′)。

设点 PPP 到原点的距离为 rrr，点 PPP 与 x 轴的夹角为 θ\thetaθ（从 x 轴逆时针量测），则有：
x=rcos⁡θy=rsin⁡θ \begin{aligned} x &= r\cos\theta \\ y &= r\sin\theta \end{aligned} xy=rcosθ=rsinθ

当坐标系绕 z 轴顺时针 旋转 γ\gammaγ 角后，x 轴向 y 轴负方向转动，点 PPP 相对于新坐标系 x' 轴的夹角变为 θ+γ\theta + \gammaθ+γ（坐标系转离了点，夹角变大）。因此新坐标为：
x′=rcos⁡(θ+γ)=r(cos⁡θcos⁡γ−sin⁡θsin⁡γ)=xcos⁡γ−ysin⁡γy′=rsin⁡(θ+γ)=r(sin⁡θcos⁡γ+cos⁡θsin⁡γ)=ycos⁡γ+xsin⁡γ \begin{aligned} x' &= r\cos(\theta + \gamma) = r(\cos\theta\cos\gamma - \sin\theta\sin\gamma) = x\cos\gamma - y\sin\gamma \\ y' &= r\sin(\theta + \gamma) = r(\sin\theta\cos\gamma + \cos\theta\sin\gamma) = y\cos\gamma + x\sin\gamma \end{aligned} x′y′=rcos(θ+γ)=r(cosθcosγ−sinθsinγ)=xcosγ−ysinγ=rsin(θ+γ)=r(sinθcosγ+cosθsinγ)=ycosγ+xsinγ

因此，绕 z 轴旋转的变换矩阵为：
Rz(γ)=cos⁡γ−sin⁡γ0sin⁡γcos⁡γ0001 R_z(\gamma) = \begin{bmatrix} \cos\gamma & -\sin\gamma & 0 \\ \sin\gamma & \cos\gamma & 0 \\ 0 & 0 & 1 \end{bmatrix} Rz(γ)= cosγsinγ0−sinγcosγ0001

变换公式为：
x′y′z′=cos⁡γ−sin⁡γ0sin⁡γcos⁡γ0001xyz \begin{bmatrix} x' \\ y' \\ z' \end{bmatrix} = \begin{bmatrix} \cos\gamma & -\sin\gamma & 0 \\ \sin\gamma & \cos\gamma & 0 \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} x \\ y \\ z \end{bmatrix} x′y′z′ = cosγsinγ0−sinγcosγ0001 xyz

1.1.2 组合旋转矩阵

综合上述步骤，按照旋转顺序（先绕 x 轴，再绕 y 轴，最后绕 z 轴），组合旋转矩阵为：
R=Rz(γ)⋅Ry(β)⋅Rx(α) R = R_z(\gamma) \cdot R_y(\beta) \cdot R_x(\alpha) R=Rz(γ)⋅Ry(β)⋅Rx(α)

详细计算过程：
R=cos⁡γ−sin⁡γ0sin⁡γcos⁡γ0001⋅cos⁡β0sin⁡β010−sin⁡β0cos⁡β⋅1000cos⁡α−sin⁡α0sin⁡αcos⁡α=cos⁡γ−sin⁡γ0sin⁡γcos⁡γ0001⋅cos⁡βsin⁡βsin⁡αsin⁡βcos⁡α0cos⁡α−sin⁡α−sin⁡βcos⁡βsin⁡αcos⁡βcos⁡α=cos⁡βcos⁡γ−cos⁡αsin⁡γ+sin⁡αsin⁡βcos⁡γsin⁡αsin⁡γ+cos⁡αsin⁡βcos⁡γcos⁡βsin⁡γcos⁡αcos⁡γ+sin⁡αsin⁡βsin⁡γsin⁡αcos⁡γ−cos⁡αsin⁡βsin⁡γ−sin⁡βsin⁡αcos⁡βcos⁡αcos⁡β \begin{aligned} R &= \begin{bmatrix} \cos\gamma & -\sin\gamma & 0 \\ \sin\gamma & \cos\gamma & 0 \\ 0 & 0 & 1 \end{bmatrix} \cdot \begin{bmatrix} \cos\beta & 0 & \sin\beta \\ 0 & 1 & 0 \\ -\sin\beta & 0 & \cos\beta \end{bmatrix} \cdot \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos\alpha & -\sin\alpha \\ 0 & \sin\alpha & \cos\alpha \end{bmatrix} \\ &= \begin{bmatrix} \cos\gamma & -\sin\gamma & 0 \\ \sin\gamma & \cos\gamma & 0 \\ 0 & 0 & 1 \end{bmatrix} \cdot \begin{bmatrix} \cos\beta & \sin\beta\sin\alpha & \sin\beta\cos\alpha \\ 0 & \cos\alpha & -\sin\alpha \\ -\sin\beta & \cos\beta\sin\alpha & \cos\beta\cos\alpha \end{bmatrix} \\ &= \begin{bmatrix} \cos\beta\cos\gamma & -\cos\alpha\sin\gamma + \sin\alpha\sin\beta\cos\gamma & \sin\alpha\sin\gamma + \cos\alpha\sin\beta\cos\gamma \\ \cos\beta\sin\gamma & \cos\alpha\cos\gamma + \sin\alpha\sin\beta\sin\gamma & \sin\alpha\cos\gamma - \cos\alpha\sin\beta\sin\gamma \\ -\sin\beta & \sin\alpha\cos\beta & \cos\alpha\cos\beta \end{bmatrix} \end{aligned} R= cosγsinγ0−sinγcosγ0001 ⋅ cosβ0−sinβ010sinβ0cosβ ⋅ 1000cosαsinα0−sinαcosα = cosγsinγ0−sinγcosγ0001 ⋅ cosβ0−sinβsinβsinαcosαcosβsinαsinβcosα−sinαcosβcosα = cosβcosγcosβsinγ−sinβ−cosαsinγ+sinαsinβcosγcosαcosγ+sinαsinβsinγsinαcosβsinαsinγ+cosαsinβcosγsinαcosγ−cosαsinβsinγcosαcosβ

1.1.3 完整坐标转换公式

设两坐标系 o−x′y′z′o-x'y'z'o−x′y′z′ 与 O−XYZO-XYZO−XYZ 的尺度因子为 mmm，平移向量为 Δ=ΔX,ΔY,ΔZT\Delta = \\Delta X, \\Delta Y, \\Delta Z^TΔ=ΔX,ΔY,ΔZT，则可得到转换模型：
XYZ=m⋅R⋅xyz+ΔXΔYΔZ \begin{bmatrix} X \\ Y \\ Z \end{bmatrix} = m \cdot R \cdot \begin{bmatrix} x \\ y \\ z \end{bmatrix} + \begin{bmatrix} \Delta X \\ \Delta Y \\ \Delta Z \end{bmatrix} XYZ =m⋅R⋅ xyz + ΔXΔYΔZ

其中旋转矩阵 RRR 展开为：
R=cos⁡βcos⁡γ−cos⁡αsin⁡γ+sin⁡αsin⁡βcos⁡γsin⁡αsin⁡γ+cos⁡αsin⁡βcos⁡γcos⁡βsin⁡γcos⁡αcos⁡γ+sin⁡αsin⁡βsin⁡γsin⁡αcos⁡γ−cos⁡αsin⁡βsin⁡γ−sin⁡βsin⁡αcos⁡βcos⁡αcos⁡β R = \begin{bmatrix} \cos\beta\cos\gamma & -\cos\alpha\sin\gamma + \sin\alpha\sin\beta\cos\gamma & \sin\alpha\sin\gamma + \cos\alpha\sin\beta\cos\gamma \\ \cos\beta\sin\gamma & \cos\alpha\cos\gamma + \sin\alpha\sin\beta\sin\gamma & \sin\alpha\cos\gamma - \cos\alpha\sin\beta\sin\gamma \\ -\sin\beta & \sin\alpha\cos\beta & \cos\alpha\cos\beta \end{bmatrix} R= cosβcosγcosβsinγ−sinβ−cosαsinγ+sinαsinβcosγcosαcosγ+sinαsinβsinγsinαcosβsinαsinγ+cosαsinβcosγsinαcosγ−cosαsinβsinγcosαcosβ

上式简化为一般式：
XYZ=r11r12r13r21r22r23r31r32r33xyz+ΔXΔYΔZ(1) \begin{bmatrix} X \\ Y \\ Z \end{bmatrix} = \begin{bmatrix} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{bmatrix} \begin{bmatrix} x \\ y \\ z \end{bmatrix} + \begin{bmatrix} \Delta X \\ \Delta Y \\ \Delta Z \end{bmatrix} \tag{1} XYZ = r11r21r31r12r22r32r13r23r33 xyz + ΔXΔYΔZ (1)

其中，mmm 为尺度因子，RRR 为旋转矩阵与两坐标系之间的角度有关，Δ\DeltaΔ 为平移向量与两坐标系原点之间的距离有关。

此时只需要求出 r11r_{11}r11、r12r_{12}r12、r13r_{13}r13、r21r_{21}r21、r22r_{22}r22、r23r_{23}r23、r31r_{31}r31、r32r_{32}r32、r33r_{33}r33、ΔX\Delta XΔX、ΔY\Delta YΔY、ΔZ\Delta ZΔZ、mmm 就得到两空间直角坐标系之间的转换参数。其中旋转矩阵元素满足以下关系：
r11=cos⁡βcos⁡γr12=−cos⁡αsin⁡γ+sin⁡αsin⁡βcos⁡γr13=sin⁡αsin⁡γ+cos⁡αsin⁡βcos⁡γr21=cos⁡βsin⁡γr22=cos⁡αcos⁡γ+sin⁡αsin⁡βsin⁡γr23=sin⁡αcos⁡γ−cos⁡αsin⁡βsin⁡γr31=−sin⁡βr32=sin⁡αcos⁡βr33=cos⁡αcos⁡β \begin{aligned} r_{11} &= \cos\beta\cos\gamma \\ r_{12} &= -\cos\alpha\sin\gamma + \sin\alpha\sin\beta\cos\gamma \\ r_{13} &= \sin\alpha\sin\gamma + \cos\alpha\sin\beta\cos\gamma \\ r_{21} &= \cos\beta\sin\gamma \\ r_{22} &= \cos\alpha\cos\gamma + \sin\alpha\sin\beta\sin\gamma \\ r_{23} &= \sin\alpha\cos\gamma - \cos\alpha\sin\beta\sin\gamma \\ r_{31} &= -\sin\beta \\ r_{32} &= \sin\alpha\cos\beta \\ r_{33} &= \cos\alpha\cos\beta \end{aligned} r11r12r13r21r22r23r31r32r33=cosβcosγ=−cosαsinγ+sinαsinβcosγ=sinαsinγ+cosαsinβcosγ=cosβsinγ=cosαcosγ+sinαsinβsinγ=sinαcosγ−cosαsinβsinγ=−sinβ=sinαcosβ=cosαcosβ

在实际控制点的测量坐标系中有对应点的坐标值，采用最小二乘法进行解算。但传统的布尔沙模型只适用于小角度的转换，没有考虑到两坐标系之间是存在一个大角度关系的。在最小二乘法的解算过程中还应使用约束条件。

式（1）中各旋转矩阵元素有如下关系（正交约束）：
r112+r122+r132=1r212+r222+r232=1r312+r322+r332=1r11r21+r12r22+r13r23=0r11r31+r12r32+r13r33=0r21r31+r22r32+r23r33=0(2) \begin{aligned} r_{11}^2 + r_{12}^2 + r_{13}^2 &= 1 \\ r_{21}^2 + r_{22}^2 + r_{23}^2 &= 1 \\ r_{31}^2 + r_{32}^2 + r_{33}^2 &= 1 \\ r_{11}r_{21} + r_{12}r_{22} + r_{13}r_{23} &= 0 \\ r_{11}r_{31} + r_{12}r_{32} + r_{13}r_{33} &= 0 \\ r_{21}r_{31} + r_{22}r_{32} + r_{23}r_{33} &= 0 \end{aligned} \tag{2} r112+r122+r132r212+r222+r232r312+r322+r332r11r21+r12r22+r13r23r11r31+r12r32+r13r33r21r31+r22r32+r23r33=1=1=1=0=0=0(2)

所以，只需要知道两坐标系的控制点就可以对式（1）列出 3n3n3n 个方程，再加约束条件方程，方程组可解出上面 13 个参数。

1.2 约束最小二乘法解算

采用最小二乘法解算上述方程组，能够有效的减小误差。由于式（2）中包含 9 个元素且存在2次方参数，因此需要将非线性方程进行线性化。首先，采用泰勒级数展开的方法进行线性化。

设 r11,r12,r13,r21,r22,r23,r31,r32,r33,ΔX,ΔY,ΔZ,mr_{11}, r_{12}, r_{13}, r_{21}, r_{22}, r_{23}, r_{31}, r_{32}, r_{33}, \Delta X, \Delta Y, \Delta Z, mr11,r12,r13,r21,r22,r23,r31,r32,r33,ΔX,ΔY,ΔZ,m 为待求参数，取初始近似值 r110,r120,r130,r210,r220,r230,r310,r320,r330,ΔX0,ΔY0,ΔZ0,m0r_{11}^0, r_{12}^0, r_{13}^0, r_{21}^0, r_{22}^0, r_{23}^0, r_{31}^0, r_{32}^0, r_{33}^0, \Delta X^0, \Delta Y^0, \Delta Z^0, m^0r110,r120,r130,r210,r220,r230,r310,r320,r330,ΔX0,ΔY0,ΔZ0,m0，则参数可表示为：

r11=r110+dr11,r12=r120+dr12,r13=r130+dr13r21=r210+dr21,r22=r220+dr22,r23=r230+dr23r31=r310+dr31,r32=r320+dr32,r33=r330+dr33ΔX=ΔX0+dΔX,ΔY=ΔY0+dΔY,ΔZ=ΔZ0+dΔZm=m0+dm \begin{aligned} r_{11} &= r_{11}^0 + dr_{11}, & r_{12} &= r_{12}^0 + dr_{12}, & r_{13} &= r_{13}^0 + dr_{13} \\ r_{21} &= r_{21}^0 + dr_{21}, & r_{22} &= r_{22}^0 + dr_{22}, & r_{23} &= r_{23}^0 + dr_{23} \\ r_{31} &= r_{31}^0 + dr_{31}, & r_{32} &= r_{32}^0 + dr_{32}, & r_{33} &= r_{33}^0 + dr_{33} \\ \Delta X &= \Delta X^0 + d\Delta X, & \Delta Y &= \Delta Y^0 + d\Delta Y, & \Delta Z &= \Delta Z^0 + d\Delta Z \\ m &= m^0 + dm \end{aligned} r11r21r31ΔXm=r110+dr11,=r210+dr21,=r310+dr31,=ΔX0+dΔX,=m0+dmr12r22r32ΔY=r120+dr12,=r220+dr22,=r320+dr32,=ΔY0+dΔY,r13r23r33ΔZ=r130+dr13=r230+dr23=r330+dr33=ΔZ0+dΔZ

其中 dr11,dr12,...,dmdr_{11}, dr_{12}, \dots, dmdr11,dr12,...,dm 为待求的改正数。将式（1）在初始值处展开，取一次项则有：

XYZ=X0Y0Z0+∂X∂r11∂X∂r12∂X∂r13∂X∂r21∂X∂r22∂X∂r23∂X∂r31∂X∂r32∂X∂r33∂X∂ΔX∂X∂ΔY∂X∂ΔZ∂X∂m∂Y∂r11∂Y∂r12∂Y∂r13∂Y∂r21∂Y∂r22∂Y∂r23∂Y∂r31∂Y∂r32∂Y∂r33∂Y∂ΔX∂Y∂ΔY∂Y∂ΔZ∂Y∂m∂Z∂r11∂Z∂r12∂Z∂r13∂Z∂r21∂Z∂r22∂Z∂r23∂Z∂r31∂Z∂r32∂Z∂r33∂Z∂ΔX∂Z∂ΔY∂Z∂ΔZ∂Z∂mdr11dr12dr13dr21dr22dr23dr31dr32dr33dΔXdΔYdΔZdm \begin{bmatrix} X \\ Y \\ Z \end{bmatrix} = \begin{bmatrix} X_0 \\ Y_0 \\ Z_0 \end{bmatrix} + \begin{bmatrix} \frac{\partial X}{\partial r_{11}} & \frac{\partial X}{\partial r_{12}} & \frac{\partial X}{\partial r_{13}} & \frac{\partial X}{\partial r_{21}} & \frac{\partial X}{\partial r_{22}} & \frac{\partial X}{\partial r_{23}} & \frac{\partial X}{\partial r_{31}} & \frac{\partial X}{\partial r_{32}} & \frac{\partial X}{\partial r_{33}} & \frac{\partial X}{\partial \Delta X} & \frac{\partial X}{\partial \Delta Y} & \frac{\partial X}{\partial \Delta Z} & \frac{\partial X}{\partial m} \\ \frac{\partial Y}{\partial r_{11}} & \frac{\partial Y}{\partial r_{12}} & \frac{\partial Y}{\partial r_{13}} & \frac{\partial Y}{\partial r_{21}} & \frac{\partial Y}{\partial r_{22}} & \frac{\partial Y}{\partial r_{23}} & \frac{\partial Y}{\partial r_{31}} & \frac{\partial Y}{\partial r_{32}} & \frac{\partial Y}{\partial r_{33}} & \frac{\partial Y}{\partial \Delta X} & \frac{\partial Y}{\partial \Delta Y} & \frac{\partial Y}{\partial \Delta Z} & \frac{\partial Y}{\partial m} \\ \frac{\partial Z}{\partial r_{11}} & \frac{\partial Z}{\partial r_{12}} & \frac{\partial Z}{\partial r_{13}} & \frac{\partial Z}{\partial r_{21}} & \frac{\partial Z}{\partial r_{22}} & \frac{\partial Z}{\partial r_{23}} & \frac{\partial Z}{\partial r_{31}} & \frac{\partial Z}{\partial r_{32}} & \frac{\partial Z}{\partial r_{33}} & \frac{\partial Z}{\partial \Delta X} & \frac{\partial Z}{\partial \Delta Y} & \frac{\partial Z}{\partial \Delta Z} & \frac{\partial Z}{\partial m} \end{bmatrix} \begin{bmatrix} dr_{11} \\ dr_{12} \\ dr_{13} \\ dr_{21} \\ dr_{22} \\ dr_{23} \\ dr_{31} \\ dr_{32} \\ dr_{33} \\ d\Delta X \\ d\Delta Y \\ d\Delta Z \\ dm \end{bmatrix} XYZ = X0Y0Z0 + ∂r11∂X∂r11∂Y∂r11∂Z∂r12∂X∂r12∂Y∂r12∂Z∂r13∂X∂r13∂Y∂r13∂Z∂r21∂X∂r21∂Y∂r21∂Z∂r22∂X∂r22∂Y∂r22∂Z∂r23∂X∂r23∂Y∂r23∂Z∂r31∂X∂r31∂Y∂r31∂Z∂r32∂X∂r32∂Y∂r32∂Z∂r33∂X∂r33∂Y∂r33∂Z∂ΔX∂X∂ΔX∂Y∂ΔX∂Z∂ΔY∂X∂ΔY∂Y∂ΔY∂Z∂ΔZ∂X∂ΔZ∂Y∂ΔZ∂Z∂m∂X∂m∂Y∂m∂Z dr11dr12dr13dr21dr22dr23dr31dr32dr33dΔXdΔYdΔZdm

式中 D=X,Y,ZTD = X, Y, Z^TD=X,Y,ZT 为观测值向量，C=X0,Y0,Z0TC = X_0, Y_0, Z_0^TC=X0,Y0,Z0T 为用初始值计算的近似值，可以列出误差方程如下：

F=D−(C+AV′)(3) F = D - (C + A V') \tag{3} F=D−(C+AV′)(3)

式中系数矩阵 AAA 为：

A=∂X∂r11∂X∂r12∂X∂r13∂X∂r21∂X∂r22∂X∂r23∂X∂r31∂X∂r32∂X∂r33∂X∂ΔX∂X∂ΔY∂X∂ΔZ∂X∂m∂Y∂r11∂Y∂r12∂Y∂r13∂Y∂r21∂Y∂r22∂Y∂r23∂Y∂r31∂Y∂r32∂Y∂r33∂Y∂ΔX∂Y∂ΔY∂Y∂ΔZ∂Y∂m∂Z∂r11∂Z∂r12∂Z∂r13∂Z∂r21∂Z∂r22∂Z∂r23∂Z∂r31∂Z∂r32∂Z∂r33∂Z∂ΔX∂Z∂ΔY∂Z∂ΔZ∂Z∂mV0 A = \begin{bmatrix} \frac{\partial X}{\partial r_{11}} & \frac{\partial X}{\partial r_{12}} & \frac{\partial X}{\partial r_{13}} & \frac{\partial X}{\partial r_{21}} & \frac{\partial X}{\partial r_{22}} & \frac{\partial X}{\partial r_{23}} & \frac{\partial X}{\partial r_{31}} & \frac{\partial X}{\partial r_{32}} & \frac{\partial X}{\partial r_{33}} & \frac{\partial X}{\partial \Delta X} & \frac{\partial X}{\partial \Delta Y} & \frac{\partial X}{\partial \Delta Z} & \frac{\partial X}{\partial m} \\ \frac{\partial Y}{\partial r_{11}} & \frac{\partial Y}{\partial r_{12}} & \frac{\partial Y}{\partial r_{13}} & \frac{\partial Y}{\partial r_{21}} & \frac{\partial Y}{\partial r_{22}} & \frac{\partial Y}{\partial r_{23}} & \frac{\partial Y}{\partial r_{31}} & \frac{\partial Y}{\partial r_{32}} & \frac{\partial Y}{\partial r_{33}} & \frac{\partial Y}{\partial \Delta X} & \frac{\partial Y}{\partial \Delta Y} & \frac{\partial Y}{\partial \Delta Z} & \frac{\partial Y}{\partial m} \\ \frac{\partial Z}{\partial r_{11}} & \frac{\partial Z}{\partial r_{12}} & \frac{\partial Z}{\partial r_{13}} & \frac{\partial Z}{\partial r_{21}} & \frac{\partial Z}{\partial r_{22}} & \frac{\partial Z}{\partial r_{23}} & \frac{\partial Z}{\partial r_{31}} & \frac{\partial Z}{\partial r_{32}} & \frac{\partial Z}{\partial r_{33}} & \frac{\partial Z}{\partial \Delta X} & \frac{\partial Z}{\partial \Delta Y} & \frac{\partial Z}{\partial \Delta Z} & \frac{\partial Z}{\partial m} \end{bmatrix}_{V_0} A= ∂r11∂X∂r11∂Y∂r11∂Z∂r12∂X∂r12∂Y∂r12∂Z∂r13∂X∂r13∂Y∂r13∂Z∂r21∂X∂r21∂Y∂r21∂Z∂r22∂X∂r22∂Y∂r22∂Z∂r23∂X∂r23∂Y∂r23∂Z∂r31∂X∂r31∂Y∂r31∂Z∂r32∂X∂r32∂Y∂r32∂Z∂r33∂X∂r33∂Y∂r33∂Z∂ΔX∂X∂ΔX∂Y∂ΔX∂Z∂ΔY∂X∂ΔY∂Y∂ΔY∂Z∂ΔZ∂X∂ΔZ∂Y∂ΔZ∂Z∂m∂X∂m∂Y∂m∂Z V0

其中 V′=dr11,dr12,dr13,dr21,dr22,dr23,dr31,dr32,dr33,dΔX,dΔY,dΔZ,dmTV' = dr_{11}, dr_{12}, dr_{13}, dr_{21}, dr_{22}, dr_{23}, dr_{31}, dr_{32}, dr_{33}, d\\Delta X, d\\Delta Y, d\\Delta Z, dm^TV′=dr11,dr12,dr13,dr21,dr22,dr23,dr31,dr32,dr33,dΔX,dΔY,dΔZ,dmT。

由式（1）可知偏导数为：

∂X∂r11=x,∂X∂r12=y,∂X∂r13=z,∂X∂ΔX=1,∂X∂m=r11x+r12y+r13z∂Y∂r21=x,∂Y∂r22=y,∂Y∂r23=z,∂Y∂ΔY=1,∂Y∂m=r21x+r22y+r23z∂Z∂r31=x,∂Z∂r32=y,∂Z∂r33=z,∂Z∂ΔZ=1,∂Z∂m=r31x+r32y+r33z \begin{aligned} \frac{\partial X}{\partial r_{11}} &= x, & \frac{\partial X}{\partial r_{12}} &= y, & \frac{\partial X}{\partial r_{13}} &= z, & \frac{\partial X}{\partial \Delta X} &= 1, & \frac{\partial X}{\partial m} &= r_{11}x + r_{12}y + r_{13}z \\ \frac{\partial Y}{\partial r_{21}} &= x, & \frac{\partial Y}{\partial r_{22}} &= y, & \frac{\partial Y}{\partial r_{23}} &= z, & \frac{\partial Y}{\partial \Delta Y} &= 1, & \frac{\partial Y}{\partial m} &= r_{21}x + r_{22}y + r_{23}z \\ \frac{\partial Z}{\partial r_{31}} &= x, & \frac{\partial Z}{\partial r_{32}} &= y, & \frac{\partial Z}{\partial r_{33}} &= z, & \frac{\partial Z}{\partial \Delta Z} &= 1, & \frac{\partial Z}{\partial m} &= r_{31}x + r_{32}y + r_{33}z \end{aligned} ∂r11∂X∂r21∂Y∂r31∂Z=x,=x,=x,∂r12∂X∂r22∂Y∂r32∂Z=y,=y,=y,∂r13∂X∂r23∂Y∂r33∂Z=z,=z,=z,∂ΔX∂X∂ΔY∂Y∂ΔZ∂Z=1,=1,=1,∂m∂X∂m∂Y∂m∂Z=r11x+r12y+r13z=r21x+r22y+r23z=r31x+r32y+r33z

其余偏导数均为 0。

对式（2）在初始值处进行泰勒展开，得到误差方程：

F′=E−(N+GV′)(4) F' = E - (N + G V') \tag{4} F′=E−(N+GV′)(4)

其中 E=0,0,0,0,0,0TE = 0, 0, 0, 0, 0, 0^TE=0,0,0,0,0,0T，NNN 为用初始值计算的约束条件残差：

N=(r110)2+(r120)2+(r130)2−1(r210)2+(r220)2+(r230)2−1(r310)2+(r320)2+(r330)2−1r110r210+r120r220+r130r230r110r310+r120r320+r130r330r210r310+r220r320+r230r330 N = \begin{bmatrix} (r_{11}^0)^2 + (r_{12}^0)^2 + (r_{13}^0)^2 - 1 \\ (r_{21}^0)^2 + (r_{22}^0)^2 + (r_{23}^0)^2 - 1 \\ (r_{31}^0)^2 + (r_{32}^0)^2 + (r_{33}^0)^2 - 1 \\ r_{11}^0 r_{21}^0 + r_{12}^0 r_{22}^0 + r_{13}^0 r_{23}^0 \\ r_{11}^0 r_{31}^0 + r_{12}^0 r_{32}^0 + r_{13}^0 r_{33}^0 \\ r_{21}^0 r_{31}^0 + r_{22}^0 r_{32}^0 + r_{23}^0 r_{33}^0 \end{bmatrix} N= (r110)2+(r120)2+(r130)2−1(r210)2+(r220)2+(r230)2−1(r310)2+(r320)2+(r330)2−1r110r210+r120r220+r130r230r110r310+r120r320+r130r330r210r310+r220r320+r230r330

GGG 为约束条件对参数的偏导数矩阵，由于约束条件只与旋转矩阵元素有关，因此 GGG 中只有对应 rijr_{ij}rij 的列有值，其余列为 0。

选取两坐标系的 nnn（n≥4n \ge 4n≥4）个以上对应点，能够得到包含 3n+63n+63n+6 个方程的法方程组，从而可以求解出 V′V'V′。将解出的改正数与初始值相加作为新的初始值，重复上述解算过程，直到两次迭代所得未知参数的差值足够小时就得到参数的最优值。

2 算例分析

为验证算法的可行性，设置两个不同的空间直角坐标系，采集 10 个控制点的坐标，其坐标值分别如表 1 和表 2 所示。表 1 为在空间直角坐标系 O−XYZO-XYZO−XYZ 中测量的坐标，表 2 中的坐标为在空间直角坐标系 o−xyzo-xyzo−xyz 中测量的坐标。现通过坐标转换将空间直角坐标系 o−xyzo-xyzo−xyz 转换到空间直角坐标系 O−XYZO-XYZO−XYZ 的转换参数。

表 1 坐标系 O-XYZ 中测量的 10 个控制点坐标值

点号	X	Y	Z
1	-359.392	2296.548	-1050.287
2	714.311	3265.226	-25.312
3	-1866.682	4376.503	36.113
4	-519.674	5105.979	897.387
5	-672.930	-2137.437	-1047.305
6	1438.919	7906.840	149.080
7	-3819.509	8420.439	867.560
8	-9527.940	5080.401	-664.500
9	-6350.206	-2588.080	1547.745
10	-3939.282	1399.917	11.490

表 2 坐标系 o-xyz 中测量的 10 个控制点坐标值

点号	x	y	z
1	-2971.729	-2824.222	-988.354
2	-3230.100	-1401.401	36.621
3	-5533.626	-3010.820	98.046
4	-5444.748	-1481.550	959.320
5	633.094	-5424.949	-985.372
6	-6795.057	1658.127	211.013
7	-9999.886	-2542.339	929.493
8	-10165.155	-9154.050	-602.567
9	-1972.405	-10489.129	1609.678
10	-4093.927	-6339.934	73.423

计算结果

先将表 1、表 2 中的数据代入转换式（1）中，列出 30 个方程。将这 30 个方程进行泰勒级数展开，取近似值 r110=1,r120=0,r130=0,r210=0,r220=1,r230=0,r310=0,r320=0,r330=1,ΔX0=0,ΔY0=0,ΔZ0=0,m0=1r_{11}^0=1, r_{12}^0=0, r_{13}^0=0, r_{21}^0=0, r_{22}^0=1, r_{23}^0=0, r_{31}^0=0, r_{32}^0=0, r_{33}^0=1, \Delta X_0=0, \Delta Y_0=0, \Delta Z_0=0, m_0=1r110=1,r120=0,r130=0,r210=0,r220=1,r230=0,r310=0,r320=0,r330=1,ΔX0=0,ΔY0=0,ΔZ0=0,m0=1，代入式（3）的误差方程中，得到 30 个关于未知参数 dr11,dr12,dr13,dr21,dr22,dr23,dr31,dr32,dr33,dΔX,dΔY,dΔZ,dmdr_{11}, dr_{12}, dr_{13}, dr_{21}, dr_{22}, dr_{23}, dr_{31}, dr_{32}, dr_{33}, d\Delta X, d\Delta Y, d\Delta Z, dmdr11,dr12,dr13,dr21,dr22,dr23,dr31,dr32,dr33,dΔX,dΔY,dΔZ,dm 的法方程。然后将式（2）进行泰勒级数展开，取近似值代入，得到式（4）的误差方程并进行解算，得到 6 个关于上述未知数的法方程。将以上 36 个方程用最小二乘法进行解算，解算出未知参数的改正数。将改正数与初始值相加作为新的初始值重复上述解算过程，直到两次迭代所得未知参数的差值小于 0.001 为止。

通过 C++ 编程可以得到由表 1 和表 2 中的 10 个控制点计算的空间直角坐标系 o−xyzo-xyzo−xyz 到空间直角坐标系 O−XYZO-XYZO−XYZ 的转换参数为：

ΔXΔYΔZ=3606.2123111256.649714−61.933000 \begin{bmatrix} \Delta X \\ \Delta Y \\ \Delta Z \end{bmatrix} = \begin{bmatrix} 3606.212311 \\ 1256.649714 \\ -61.933000 \end{bmatrix} ΔXΔYΔZ = 3606.2123111256.649714−61.933000

R=r11r12r13r21r22r23r31r32r33=0.5264230.8502230−0.8502230.5264230001 R = \begin{bmatrix} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{bmatrix} = \begin{bmatrix} 0.526423 & 0.850223 & 0 \\ -0.850223 & 0.526423 & 0 \\ 0 & 0 & 1 \end{bmatrix} R= r11r21r31r12r22r32r13r23r33 = 0.526423−0.85022300.8502230.5264230001

m=1 m = 1 m=1

将上述参数代入转换式（1）进行验证，残差均小于 0.001。计算结果如图所示。

计算结果示意图