基于Bursa模型的七参数空间三维坐标转换

程序员文章站 2022-04-04 16:07:36

...

一、Bursa模型简介

模型简介百度即可，这里不做介绍，因为不是自己整理的。

二、Bursa模型的推导

2.1 Bursa坐标转换模型

${\left[ \begin{matrix} X\\ Y\\ Z \end{matrix} \right]}_C ={\left[ \begin{matrix} 1 & 0 & 0 & 0 & -Z_D & Y_D & X_D\\ 0 & 1 & 0 & Z_D & 0 & -X_D & Y_D\\ 0 & 0 & 1 & -Y_D & X_D & 0 & Z_D \end{matrix} \right]} {\left[ \begin{matrix} T_X\\ T_Y\\ T_Z\\ \varepsilon_X\\ \varepsilon_Y\\ \varepsilon_Z\\ m \end{matrix} \right]} + {\left[ \begin{matrix} X\\ Y\\ Z \end{matrix} \right]}_D\tag{1}$

2.3 建立间接平差模型

设有 $N$ 个重合点，七个参数看作必要观测数 $t=7$ ，其中总观测数为 $n=3N$ ，多余观测数 $r=n-t$ 。由此关系可知，至少需要3个点才能完成结算。根据间接平差模型，列出误差方程为：

${\left[ \begin{matrix} V_{X_1}\\ V_{Y_1}\\ V_{Z_1}\\ \vdots \\ V_{X_N}\\ V_{Y_N}\\ V_{Z_N} \end{matrix} \right]}_C ={\left[ \begin{matrix} 1 & 0 & 0 & 0 & -Z_{D_1} & Y_{D_1} & X_{D_1}\\ 0 & 1 & 0 & Z_{D_1} & 0 & -X_{D_1} & Y_{D_1}\\ 0 & 0 & 1 & -Y_{D_1} & X_{D_1} & {0} & Z_{D_1}\\ \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \\ 1 & 0 & 0 & 0 & -Z_{D_N} & Y_{D_N} & X_{D_N}\\ 0 & 1 & 0 & Z_{D_N} & 0 & -X_{D_N} & Y_{D_N}\\ 0 & 0 & 1 & -Y_{D_N} & X_{D_N} & {0} & Z_{D_N} \end{matrix} \right]} {\left[ \begin{matrix} T_X\\ T_Y\\ T_Z\\ \varepsilon_X\\ \varepsilon_Y\\ \varepsilon_Z\\ m \end{matrix} \right]}- {\left[ \begin{matrix} {X_1}\\ {Y_1}\\ {Z_1}\\ \vdots \\ {X_N}\\ {Y_N}\\ {Z_N} \end{matrix} \right]}_C + {\left[ \begin{matrix} {X_1}\\ {Y_1}\\ {Z_1}\\ \vdots \\ {X_N}\\ {Y_N}\\ {Z_N} \end{matrix} \right]}_D\tag{2}$ ⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡VX1VY1VZ1⋮VXNVYNVZN⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤C=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡100⋮100010⋮010001⋮0010ZD1−YD1⋮0ZDN−YDN−ZD10XD1⋮−ZDN0XDNYD1−XD10⋮YDN−XDN0XD1YD1ZD1⋮XDNYDNZDN⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤⎣⎢⎢⎢⎢⎢⎢⎢⎢⎡TXTYTZεXεYεZm⎦⎥⎥⎥⎥⎥⎥⎥⎥⎤−⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡X1Y1Z1⋮XNYNZN⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤C+⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡X1Y1Z1⋮XNYNZN⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤D(2)

将(2)上式简化为新的矩阵形式为：

$V = B \hat X -L \tag{3}$

然后利用最小二乘法来求解坐标转换参数，这种方法利用了所有的公共点，可以得到较好的结果，但是由于将每个点的坐标精度都视为精度相同的观测值，所以得到是一种近似的结果。因此，各点的坐标视为同精度独立观测值，P为单位矩阵，则可由(3)式得到：

$\hat X = (B^T B)^{-1}(B^T L)\tag{4}$

精度评定，其单位权中误差为：

$\sigma _0 = \sqrt{\frac{V^T PV}{n-t}}\tag{5}$

注意

需要将旋转参数 $\varepsilon _X$ 、 $\varepsilon _Y$ 和 $\varepsilon _Z$ 的单位为弧度，要将其转换到秒；尺度参数m单位换位“ppm”，ppmpart per million 百万分之……。
具体计算公式为：将旋转角乘以206265即可换为“s”，将尺度参数乘以1000000单位即为 “ppm”。

$1弧度(rad) = 206264.8062471秒$

七参数	单位
$T_X$	$m$
$T_Y$	$m$
$T_Z$	$m$
$\varepsilon _X$	$s$
$\varepsilon _Y$	$s$
$\varepsilon _Z$	$s$
$m$	$ppm$

2.4 非重合点的坐标转换

利用求得的七参数，将数据代入即可解得转换后的坐标。

精度评定：无法像重合点那样可以利用原有的坐标与转换的坐标来计算残差。此时可利用配置法，将重合点的转换值改正数作为已知值，然后对非公共点进行配置，具体的方法为：

①计算重合点转换值得改正数，其重合点的坐标采用已知值。

$V = 已知值-转换值\tag{6}$

②采用配置可计算出非公共点转换值的改正数。

$V' = \frac{\sum_{1}^n{P_i V_i}}{\sum_{1}^n{P_i}}\tag{7}$

$n$ 为选择重合点的个数，根据非重合点与重合点的距离来定权，其权为：

$P_i = \frac {1} {S_i^2}\tag 8$

三、程序的实现

# 忽略烦人的红色提示
import warnings
warnings.filterwarnings("ignore")

# 导入Python的数据处理库pandas，相当于python里的excel
import pandas as pd

# 导入python绘图matplotlib
import matplotlib.pyplot as plt

# 使用ipython的魔法方法，将绘制出的图像直接嵌入在notebook单元格中
%matplotlib inline

# 设置绘图大小
plt.style.use({'figure.figsize':(25,20)})
plt.rcParams['font.sans-serif']=['SimHei']  # 用来正常显示中文标签  
plt.rcParams['axes.unicode_minus']=False  # 用来正常显示负号

3.1 坐标的导入

# 读取CGCS2000坐标系下的坐标
cc = pd.read_csv('new.csv')
# 查看坐标
cc

	点号	X	Y	Z
0	GPS04	-1964642.836	4484908.586	4075486.898
1	GPS30	-1967082.716	4490541.646	4068048.151
2	GPS18	-1958106.370	4482074.179	4082054.321
3	GPS22	-1958396.995	4485396.445	4077966.297
4	GPS27	-1953364.459	4481502.655	4084942.265
5	GPS26	-1957928.755	4492765.305	4070011.563

import numpy as np
np.set_printoptions(suppress=True)
C_XYZ = np.empty((0,3))
for index,row in cc.iterrows():
    pd.set_option('precision', 4) 
    C_x = row['X']
    C_y = row['Y']
    C_z = row['Z']
    C_xyz = np.array([[C_x,C_y,C_z]])
    C_XYZ = np.append(C_XYZ,C_xyz,axis=0)
print(C_XYZ)

[[-1964642.836  4484908.586  4075486.898]
 [-1967082.716  4490541.646  4068048.151]
 [-1958106.37   4482074.179  4082054.321]
 [-1958396.995  4485396.445  4077966.297]
 [-1953364.459  4481502.655  4084942.265]
 [-1957928.755  4492765.305  4070011.563]]

# 读取地方坐标系下的坐标
dd = pd.read_csv('old.csv')
# 查看信息
print(dd.shape)
dd

(6, 4)

	点号	X	Y	Z
0	GPS04	-1.9647e+06	4.4848e+06	4.0754e+06
1	GPS30	-1.9672e+06	4.4904e+06	4.0679e+06
2	GPS18	-1.9582e+06	4.4819e+06	4.0820e+06
3	GPS22	-1.9585e+06	4.4853e+06	4.0779e+06
4	GPS27	-1.9535e+06	4.4814e+06	4.0848e+06
5	GPS26	-1.9580e+06	4.4926e+06	4.0699e+06

import numpy as np
D_XYZ = np.empty((0,3))
for index,row in dd.iterrows():
    pd.set_option('precision', 4) 
    D_x = row['X']
    D_y = row['Y']
    D_z = row['Z']
    D_xyz = np.array([[D_x,D_y,D_z]])
    D_XYZ = np.append(D_XYZ,D_xyz,axis=0)
print(D_XYZ)

[[-1964734.964  4484768.547  4075386.77 ]
 [-1967174.802  4490401.508  4067948.166]
 [-1958198.61   4481934.193  4081954.089]
 [-1958489.229  4485256.399  4077866.134]
 [-1953456.789  4481362.679  4084841.963]
 [-1958020.995  4492625.151  4069911.537]]

3.2 解算七参数

$\hat X = (B^T B)^{-1}(B^T L)\tag{4}$

L = []
B = np.empty((0,7))
for i in range(D_XYZ.shape[0]):
    # 提取元素
    X_C = C_XYZ[i][0]
    Y_C = C_XYZ[i][1]
    Z_C = C_XYZ[i][2]
    X_D = D_XYZ[i][0]
    Y_D = D_XYZ[i][1]
    Z_D = D_XYZ[i][2]
    # 构建L矩阵
    L.extend((X_C - X_D,Y_C - Y_D,Z_C - Z_D))
    #L = np.append(L,LL,axis=1)
    # 构建B矩阵
    b1 = np.array([1,0,0,0,-Z_D,Y_D,X_D])
    b2 = np.array([0,1,0,Z_D,0,-X_D,Y_D])
    b3 = np.array([0,0,1,-Y_D,X_D,0,Z_D])
    BB = np.row_stack((b1,b2,b3))
    B = np.append(B,BB,axis=0)
B = B
L = np.array([L]).T
# print("L矩阵为：\n",L)
# print("B矩阵为：\n",B)

a = np.linalg.inv(np.dot(B.T,B))
b = np.dot(B.T,L)
# 求取伪七参数
x = np.dot(a,b)
print("解算的七参数为：")
print("X平移参数：{} m".format(x[0][0]))
print("Y平移参数：{} m".format(x[1][0]))
print("Z平移参数：{} m".format(x[2][0]))
print("X旋转参数：{} s".format(x[3][0]* 206265))
print("Y旋转参数：{} s".format(x[4][0]* 206265))
print("Z旋转参数：{} s".format(x[5][0]* 206265))
print("m尺度参数：{} ppm".format(x[6][0]* 1000000))

解算的七参数为：
X平移参数：121.62369966506958 m
Y平移参数：55.88656985759735 m
Z平移参数：31.898368503898382 m
X旋转参数：0.18627551317509372 s
Y旋转参数：-0.06667437699100276 s
Z旋转参数：0.17122195111095806 s
m尺度参数：17.579272022061332 ppm

3.3 重合点精度评定

$V = B \hat X -L \tag{3}$

$\sigma _0 = \sqrt{\frac{V^T PV}{n-t}}\tag{5}$

V = np.dot(B,x)-L
print(V)

[[-0.00272118]
 [-0.0020901 ]
 [-0.00234788]
 [-0.0013403 ]
 [-0.00675914]
 [ 0.00558849]
 [-0.00004679]
 [ 0.00158902]
 [ 0.00954763]
 [ 0.00228071]
 [-0.00345956]
 [ 0.00377805]
 [-0.00622995]
 [ 0.000214  ]
 [-0.01070229]
 [ 0.00805748]
 [ 0.01050535]
 [-0.00586396]]

import math
xx = math.sqrt(np.dot(V.T,V)/(3*D_XYZ.shape[0]-7))
print(xx)

0.007287575001997314

3.4 非重合点的转换

# 读取地方坐标系下的坐标
ddno = pd.read_csv('oldno.csv')
# 查看信息
print(ddno.shape)
ddno

(5, 4)

	点号	X	Y	Z
0	GPS21	-1.9545e+06	4.4903e+06	4.0742e+06
1	GPS17	-1.9520e+06	4.4853e+06	4.0811e+06
2	GPS29	-1.9569e+06	4.4974e+06	4.0652e+06
3	GPS25	-1.9519e+06	4.4949e+06	4.0704e+06
4	GPS28	-1.9535e+06	4.5010e+06	4.0629e+06

import numpy as np
DN_XYZ = np.empty((0,3))
for index,row in ddno.iterrows():
    pd.set_option('precision', 4) 
    Dno_x = row['X']
    Dno_y = row['Y']
    Dno_z = row['Z']
    DN_xyz = np.array([[Dno_x,Dno_y,Dno_z]])
    DN_XYZ = np.append(DN_XYZ,DN_xyz,axis=0)
print(DN_XYZ.shape)

(5, 3)

for i in range(DN_XYZ.shape[0]):
    # 提取元素
    XN_D = DN_XYZ[i][0]
    YN_D = DN_XYZ[i][1]
    ZN_D = DN_XYZ[i][2] 
    LN = np.row_stack((XN_D,YN_D,ZN_D))
    # 构建L矩阵
    # 构建B矩阵
    bn1 = np.array([1,0,0,0,-ZN_D,YN_D,XN_D])
    bn2 = np.array([0,1,0,ZN_D,0,-XN_D,YN_D])
    bn3 = np.array([0,0,1,-YN_D,XN_D,0,ZN_D])
    BN = np.row_stack((b1,b2,b3))
    NCC = np.dot(BN,x) + LN
    print('第{}个点的坐标为：{},{},{}'.format(i,NCC[0][0],NCC[1][0],NCC[2][0]))

第0个点的坐标为：-1954403.217942523,4490401.535505347,4074332.8511360423
第1个点的坐标为：-1951867.9859425232,4485404.294505347,4081154.3441360416
第2个点的坐标为：-1956813.4649425233,4497507.388505346,4065328.6971360417
第3个点的坐标为：-1951778.264942523,4495070.800505347,4070465.314136042
第4个点的坐标为：-1953427.302942523,4501107.370505347,4062974.232136042

3.5 非重合点的精度评定

3.5.1 定权

$V' = \frac{\sum_{1}^n{P_i V_i}}{\sum_{1}^n{P_i}}\tag{7}$

$P_i = \frac {1} {S_i^2}\tag 8$

aa = list(range(0,V.shape[0],3))
bb = list(range(1,V.shape[0],3))
cc = list(range(2,V.shape[0],3))
# 已知点的x残差
v_x = V[aa,:]
# 已知点的y残差
v_y = V[bb,:]
# 已知点的z残差
v_z = V[cc,:]
v_x.shape

(6, 1)

V_xx,V_yy,V_zz = [],[],[]
for i in range(DN_XYZ.shape[0]):
    # 提取元素
    XN_D = DN_XYZ[i][0]
    YN_D = DN_XYZ[i][1]
    ZN_D = DN_XYZ[i][2]
    LC = np.row_stack((XN_D,YN_D,ZN_D))
    # 定权
    PP = np.dot(D_XYZ,LC).T
    ccc = []
    for j in range(PP.shape[1]):
        ccc.append(1/PP[0][j])
    # 公式(7)
    VV_S = sum(ccc)
    print(VV_S)
    V_x = np.dot(np.array([ccc]),v_x)[0][0] / VV_S
    V_y = np.dot(np.array([ccc]),v_y)[0][0] / VV_S
    V_z = np.dot(np.array([ccc]),v_z)[0][0] / VV_S
    V_xx.append(V_x)
    V_yy.append(V_y)
    V_yy.append(V_z)
    print("非重合点GPS{}的残差V_x{}  V_y{}  V_z{} ".format(i+1,V_x,V_y,V_z))

1.478477870203965e-13
非重合点GPS1的残差V_x2.621759300967811e-08  V_y-8.17546918711022e-08  V_z1.1506213912802006e-08 
1.478462584622879e-13
非重合点GPS2的残差V_x3.02685943659054e-08  V_y-8.170440346251001e-08  V_z1.3431354650058274e-08 
1.4784816011772054e-13
非重合点GPS3的残差V_x2.0702207439612064e-08  V_y-8.220030115808458e-08  V_z9.309236986800881e-09 
1.4784765461761397e-13
非重合点GPS4的残差V_x2.3313321582638324e-08  V_y-8.337122325453742e-08  V_z1.190322739292841e-08 
1.4784846884028183e-13
非重合点GPS5的残差V_x1.8683221941032832e-08  V_y-8.387564655386071e-08  V_z1.0205456918699615e-08

3.5.2 精度评定

# X的中误差
S = 0
for i in V_xx:
    S += i*i
VXX = math.sqrt(S/(len(V_xx)-1))
print("空间直角坐标X残差中误差",VXX)
# Y的中误差
S = 0
for i in V_yy:
    S += i*i
VYY = math.sqrt(S/(len(V_yy)-1))
print("空间直角坐标Y残差中误差",VYY)
# Z的中误差
S = 0
for i in V_zz:
    S += i*i
VZZ = math.sqrt(S/(len(V_zz)-1))
print("空间直角坐标Z残差中误差",VZZ)
# 点位中误差
print("空间直角坐标点位中误差",math.sqrt(VXX**2 + VYY**2 + VZZ**2))

空间直角坐标X残差中误差 2.7040271477441625e-08
空间直角坐标Y残差中误差 6.21356115133806e-08
空间直角坐标Z残差中误差 -0.0
空间直角坐标点位中误差 6.776437485667154e-08