CN106546262B - 基于平面控制和约束结合的移动测量系统外参数标定方法 - Google Patents

Abstract

本发明公开了一种基于平面控制和约束结合的移动测量系统外参数标定方法。该方法基于Gauss‑Helmert平差模型，利用已知平面参数的平面方程作为控制条件，以未知平面参数的平面作为约束条件，在不断增加已知控制平面和未知约束平面的过程中，构建并推导所需法方程，求解最终的未知参数。整个检校过程不仅具备了自检校技术的高效率特性，还限制了自检校过程中参数之间的相关性，提高了检校质量，具有广阔的使用前景。

Description

基于平面控制和约束结合的移动测量系统外参数标定方法

技术领域

本发明涉及一种基于平面控制和约束结合的移动测量系统外参数标定方法。

背景技术

移动测量系统集成有POS系统、激光扫描仪等多种传感器，系统工作时通过POS系统来获取载体平台的位置和姿态信息，并通过一系列的坐标转换来获得扫描点WGS-84坐标系下的三维坐标。然而，由于激光扫描仪中心和POS系统的中心不重合，需要解算出相应的偏移量，同时因为激光扫描仪与POS系统自身的轴向不可能完全重合，所以也需要解算视准轴误差，这个过程称之为移动测量系统的外参数标定。

移动车载测量系统的高密度和高精度使点云技术应用到各个领域中，在获取目标的三维点云，需借助POS系统实现激光点坐标到WGS-84坐标系的转换，这个过程中影响系统精度的因素很多，需要相应的检校方法进行标定以获取高精度的激光脚点坐标。

目前移动测量系统的检校技术主要有以下三种：基于控制点的检校技术、基于平面控制的检校方法以及基于平面约束的自检校方法。

(1)基于控制点的检校方法就是首先在实验场布设大量控制点，然后利用全站仪等手段测一组控制点的真实坐标，再将激光扫描仪的测量结果与之相比较，用解方程组的方法得到误差参数的具体数值，对测量结果进行补偿，使测量结果与真实值相一致。

基于控制点的检校方法的求解思路是首先建立含有激光扫描仪误差参数的数学模型，依据此模型求解关于激光扫描仪系统误差参数的线性化误差方程，然后根据最小二乘间接平差原理即可列出相应的误差参数解的表达式，此时将实验获取的相关数据代入表达式中即可求解出误差参数。该方法需要布设一定数量的控制点，利用全站仪测得实验所需所有控制点的全部真实坐标，再与激光扫描仪所获数据进行比较，整个过程繁琐，所需工作量庞大，同时，最重要的一点是上述校验方法很难准确的将同名控制点提取出来，并且这一部分主要依靠人工操作进行，就精度本身而言就已经增添了许多不确定因素。

(2)基于平面控制的检校方法是在控制点检校方法的基础延伸发展而来，通过利用全站仪测量一组位于同一平面上的控制点的真实坐标以此确定平面方程，再将激光测量得到的此平面上的激光点云坐标代入平面方程中，此时点云数据中包含了激光的三个姿态角旋转分量误差参数角度以及三个平移分量误差参数，根据带有这些误差参数的平面方程对每个误差参数求导建立误差方程，然后依据各个误差变量的初始值求解出误差改正数并对误差变量进行修正，通过设定阈值不断迭代最终求解出误差参数。

基于平面控制的检校方法是对基于控制点技术的扩展延伸，解决了提取同名控制点难的缺陷，但是为了确定平面方程，便需要利用全站仪测量该平面上的不共线控制点，对所有的待测平面都要一一进行测量，对于较多的检校平面，同样具有较大的工作量。

(3)基于平面约束的自检校方法以视准轴角误差会导致多方位扫描同一平面特征时所获得的点云不共面为依据，根据点云生成模型、平面特征的几何模型以及观测值精度，建立起严格的数学模型，再对函数模型线性化得到平差模型，并根据最小二乘原理推导出计算视准轴角误差的法方程，求解得到视准轴误差改正量。该方法是以视准轴误差导致多方位扫描同一平面特征点云不共面为原理，基于Gauss-Helmert平差模型实现视准轴误差自标定的过程，但是缺少相应的控制平面条件对检校结果产生影响，例如参数之间的相关性等误差影响因素，其中，像平面参数与视准轴误差参数之间的相关性较强，需要很多不同朝向的平面限制其影响，但是要获取众多不同朝向的平面在实际中有时很困难。

发明内容

本发明的目的在于提出一种基于平面控制和约束结合的移动测量系统外参数标定方法，以解决现有技术中自检校参数相关性引起的误差因素，提高检校精度。

为了实现上述目的，本发明采用如下技术方案：

基于平面控制和约束结合的移动测量系统外参数标定方法，包括如下步骤：

a在移动测量系统中进行坐标转换

激光点经过一系列坐标转换后在WGS-84坐标系下的最终形式为：

其中，为激光扫描仪坐标系到惯性平台坐标系的旋转矩阵，为惯性平台坐标系到当地水平坐标系的旋转矩阵，为当地水平坐标系到WGS-84坐标系的旋转矩阵；

X、Y、Z为坐标系的三个坐标轴；l_x、l_y、l_z为偏心量在三个坐标轴上的分量；

为扫描点在WGS-84坐标系下的坐标；

为当地水平坐标系原点在WGS-84坐标系下的空间直角坐标；

为扫描点在激光扫描仪坐标系下的坐标；

为激光扫描仪坐标系与载体坐标系之间的偏心量；

b建立数学函数模型

b.1基础平差模型

对于t个点，定义向量x_c∈R^u为标定参数，x_p∈R^s为平面参数，l∈Rⁿ为观测向量；u、s、n分别表示标定参数x_c、平面参数x_p和观测向量l的个数；

观测向量值：l＝[X_oe Y_oe Z_oe r p y ρ θ φ]；

其中，X_oe、Y_oe、Z_oe表示当地水平坐标系原点在WGS-84坐标系下的空间直角坐标系中的三个坐标；r、p、y分别表示惯导记录姿态角测量值侧滚角、俯仰角和偏航角；ρ表示测量点到激光扫描仪原点的距离，θ表示激光束扫描竖直角，φ表示激光束扫描水平角；

对于Gauss-Helmert平差模型，其一般线性化形式为：

其中，为未知待估计参数，A为对未知参数求一阶偏导后的设计矩阵，v表示观测值的改正数，B为对观测量求一阶偏导后的设计矩阵，w为闭合差向量；

b.2基于控制平面的函数模型

以Gauss-Helmert平差模型为基础，即公式(2)，基于控制平面的函数模型有：

其中，

其中，X_e、Y_e、Z_e为扫描点在WGS-84坐标系下坐标；f_c表示控制平面的函数模型，为观测值平差值，表示为标定参数初始值，为构建模型标定参数；

a_p、b_p、c_p、d_p表示平面参数；I表示单位矩阵，表示误差补偿矩阵，为旋转矩阵的近似值；Δκ、Δω、Δl_x、Δl_y、Δl_z为待标定参数；

对式(3)进行线性化得Gauss-Helmert模型：

其中，A_c表示为对标定参数求偏导的设计矩阵；B_c表示对观测量求偏导后的设计矩阵；即为标定参数改正平差值，为观测值的改正数，w_c为该方程的闭合差向量；

b.3基于约束平面的函数模型

移动测量系统获取的具有平面特征的点云数据所满足的共面方程为：

ax+by+cz-d＝0 (5)

其中，a、b、c为平面的法线向量，d为原点到平面的距离；

平面参数满足的约束条件如下：

其中，为待估计平面参数，为平面参数满足的约束条件方程，表示平面法线向量参数；

其中，f_p表示所列约束平面函数模型；表示对标定参数求偏导的设计矩阵，表示对平面参数求偏导的设计矩阵；表示对观测量求偏导的设计矩阵；为约束平面闭合差向量，为标定参数初始值，为平面参数初始值；表示平面参数改正平差值，表示约束平面观测值改正数；

并有约束条件，对式(6)线性化有：

其中，表示为约束函数关于平面参数的偏导矩阵，g表示为约束函数模型，为平面参数，表示约束平面对应的约束条件的改正向量平差值；

c建立综合函数模型

定义m为平面参数已知的控制平面个数，1≤j≤m，其中，j表示为第j个控制平面；k为平面参数未知的约束平面个数，1≤q≤k，其中，q表示为第q个约束平面；

c.1控制平面累加

基于基础模型，控制平面不断累加，对于m个不断添加的已知的控制平面，函数模型变化为：

对第j个控制平面，有：

其中，表示对第j个控制平面标定参数求偏导的设计矩阵；

表示对第j个控制平面观测量求偏导后的设计矩阵；

为第j个控制平面观测值的改正数；为第j个控制平面闭合差向量；

对标定参数，求偏导的设计矩阵：

其中，表示标定参数偏导向量，1≤i≤t；

对观测量求偏导后的设计矩阵：

其中，表示观测值偏导向量，v_i表示观测值改正向量，1≤i≤t；

c.2约束平面累加

在基于控制平面函数模型基础上，不断添加未知平面参数的约束平面，函数模型变化为：

第q个未知平面，1≤q≤k，线性化后的Gauss-Helmert模型是：

并有约束条件：

其中，表示第q个约束平面对标定参数求偏导的设计矩阵；

表示对第q个约束平面的平面参数求偏导后的设计矩阵；

表示对第q个约束平面观测量求偏导后的设计矩阵；

表示第q个约束平面的平面参数改正平差值；

为第q个约束平面观测值改正数；

为表示第q个约束平面闭合差向量；

其中，为第q个约束平面的平面参数初始值；为第1个约束方程关于平面参数的偏导矩阵，为第q个约束方程关于的偏导矩阵，为第q个约束平面的平面参数；为第q个约束平面对应的约束条件的闭合差向量；表示第1个约束平面对应的约束条件的改正向量平差值，为第q个约束平面对应的约束条件的改正向量平差值；表示第q个约束平面的平面参数改正平差值；

对标定参数求偏导的设计矩阵：

其中，表示视准轴误差参数偏导向量，1≤i≤t；

对平面参数求偏导的设计矩阵：

其中，表示约束平面对平面参数求偏导的向量，

表示约束平面对应的约束条件对平面参数求偏导的向量；

对观测量求偏导后的设计矩阵：

其中，表示观测量偏导向量，1≤i≤t；

c.3最终法方程形式

对于m个控制平面，k个约束平面，根据综合标定函数模型，求解其法方程最终形式为：

式中， 表示观测值的权重矩阵，表示约束条件的权重矩阵；

通过法方程，计算求解出标定参数改正平差值平面参数改正平差值

c.4迭代解算

将通过法方程估计出的标定参数改正平差值和平面参数改正平差值与规定的限差相比较，若改正平差值的绝对值均小于限差，则计算结束；

否则，利用参数初始值加入改正，即作为新的近似值，通过控制平面和约束平面循环累加得到的法方程重新计算参数的改正平差值，并再次比较参数的改正平差值的绝对值与限差大小，做出是否继续求解的判断；

如此重复进行下去，直到改正值均小于限差为止。

本发明具有如下优点：

本发明方法利用已知平面参数的平面方程作为控制条件，并根据实际要求来不断增添已知平面，再以此作为基础方程，不断添加检校所需的一系列未知平面参数的实验平面，在这个过程中基于Gauss-Helmert平差模型构建函数模型，根据最小二乘原理解算推导法方程，求解最终的未知参数。整个检校过程，不仅具备了自检校技术的高效率特性，还限制了自检校过程中由参数之间的相关性，提高了检校质量，无论是在精度要求还是在实际的工作需求上都具有广阔的使用前景。

具体实施方式

下面结合具体实施方式对本发明作进一步详细说明：

基于平面控制和约束结合的移动测量系统外参数标定方法，包括如下步骤：

a在移动测量系统中进行坐标转换

移动测量系统定位的几何模型是在不同坐标系转换的基础上推导的。定位的坐标转换顺序是激光扫描仪坐标系→惯性平台坐标系→当地水平坐标系→WGS-84坐标系。

激光点经过一系列坐标转换后在WGS-84坐标系下的最终形式为：

其中，为激光扫描仪坐标系到惯性平台坐标系的旋转矩阵，为惯性平台坐标系到当地水平坐标系的旋转矩阵，为当地水平坐标系到WGS-84坐标系的旋转矩阵；

X、Y、Z为坐标系的三个坐标轴；l_x、l_y、l_z为偏心量在三个坐标轴上的分量；

为扫描点在WGS-84坐标系下的坐标；

为当地水平坐标系原点在WGS-84坐标系下的空间直角坐标；

为扫描点在激光扫描仪坐标系下的坐标；

为激光扫描仪坐标系与载体坐标系之间的偏心量；

b建立数学函数模型

b.1基础平差模型

对于t个点，定义向量x_c∈R^u为标定参数，x_p∈R^s为平面参数，l∈Rⁿ为观测向量；u、s、n分别表示标定参数x_c、平面参数x_p和观测向量l的个数；

观测向量值：l＝[X_oe Y_oe Z_oe r p y ρ θ φ]；

其中，X_oe、Y_oe、Z_oe表示当地水平坐标系原点在WGS-84坐标系下的空间直角坐标系中的三个坐标；r、p、y分别表示惯导记录姿态角测量值侧滚角、俯仰角和偏航角；ρ表示测量点到激光扫描仪原点的距离，θ表示激光束扫描竖直角，φ表示激光束扫描水平角；

对于Gauss-Helmert平差模型，其一般线性化形式为：

其中，为未知待估计参数，A为对未知参数求一阶偏导后的设计矩阵，v表示观测值的改正数，B为对观测量求一阶偏导后的设计矩阵，w为闭合差向量；

b.2基于控制平面的函数模型

以Gauss-Helmert平差模型为基础，即公式(2)，基于控制平面的函数模型有：

其中，

其中，X_e、Y_e、Z_e为扫描点在WGS-84坐标系下坐标；f_c表示控制平面的函数模型，为观测值平差值，表示为标定参数初始值，为构建模型标定参数；

a_p、b_p、c_p、d_p表示平面参数；I表示单位矩阵，表示误差补偿矩阵，为旋转矩阵的近似值；Δκ、Δω、Δl_x、Δl_y、Δl_z为待标定参数；

对式(3)进行线性化得Gauss-Helmert模型：

其中，A_c表示为对标定参数求偏导的设计矩阵；B_c表示对观测量求偏导后的设计矩阵；即为标定参数改正平差值，为观测值的改正数，w_c为该方程的闭合差向量；

b.3基于约束平面的函数模型

移动测量系统获取的具有平面特征的点云数据所满足的共面方程为：

ax+by+cz-d＝0 (5)

其中，a、b、c为平面的法线向量，d为原点到平面的距离；

平面参数满足的约束条件如下：

其中，为待估计平面参数，为平面参数满足的约束条件方程，表示平面法线向量参数；

其中，f_p表示所列约束平面函数模型；表示对标定参数求偏导的设计矩阵，表示对平面参数求偏导的设计矩阵；表示对观测量求偏导的设计矩阵；为约束平面闭合差向量，为标定参数初始值，为平面参数初始值；表示平面参数改正平差值，表示约束平面观测值改正数；

并有约束条件，对式(6)线性化有：

其中，表示为约束函数关于平面参数的偏导矩阵，g表示为约束函数模型，为平面参数，表示约束平面对应的约束条件的改正向量平差值；

c建立综合函数模型

定义m为平面参数已知的控制平面个数，1≤j≤m，其中，j表示为第j个控制平面；k为平面参数未知的约束平面个数，1≤q≤k，其中，q表示为第q个约束平面；

c.1控制平面累加

基于基础模型，控制平面不断累加，对于m个不断添加的已知的控制平面，函数模型变化为：

对第j个控制平面，有：

其中，表示对第j个控制平面标定参数求偏导的设计矩阵；

表示对第j个控制平面观测量求偏导后的设计矩阵；

为第j个控制平面观测值的改正数；为第j个控制平面闭合差向量；

对标定参数，求偏导的设计矩阵：

其中，表示标定参数偏导向量，1≤i≤t；

对观测量求偏导后的设计矩阵：

其中，表示观测值偏导向量，v_i表示观测值改正向量，1≤i≤t；

c.2约束平面累加

在基于控制平面函数模型基础上，不断添加未知平面参数的约束平面，函数模型变化为：

第q个未知平面，1≤q≤k，线性化后的Gauss-Helmert模型是：

并有约束条件：

其中，表示第q个约束平面对标定参数求偏导的设计矩阵；

表示对第q个约束平面的平面参数求偏导后的设计矩阵；

表示对第q个约束平面观测量求偏导后的设计矩阵；

表示第q个约束平面的平面参数改正平差值；

为第q个约束平面观测值改正数；

为表示第q个约束平面闭合差向量；

其中，为第q个约束平面的平面参数初始值；为第1个约束方程关于平面参数的偏导矩阵，为第q个约束方程关于的偏导矩阵，为第q个约束平面的平面参数；为第q个约束平面对应的约束条件的闭合差向量；表示第1个约束平面对应的约束条件的改正向量平差值，为第q个约束平面对应的约束条件的改正向量平差值；表示第q个约束平面的平面参数改正平差值；

对标定参数求偏导的设计矩阵：

其中，表示视准轴误差参数偏导向量，1≤i≤t；

对平面参数求偏导的设计矩阵：

其中，表示约束平面对平面参数求偏导的向量，

表示约束平面对应的约束条件对平面参数求偏导的向量；

对观测量求偏导后的设计矩阵：

其中，表示观测量偏导向量，1≤i≤t；

c.3最终法方程形式

对于m个控制平面，k个约束平面，根据综合标定函数模型，求解其法方程最终形式为：

式中， 表示观测值的权重矩阵，表示约束条件的权重矩阵；

通过法方程，计算求解出标定参数改正平差值平面参数改正平差值

c.4迭代解算

将通过法方程估计出的标定参数改正平差值和平面参数改正平差值与规定的限差相比较，若改正平差值的绝对值均小于限差，则计算结束；

否则，利用参数初始值加入改正，即作为新的近似值，通过控制平面和约束平面循环累加得到的法方程重新计算参数的改正平差值，并再次比较参数的改正平差值的绝对值与限差大小，做出是否继续求解的判断；

如此重复进行下去，直到改正值均小于限差为止。

通过上述基于平面控制和约束结构的移动测量系统外参数标定方法可以发现，本发明一方面解决了利用地面控制点进行标定的传统检校手段工作量大、提取特征控制点困难的缺陷，提高了工作效率和检校质量；另一方面又在基于平面约束自检校方法的技术上的基础上削弱了检校参数的相关性影响，通过与一系列未知平面结合，不仅做到了效率的保证又实现了对检校精度的提高；另外，在实验过程中，检校场的建立相对较为困难，特别是控制点检校场，灵活性不足，而平面约束可以在任何区域进行，可扩展性强，本发明同样具有该优越性。

当然，以上说明仅仅为本发明的较佳实施例，本发明并不限于列举上述实施例，应当说明的是，任何熟悉本领域的技术人员在本说明书的教导下，所做出的所有等同替代、明显变形形式，均落在本说明书的实质范围之内，理应受到本发明的保护。

Claims (1)

1.基于平面控制和约束结合的移动测量系统外参数标定方法，包括如下步骤：

a在移动测量系统中进行坐标转换

激光点经过一系列坐标转换后在WGS-84坐标系下的最终形式为：

其中，为激光扫描仪坐标系到惯性平台坐标系的旋转矩阵，为惯性平台坐标系到当地水平坐标系的旋转矩阵，为当地水平坐标系到WGS-84坐标系的旋转矩阵；

X、Y、Z为坐标系的三个坐标轴；l_x、l_y、l_z为偏心量在三个坐标轴上的分量；

为扫描点在WGS-84坐标系下的坐标；

为当地水平坐标系原点在WGS-84坐标系下的空间直角坐标；

为扫描点在激光扫描仪坐标系下的坐标；

为激光扫描仪坐标系与载体坐标系之间的偏心量；

b建立数学函数模型

b.1基础平差模型

对于t个点，定义向量x_c∈R^u为标定参数，x_p∈R^s为平面参数，l∈Rⁿ为观测向量；u、s、n分别表示标定参数x_c、平面参数x_p和观测向量l的个数；

观测向量值：l＝[X_oe Y_oe Z_oe r p y ρ θ φ]；

其中，X_oe、Y_oe、Z_oe表示当地水平坐标系原点在WGS-84坐标系下的空间直角坐标系中的三个坐标；r、p、y分别表示惯导记录姿态角测量值侧滚角、俯仰角和偏航角；ρ表示测量点到激光扫描仪原点的距离，θ表示激光束扫描竖直角，φ表示激光束扫描水平角；

对于Gauss-Helmert平差模型，其一般线性化形式为：

其中，为未知待估计参数，A为对未知参数求一阶偏导后的设计矩阵，v表示观测值的改正数，B为对观测量求一阶偏导后的设计矩阵，w为闭合差向量；

b.2基于控制平面的函数模型

以Gauss-Helmert平差模型为基础，即公式(2)，基于控制平面的函数模型有：

其中，

其中，X_e、Y_e、Z_e为扫描点在WGS-84坐标系下坐标；f_c表示控制平面的函数模型，为观测值平差值，表示为标定参数初始值，为构建模型标定参数；

a_p、b_p、c_p、d_p表示平面参数；I表示单位矩阵，表示误差补偿矩阵，为旋转矩阵的近似值；Δκ、Δω、Δl_x、Δl_y、Δl_z为待标定参数；

对式(3)进行线性化得Gauss-Helmert模型：

其中，A_c表示为对标定参数求偏导的设计矩阵；B_c表示对观测量求偏导后的设计矩阵；即为标定参数改正平差值，为观测值的改正数，w_c为该方程的闭合差向量；

b.3基于约束平面的函数模型

移动测量系统获取的具有平面特征的点云数据所满足的共面方程为：

ax+by+cz-d＝0 (5)

其中，a、b、c为平面的法线向量，d为原点到平面的距离；

平面参数满足的约束条件如下：

其中，为待估计平面参数，为平面参数满足的约束条件方程，表示平面法线向量参数；

其中，f_p表示所列约束平面函数模型；表示对标定参数求偏导的设计矩阵，表示对平面参数求偏导的设计矩阵；表示对观测量求偏导的设计矩阵；为约束平面闭合差向量，为标定参数初始值，为平面参数初始值；表示平面参数改正平差值，表示约束平面观测值改正数；

并有约束条件，对式(6)线性化有：

其中，表示为约束函数关于平面参数的偏导矩阵，g表示为约束函数模型，为平面参数，表示约束平面对应的约束条件的改正向量平差值；

c建立综合函数模型

定义m为平面参数已知的控制平面个数，1≤j≤m，其中，j表示为第j个控制平面；k为平面参数未知的约束平面个数，1≤q≤k，其中，q表示为第q个约束平面；

c.1控制平面累加

基于基础模型，控制平面不断累加，对于m个不断添加的已知的控制平面，函数模型变化为：

对第j个控制平面，有：

其中，表示对第j个控制平面标定参数求偏导的设计矩阵；

表示对第j个控制平面观测量求偏导后的设计矩阵；

为第j个控制平面观测值的改正数；为第j个控制平面闭合差向量；

对标定参数，求偏导的设计矩阵：

其中，表示标定参数偏导向量，1≤i≤t；

对观测量求偏导后的设计矩阵：

其中，表示观测值偏导向量，v_i表示观测值改正向量，1≤i≤t；

c.2约束平面累加

在基于控制平面函数模型基础上，不断添加未知平面参数的约束平面，函数模型变化为：

第q个未知平面，1≤q≤k，线性化后的Gauss-Helmert模型是：

并有约束条件：

其中，表示第q个约束平面对标定参数求偏导的设计矩阵；

表示对第q个约束平面的平面参数求偏导后的设计矩阵；

表示对第q个约束平面观测量求偏导后的设计矩阵；

表示第q个约束平面的平面参数改正平差值；

为第q个约束平面观测值改正数；

为表示第q个约束平面闭合差向量；

其中，为第q个约束平面的平面参数初始值；为第1个约束方程关于平面参数的偏导矩阵，为第q个约束方程关于的偏导矩阵，为第q个约束平面的平面参数；为第q个约束平面对应的约束条件的闭合差向量；表示第1个约束平面对应的约束条件的改正向量平差值，为第q个约束平面对应的约束条件的改正向量平差值；表示第q个约束平面的平面参数改正平差值；

对标定参数求偏导的设计矩阵：

其中，表示视准轴误差参数偏导向量，1≤i≤t；

对平面参数求偏导的设计矩阵：

其中，表示约束平面对平面参数求偏导的向量，

表示约束平面对应的约束条件对平面参数求偏导的向量；

对观测量求偏导后的设计矩阵：

其中，表示观测量偏导向量，1≤i≤t；

c.3最终法方程形式

对于m个控制平面，k个约束平面，根据综合标定函数模型，求解其法方程最终形式为：

式中， 表示观测值的权重矩阵，表示约束条件的权重矩阵；

通过法方程，计算求解出标定参数改正平差值平面参数改正平差值

c.4迭代解算

将通过法方程估计出的标定参数改正平差值和平面参数改正平差值与规定的限差相比较，若改正平差值的绝对值均小于限差，则计算结束；

否则，利用参数初始值加入改正，即作为新的近似值，通过控制平面和约束平面循环累加得到的法方程重新计算参数的改正平差值，并再次比较参数的改正平差值的绝对值与限差大小，做出是否继续求解的判断；

如此重复进行下去，直到改正值均小于限差为止。