CN104048645A - 线性拟合地面扫描点云整体定向方法 - Google Patents

Abstract

本发明涉及一种线性拟合地面扫描点云整体定向方法，其特征在于采用以下步骤：1)在测量区域网内，布设球形定向标靶，进行多站扫描，获取扫描点云数据；2)将各扫描站和在扫描站上能观测到的球形定向标靶连线，构成区域网；3)拟合球形标靶球心在扫描仪坐标系中的坐标；4)整理平差信息；5)列立单激光束的数学模型，推导同名激光束的线性拟合数学模型，评估点云定向的点位误差，将点云坐标转换到工程测量指定的坐标系，实现点云整体定向。本发明不需要计算参数近似值，直接进行拟合求解参数、公式简单，扫描站越多其解算精度高的优势越明显。

Description

线性拟合地面扫描点云整体定向方法

技术领域

本发明提供一种线性拟合地面扫描点云整体定向方法，属于工程测量领域。 

背景技术

地面三维激光扫描仪扫描速度越来越快，由早期的0.5万点/秒提高到100万点/秒，每天在野外扫描的站数可达50站甚至上百站，如何快速将扫描设备坐标系中的点云转换到工程指定的测量坐标系则成为一个关键技术。确定扫描时设备在工程测量坐标系的位置点和姿态的过程称为点云定向，也就是确定扫描设备坐标系与工程测量坐标系的坐标转换参数的过程。点云定向是三维建模、地形测量等后续工作的基础，没有快速准确的点云定向，扫描数据处理将滞后于野外测量，点云定向依然是三维激光扫描技术亟待解决的瓶颈。对于小范围地形扫描，用点云拼接方法将多站点云先统一到一个坐标系中(目前多数软件都能实现自动拼接，但精度不高)，然后用3个以上的控制点，将拼接后点云坐标转换到工程测量坐标系中，然而对于每站扫描的2000万以上的点(2分钟点云采集数量)，内存为2G的普通计算机只能拼接2站数据，在实际的测绘中不能用先拼接后转换方法实现点云定向。目前对于大型建筑物、地形测量的扫描点云定向方法，国内外大部分用独立模型法进行点云定向，还有少数采用约束条件，如对大物体扫描一周，产生闭合条件(张剑清等)、在相邻站布设连接标靶提高拼接精度。独立模型法定向相对于连续拼接(ICP法)精度较高、标靶分布合理、容易操作，但有以下几点不足：①外业工作量大。②用相邻各站参数计算的其公共标靶中心的坐标相互有差异，是相互矛盾的，理论上是不严密的。③各站精度不均匀，差异较大，精度评价时缺乏整体说服力。现有独立模型数学模型为 

$\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]=\left[\begin{array}{c}{X}_S\\ Y_S\\ Z_S\end{array}\right]+R\left[\begin{array}{c}x\\ y\\ z\end{array}\right],R=\left[\begin{array}{ccc}{a}_1& a_2& a_3\\ b_1& b_2& b_3\\ c_1& c_2& c_3\end{array}\right]$

这是一个非线性方程，平差时未知数参数选择有三种情况：(1)以3个平移量3个旋转角为平差参数进行间接平差。(2)以3个平移量和旋转矩阵R中的9个元素为平差参数按附有限制条件的间接平差计算坐标转换参数。(3)以3个平移量和反对称矩阵3个元素为平差参数进行间接平差。以上3种平差方法均需求解平差参数近似值，然后线性求解。 

发明内容

本发明的目的是提供一种能克服上述缺陷、不用计算参数近似值、直接进行拟合求解参数、公式简单的线性拟合地面扫描点云整体定向方法。其技术方案为： 

一种线性拟合地面扫描点云整体定向方法，其特征在于采用以下步骤： 

1)在测量区域网内，布设球形定向标靶，进行多站扫描，获取扫描点云数据； 

2)将各扫描站和在扫描站上能观测到的球形定向标靶连线，构成区域网； 

3)拟合球形标靶球心在扫描仪坐标系中的坐标； 

4)整理平差信息； 

5)列立单激光束的数学模型，推导同名激光束的线性拟合数学模型，评估点云定向的点位误差，将点云坐标转换到工程测量指定的坐标系，实现点云整体定向。 

所述的线性拟合地面扫描点云整体定向方法，步骤4)中涉及的整理平差的信息包括定向标靶个数和重复标靶个数；若全区域定向标靶有n₁个单站标靶和n₂个公共标靶，所有单光束的拟合方程有n₁+n₂组，每组3个方程，共3(n₁+n₂)个拟合方程；全区域有n个激光束，约束方程组数为n₃＝n-n₁-n₂，约束条件方程数为3n₃；方程列立时，每一个标靶上先列组拟合方程，如果k标靶还有N-1个同名激光束，再列N-1组约束方程。 

所述的线性拟合地面扫描点云整体定向方法，步骤5)实现过程如下： 

根据公式 $\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]=\left[\begin{array}{c}{X}_S\\ Y_S\\ Z_S\end{array}\right]+R\left[\begin{array}{c}x\\ y\\ z\end{array}\right],$ 其中 $R=\left[\begin{array}{ccc}{a}_1& a_2& a_3\\ b_1& b_2& b_3\\ c_1& c_2& c_3\end{array}\right]---\left(1\right)$

R矩阵反映了扫描时的点云在指定坐标系的姿态，可由反对称矩阵 $S=\left[\begin{array}{ccc}0& -c& -b\\ c& 0& -a\\ b& a& 0\end{array}\right]$ 构成，R与S及单位矩阵I的关系为 

(1)S^T＝-S,Ρ＝(I+S)(I-S)^-1

(2)(I-S)^T＝I+S,(I+S)^T＝I-S 

(3)R^T＝R^-1＝(I+S)^-1(I-S)  (2) 

${(I+S)}^{-1}=\frac{1}{2}(I+R^{-1}),S=2{(I+R^{-1})}^{-1}-I---\left(4\right)$

只有一个扫描站的激光束射向的标靶称为单标靶，多站扫描激光束射向同一标靶，此时，各站的激光束称为同名光束，该标靶称为站公共标靶； 

单标靶方程式推导：依公式(1)，i扫描站指向单标靶h的6参数坐标转换方程为 

$\left[\begin{array}{c}{X}_h\\ Y_h\\ Z_h\end{array}\right]=\left[\begin{array}{c}{X}_{\mathrm{Si}}\\ Y_{\mathrm{Si}}\\ Z_{\mathrm{Si}}\end{array}\right]-R\left[\begin{array}{c}{x}_{\mathrm{ih}}\\ y_{\mathrm{ih}}\\ z_{\mathrm{ih}}\end{array}\right]---\left(3\right)$

公式(3)两边同乘以

$R^{-1}\left[\begin{array}{c}{X}_h\\ Y_h\\ Z_h\end{array}\right]=R^{-1}\left[\begin{array}{c}{X}_{\mathrm{Si}}\\ Y_{\mathrm{Si}}\\ Z_{\mathrm{Si}}\end{array}\right]-\left[\begin{array}{c}{x}_{\mathrm{ih}}\\ y_{\mathrm{ih}}\\ z_{\mathrm{ih}}\end{array}\right]---\left(4\right)$

公式(4)两边同乘以

$(I-S_i)\left[\begin{array}{c}{X}_h\\ Y_h\\ Z_h\end{array}\right]=(I-S_i)\left[\begin{array}{c}{X}_{\mathrm{Si}}\\ Y_{\mathrm{Si}}\\ Z_{\mathrm{Si}}\end{array}\right]+(I+S_i)\left[\begin{array}{c}{x}_{\mathrm{ih}}\\ y_{\mathrm{ih}}\\ z_{\mathrm{ih}}\end{array}\right]---\left(5\right)$

i移项后单标靶附有未知数的条件平差值方程式为 

X_h＝b_i(-Z_h-z_ih)+c_i(-Y-y_ih)+X_Si+c_iY_Si+b_iZ_Si+x_ih

Y_h＝a_i(-Z_h-z_ih)+c_i(X_h+x_ih)+-c_iX_S+Y_S+a_iZ_S+y_ih  (6) 

Z_h＝a_i(Y_h+y_ih)+b_i(X_h+x_ih)-b_iX_S-a_iY_S+Z_S+z_ih

公式(6)展开后，引入拟合参数A＝a、B＝b、C＝c、D＝X_S+cY_S+bZ_S、E＝-cX_S+Y_S+aZ_S、F＝-bX_S-aY_S+Z_S，变为 

X_h＝B(-Z_h-z_ih)+C(-Y_h-y_ih)+D+x_ih

Y_h＝A(-Z_h-z_ih)+C(X_h+x_ih)+E+y_ih  (7) 

Z_h＝A(Y_h+y_ih)+B(X_h+x_ih)+F+z_ih

顾及标靶h的指定坐标系坐标的随机误差ε_Xh、ε_Yh、ε_Zh，公式(7)变为 

X_h+ε_Xh＝B(-Z_h-z_ih)+C(-Y_h-y_ih)+D+x_ih

Y_h+ε_Yh＝A(-Z_h-z_ih)+C(X_h+x_ih)+E+y_ih  (8) 

Z_h+ε_Zh＝A(Y_h+y_ih)+B(X_h+x_ih)+F+z_ih

用拟合参数计算扫描站位置参数 

$\left[\begin{array}{c}{X}_S\\ Y_S\\ Z_S\end{array}\right]={\left[\begin{array}{ccc}1& C& B\\ -C& 1& A\\ -B& -A& 1\end{array}\right]}^{-1}\left[\begin{array}{c}D\\ E\\ F\end{array}\right]---\left(9\right)$

用拟合参数A、B、C计算旋转矩阵 

$R=\frac{1}{\mathrm{\Δ}}\left[\begin{array}{ccc}1+A^2-B^2-C^2& -2C-2\mathrm{AB}& -2B+2\mathrm{AC}\\ 2C-2\mathrm{AB}& {1-A}^2+B^2-C^2& -2A-2\mathrm{BC}\\ 2B+2\mathrm{AC}& 2A-2\mathrm{BC}& 1-a^2-b^2+c^2\end{array}\right]---\left(10\right)$

公共标靶约束方程推导：根据公式(8)，i站和j站(j≠i)指向公共标靶k，约束条件是两站坐标转换后公共标靶k的坐标相等，有约束方程 

(x_ik-x_jk)+ε_xij＝B_j(-Z_k-z_jk)+C_j(-Y_j-y_jk)+D_j+B_i(Z_k+z_ik)+C_i(Y_i+y_ik)-D_i

(y_ik-y_jk)+ε_yij＝A_j(-Z_k-z_jk)+C_j(X_k+x_jk)+E_j+A_ij(Z_k+z_ik)+C_i(-X_k-x_ik)-E_i  (11) 

(z_ik-z_jk)+ε_zij＝A_j(Y_k+y_jk)+B_j(X_k+x_jk)+F_j+A_i(-Y_k-y_ik)+B_i(-X_k-x_ik)-F_i

式中为ε_xij、ε_yij、ε_zij为扫描坐标的随机误差。如果一个标靶上有N个扫描站能观测到，可列N-1个约束方程，式中j＝2,3,…,N，l＝2,3,…,N；在最小二乘拟合准则 

${\mathrm{\ϵ}}_{\mathrm{Xh}}^2+{\mathrm{\ϵ}}_{\mathrm{Yh}}^2+{\mathrm{\ϵ}}_{\mathrm{Zh}}^2+...+{\mathrm{\ϵ}}_{\mathrm{xij}}^2+{\mathrm{\ϵ}}_{\mathrm{yij}}^2+{\mathrm{\ϵ}}_{\mathrm{zij}}^2+...=\min ---\left(12\right)$

下整体解算各站拟合参数，然后用公式(9)和公式(10)计算各站位置参数和旋转矩阵，评估点云定向的点位误差，将点云坐标转换到工程测量指定的坐标系，实现点云整体定向。 

本发明与现有技术相比，其优点在于： 

1)不需要计算参数近似值。 

2)不计算旋转矩阵9个元素的29个正弦和余弦函数。 

3)扫描站越多本发明方法的解算精度高的优势越明显。 

4)各扫描站的扫描点位置精度均匀。 

附图说明

图1是本发明实施例中由扫描站和球形定向标靶标靶布设图； 

图2是图1所示实施例中本发明与独立模型法定向的高程中误差比较图； 

图3是图1所示实施例中本发明与独立模型法定向的空间中误差比较图； 

图4是图1所示实施例中本发明与独立模型法定向的平面中误差比较图。 

具体实施方式

下面结合附图1～4对本发明作进一步说明。用Riegl VZ-1000，每扫描站布设4-5个球形定向标靶，相邻扫描站有2-3个球形公共标靶。实验过程采用坐标系为80西安坐标系，以高斯东坐标为X，高斯北坐标为Y，85高程为Z，构成近似的右手三维直角坐标系。球形标靶安装在可对中整平基座上。共扫描6站，每站扫描点约4000万个，共布设球形标 

靶15个，其中单独标靶8个，2重标靶2个，3重标靶4个，4重标靶1个。数据处理使用IDL语言开发的点云处理软件，用“区域网平差法”功能和“独立模型拟合法”。 

具体操作步骤为： 

步骤1)在测量区域网内进行多站扫描，获取扫描点云数据，完成扫描后将从扫描仪导出点云转换为EEX格式，并建立点云空间索引； 

步骤1)将各扫描站和在扫描站上能观测到的球形定向标靶连线，构成区域网； 

步骤3)获取扫描后的球形标靶表面点云，并拟合标靶球心在扫描设备坐标系的坐标；标靶表面上只要有4个以上的点，就能够探测且拟合出球心坐标。 

步骤4)整理多站点云整体定平差信息。全区域定向标靶有n₁＝8个单站标靶和n₂＝7个公共标靶，所有单光束的拟合方程有n₁+n₂＝15组，每组3个方程，共3(n₁+n₂)＝45个拟合方程。全区域有n＝28个激光束，约束方程组数为n₃＝n-n₁-n₂＝13，约束条件方程数为3n₃＝39。 

步骤5)根据三维坐标转换公式 

$\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]=\left[\begin{array}{c}{X}_S\\ Y_S\\ Z_S\end{array}\right]+R\left[\begin{array}{c}x\\ y\\ z\end{array}\right],R=\left[\begin{array}{ccc}{a}_1& a_2& a_3\\ b_1& b_2& b_3\\ c_1& c_2& c_3\end{array}\right]$

式中X_S、Y_S、Z_S表示扫描仪中心的三维坐标在指定坐标系的位置，R矩阵反映了扫描时的点云在指定坐标系的姿态，可由反对称矩阵 $S=\left[\begin{array}{ccc}0& -c& -b\\ c& 0& -a\\ b& a& 0\end{array}\right]$ 构成，R与S及单位矩阵I的关系为 

(1)S^T＝-S,Ρ＝(I+S)(I-S)^-1

(2)(I-S)^T＝I+S,(I+S)^T＝I-S 

(3)R^T＝R^-1＝(I+S)^-1(I-S) 

${(I+S)}^{-1}=\frac{1}{2}(I+R^{-1}),S=2{(I+R^{-1})}^{-1}-I---\left(4\right)$

只有一个扫描站的激光束射向的标靶称为单标靶，多站扫描激光束射向同一标靶，此时，各站的激光束称为同名光束，该标靶称为站公共标靶。 

单标靶方程式推导：依三维坐标转换公式，参照图1，i扫描站指向单标靶h的6参数 坐标转换方程为 

$\left[\begin{array}{c}{X}_h\\ Y_h\\ Z_h\end{array}\right]=\left[\begin{array}{c}{X}_{\mathrm{Si}}\\ Y_{\mathrm{Si}}\\ Z_{\mathrm{Si}}\end{array}\right]-R\left[\begin{array}{c}{x}_{\mathrm{ih}}\\ y_{\mathrm{ih}}\\ z_{\mathrm{ih}}\end{array}\right]$

两边同乘以 ${\hat{R}}^{-1}={(I+\hat{S})}^{-1}(I-\hat{S})$

$R^{-1}\left[\begin{array}{c}{X}_h\\ Y_h\\ Z_h\end{array}\right]=R^{-1}\left[\begin{array}{c}{X}_{\mathrm{Si}}\\ Y_{\mathrm{Si}}\\ Z_{\mathrm{Si}}\end{array}\right]-\left[\begin{array}{c}{x}_{\mathrm{ih}}\\ y_{\mathrm{ih}}\\ z_{\mathrm{ih}}\end{array}\right]$

两边同乘以

$(I-S_i)\left[\begin{array}{c}{X}_h\\ Y_h\\ Z_h\end{array}\right]=(I-S_i)\left[\begin{array}{c}{X}_{\mathrm{Si}}\\ Y_{\mathrm{Si}}\\ Z_{\mathrm{Si}}\end{array}\right]+(I+S_i)\left[\begin{array}{c}{x}_{\mathrm{ih}}\\ y_{\mathrm{ih}}\\ z_{\mathrm{ih}}\end{array}\right]$

i移项后单标靶附有未知数的条件平差值方程式为 

X_h＝b_i(-Z_h-z_ih)+c_i(-Y-y_ih)+X_Si+c_iY_Si+b_iZ_Si+x_ih

Y_h＝a_i(-Z_h-z_ih)+c_i(X_h+x_ih)+-c_iX_S+Y_S+a_iZ_S+y_ih  (18) 

Z_h＝a_i(Y_h+y_ih)+b_i(X_h+x_ih)-b_iX_S-a_iY_S+Z_S+z_ih

展开后，引入拟合参数A＝a、B＝b、C＝c、D＝X_S+cY_S+bZ_S、E＝-cX_S+Y_S+aZ_S、F＝-bX_S-aY_S+Z_S，变为 

X_h＝B(-Z_h-z_ih)+C(-Y_h-y_ih)+D+x_ih

Y_h＝A(-Z_h-z_ih)+C(X_h+x_ih)+E+y_ih

Z_h＝A(Y_h+y_ih)+B(X_h+x_ih)+F+z_ih

顾及标靶h的指定坐标系坐标的随机误差ε_Xh、ε_Yh、ε_Zh，变为： 

X_h+ε_Xh＝B(-Z_h-z_ih)+C(-Y_h-y_ih)+D+x_ih

Y_h+ε_Yh＝A(-Z_h-z_ih)+C(X_h+x_ih)+E+y_ih

Z_h+ε_Zh＝A(Y_h+y_ih)+B(X_h+x_ih)+F+z_ih

用拟合参数计算扫描站位置参数 

$\left[\begin{array}{c}{X}_S\\ Y_S\\ Z_S\end{array}\right]={\left[\begin{array}{ccc}1& C& B\\ -C& 1& A\\ -B& -A& 1\end{array}\right]}^{-1}\left[\begin{array}{c}D\\ E\\ F\end{array}\right]$

用拟合参数A、B、C计算旋转矩阵 

$R=\frac{1}{\mathrm{\Δ}}\left[\begin{array}{ccc}1+A^2-B^2-C^2& -2C-2\mathrm{AB}& -2B+2\mathrm{AC}\\ 2C-2\mathrm{AB}& {1-A}^2+B^2-C^2& -2A-2\mathrm{BC}\\ 2B+2\mathrm{AC}& 2A-2\mathrm{BC}& 1-a^2-b^2+c^2\end{array}\right]$

公共标靶约束方程：参照单标靶附有未知数的条件平差值方程式，i站和j站(j≠i)指向公共标靶k，约束条件是两站坐标转换后公共标靶k的坐标相等，于是有约束方程 

(x_ik-x_jk)+ε_xij＝B_j(-Z_k-z_jk)+C_j(-Y_j-y_jk)+D_j+B_i(Z_k+z_ik)+C_i(Y_i+y_ik)-D_i

(y_ik-y_jk)+ε_yij＝A_j(-Z_k-z_jk)+C_j(X_k+x_jk)+E_j+A_ij(Z_k+z_ik)+C_i(-X_k-x_ik)-E_i

(z_ik-z_jk)+ε_zij＝A_j(Y_k+y_jk)+B_j(X_k+x_jk)+F_j+A_i(-Y_k-y_ik)+B_i(-X_k-x_ik)-F_i

式中为ε_xjl、ε_yjl、ε_zjl为扫描坐标的随机误差。如果一个标靶上有N个扫描站能观测到，可列N-1个约束方程，式中j＝2,3,…,N，l＝2,3,…,N；在最小二乘拟合准则下 

${\mathrm{\ϵ}}_{\mathrm{Xh}}^2+{\mathrm{\ϵ}}_{\mathrm{Yh}}^2+{\mathrm{\ϵ}}_{\mathrm{Zh}}^2+...+{\mathrm{\ϵ}}_{\mathrm{xij}}^2+{\mathrm{\ϵ}}_{\mathrm{yij}}^2+{\mathrm{\ϵ}}_{\mathrm{zij}}^2+...=\min$

整体解算各站拟合参数，然后计算各站位置参数和旋转矩阵；分别用独立模型拟合法和平差法求解得到的定向参数来计算标靶的指定坐标系坐标与标靶原指定坐标系的坐标之差，计算各站标靶的高程中误差、平面点位中误差和空间点位中误差；拟合法与平差法在同一项坐标残差统计量精度上总体相差不大，其中拟合法的平面点位中误差和空间点位中误差最大分别为28mm、33mm，平差法的平面点位中误差和空间点位中误差最大分别为25mm、29mm，而高程中误差精度相对较高，拟合法和平差法的高程中误差最大分别为14.1mm、15mm；评估点云定向的点位误差，根据拟合的点云定向参数，将点云坐标转换到工程测量指定的坐标系，实现点云整体定向。 

实验数据证明： 

(1)本文线性拟合，既不用计算参数初值，避免了三角函数的复杂运算，又提高了运算效率。 

(2)公式少，编程容易实现。 

(3)区域网各标靶点处精度有很好的一致性，其精度高于独立模型法定向。 

Claims (3)

1.一种线性拟合地面扫描点云整体定向方法，其特征在于采用以下步骤：

1)在测量区域网内，布设球形定向标靶，进行多站扫描，获取扫描点云数据；

2)将各扫描站和在扫描站上能观测到的球形定向标靶连线，构成区域网；

3)拟合球形标靶球心在扫描仪坐标系中的坐标；

4)整理平差信息；

5)列立单激光束的数学模型，推导同名激光束的线性拟合数学模型，评估点云定向的点位误差，将点云坐标转换到工程测量指定的坐标系，实现点云整体定向。

2.根据权利要求1所述的线性拟合地面扫描点云整体定向方法，其特征在于：步骤4)中涉及的整理平差的信息包括定向标靶个数和重复标靶个数；若全区域定向标靶有n₁个单站标靶和n₂个公共标靶，所有单光束的拟合方程有n₁+n₂组，每组3个方程，共3(n₁+n₂)个拟合方程；全区域有n个激光束，约束方程组数为n₃＝n-n₁-n₂，约束条件方程数为3n₃；方程列立时，每一个标靶上先列组拟合方程，如果k标靶还有N-1个同名激光束，再列N-1组约束方程。

3.根据权利要求1所述的线性拟合地面扫描点云整体定向方法，其特征在于步骤5)实现过程如下：

根据公式 $\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]=\left[\begin{array}{c}{X}_S\\ Y_S\\ Z_S\end{array}\right]+R\left[\begin{array}{c}x\\ y\\ z\end{array}\right],$ 其中 $R=\left[\begin{array}{ccc}{a}_1& a_2& a_3\\ b_1& b_2& b_3\\ c_1& c_2& c_3\end{array}\right]---\left(1\right)$

R矩阵反映了扫描时的点云在指定坐标系的姿态，可由反对称矩阵 $S=\left[\begin{array}{ccc}0& -c& -b\\ c& 0& -a\\ b& a& 0\end{array}\right]$ 构成，R与S及单位矩阵I的关系为

(1)S^T＝-S,Ρ＝(I+S)(I-S)^-1

(2)(I-S)^T＝I+S,(I+S)^T＝I-S

(3)R^T＝R^-1＝(I+S)^-1(I-S)    (2)

$\left(4\right){(I+S)}^{-1}=\frac{1}{2}(I+R^{-1}),S=2{(I+R^{-1})}^{-1}-I$

只有一个扫描站的激光束射向的标靶称为单标靶，多站扫描激光束射向同一标靶，此时，各站的激光束称为同名光束，该标靶称为站公共标靶；

单标靶方程式推导：依公式(1)，i扫描站指向单标靶h的6参数坐标转换方程为

$\left[\begin{array}{c}{X}_h\\ Y_h\\ Z_h\end{array}\right]=\left[\begin{array}{c}{X}_{\mathrm{Si}}\\ Y_{\mathrm{Si}}\\ Z_{\mathrm{Si}}\end{array}\right]-R\left[\begin{array}{c}{x}_{\mathrm{ih}}\\ y_{\mathrm{ih}}\\ z_{\mathrm{ih}}\end{array}\right]---\left(3\right)$

公式(3)两边同乘以 ${\hat{R}}^{-1}{(I+\hat{S})}^{-1}(I+\hat{S})$

$R^{-1}\left[\begin{array}{c}{X}_h\\ Y_h\\ Z_h\end{array}\right]{=R}^{-1}\left[\begin{array}{c}{X}_{\mathrm{Si}}\\ Y_{\mathrm{Si}}\\ Z_{\mathrm{Si}}\end{array}\right]-\left[\begin{array}{c}{x}_{\mathrm{ih}}\\ y_{\mathrm{ih}}\\ z_{\mathrm{ih}}\end{array}\right]---\left(4\right)$

公式(4)两边同乘以

$(I-S_i)\left[\begin{array}{c}{X}_h\\ Y_h\\ Z_h\end{array}\right]=(I-S_i)\left[\begin{array}{c}{X}_{\mathrm{Si}}\\ Y_{\mathrm{Si}}\\ Z_{\mathrm{Si}}\end{array}\right]+(I+S_i)\left[\begin{array}{c}{x}_{\mathrm{ih}}\\ y_{\mathrm{ih}}\\ z_{\mathrm{ih}}\end{array}\right]---\left(5\right)$

i移项后单标靶附有未知数的条件平差值方程式为

X_h＝b_i(-Z_h-z_ih)+c_i(-Y-y_ih)+X_Si+c_iY_Si+b_iZ_Si+x_ih

Y_h＝a_i(-Z_h-z_ih)+c_i(X_h+x_ih)+-c_iX_S+Y_S+a_iZ_S+y_ih    (6)

Z_h＝a_i(Y_h+y_ih)+b_i(X_h+x_ih)-b_iX_S-a_iY_S+Z_S+z_ih

公式(6)展开后，引入拟合参数A＝a、B＝b、C＝c、D＝X_S+cY_S+bZ_S、E＝-cX_S+Y_S+aZ_S、F＝-bX_S-aY_S+Z_S，变为

X_h＝B(-Z_h-z_ih)+C(-Y_h-y_ih)+D+x_ih

Y_h＝A(-Z_h-z_ih)+C(X_h+x_ih)+E+y_ih    (7)

Z_h＝A(Y_h+y_ih)+B(X_h+x_ih)+F+z_ih

顾及标靶h的指定坐标系坐标的随机误差ε_Xh、ε_Yh、ε_Zh，公式(7)变为

X_h+ε_Xh＝B(-Z_h-z_ih)+C(-Y_h-y_ih)+D+x_ih

Y_h+ε_Yh＝A(-Z_h-z_ih)+C(X_h+x_ih)+E+y_ih    (8)

Z_h+ε_Zh＝A(Y_h+y_ih)+B(X_h+x_ih)+F+z_ih

用拟合参数计算扫描站位置参数

$\left[\begin{array}{c}{X}_S\\ Y_S\\ Z_S\end{array}\right]={\left[\begin{array}{ccc}1& C& B\\ -C& 1& A\\ -B& -A& 1\end{array}\right]}^{-1}\left[\begin{array}{c}D\\ E\\ F\end{array}\right]---\left(9\right)$

用拟合参数A、B、C计算旋转矩阵

$R=\frac{1}{\mathrm{\Δ}}\left[\begin{array}{ccc}1+A^2-B^2-C^2& -2C-2\mathrm{AB}& -2B+2\mathrm{AC}\\ 2C-2\mathrm{AB}& 1-A^2+B^2-C^2& -2A-2\mathrm{BC}\\ 2B+2\mathrm{AC}& 2A-2\mathrm{BC}& 1-a^2-b^2+c^2\end{array}\right]---\left(10\right)$

公共标靶约束方程推导：根据公式(8)，i站和j站(j≠i)指向公共标靶k，约束条件是两站坐标转换后公共标靶k的坐标相等，有约束方程

(x_ik-x_jk)+ε_xij＝B_j(-Z_k-z_jk)+C_j(-Y_j-y_jk)+D_j+B_i(Z_k+z_ik)+C_i(Y_i+y_ik)-D_i

(y_ik-y_jk)+ε_yij＝A_j(-Z_k-z_jk)+C_j(X_k+x_jk)+E_j+A_ij(Z_k+z_ik)+C_i(-X_k-x_ik)-E_i    (11)

(z_ik-z_jk)+ε_zij＝A_j(Y_k+y_jk)+B_j(X_k+x_jk)+F_j+A_i(-Y_k-y_ik)+B_i(-X_k-x_ik)-F_i

式中为ε_xij、ε_yij、ε_zij为扫描坐标的随机误差；如果一个标靶上有N个扫描站能观测到，可列N-1个约束方程，式中j＝2,3,…，N，l＝2,3,…,N；在最小二乘拟合准则

${\mathrm{\ϵ}}_{\mathrm{Xh}}^2+{\mathrm{\ϵ}}_{\mathrm{Yh}}^2+{\mathrm{\ϵ}}_{\mathrm{Zh}}^2+\·\·\·+{\mathrm{\ϵ}}_{\mathrm{xij}}^2+{\mathrm{\ϵ}}_{\mathrm{yij}}^2+{\mathrm{\ϵ}}_{\mathrm{zij}}^2+\·\·\·=\min ---\left(12\right)$

下整体解算各站拟合参数，然后用公式(9)和公式(10)计算各站位置参数和旋转矩阵，评估点云定向的点位误差，将点云坐标转换到工程测量指定的坐标系，实现点云整体定向。