WO2020151213A1 - 一种空地结合的潮间带一体化测绘方法 - Google Patents

Abstract

一种空地结合的潮间带一体化测绘方法，通过原始数据采集下载后，进行点云时空融合，完成从仪器坐标系到地心地固坐标系的转换；通过获取单波束点云与激光扫描仪点云、激光扫描仪点云与机载激光探测与测量***点云数据之间的点、线、面等几何特征来建立对应关系，求解旋转矩阵和平移量进行点云匹配。集成机载激光探测与测量***和拖曳测量***，可以获取潮间带区域水下地形和水上地形数据，解决了潮间带地形测量问题。两者能完美覆盖测量盲区，实现潮间带区域一体化无缝测量。

Description

一种空地结合的潮间带一体化测绘方法 技术领域

本发明属于海洋测绘技术领域，具体涉及以一种空地结合的方式进行潮间带区域水上和水下地形快速获取的测绘方法。

背景技术

潮间带属于海岸带的一部分，易受到潮汐的影响，测量条件比较困难。当前，海岸带测绘技术主要有人工实地测量、船载测量和航测遥感等方式。自然海岸带通常存在礁石、滩涂、湿地等特殊地理环境，以传统人工实地测量和船载测量方式难以高效地进行作业，甚至存在危及人身安全的作业风险，无法保障海岸带基础地理信息的快速更新。航测遥感技术手段具有范围广、频度高和实时的优点，被广泛应用于海洋测绘，但是由于受到卫星重复周期、海岸带云雨天气、分辨率低等影响，难以满足动态监测和水下地形的高精度测量需求，对潮水覆盖区域尤其是大面积浅滩和宽阔滩涂区域无能为力。

船载多传感器水岸一体化综合测量技术的发展，为海岸带一体化测量提供了依据。但当海岸带地形以滩涂为主且潮差较小时，由于测量船和测深仪存在吃水问题使得基于船载激光测量无法对潮间带进行扫测，出现潮间带的测量“盲区”。

机载LiDAR技术具有高便捷、无控制、高精度、高密集、高效率、高分辨率、多回波等特点，作为精确、快速地获取地面三维数据的工具已得到广泛的认同，但由于潮间带地形特殊性，激光信号受地物遮挡、水面反射等影响，也会出现测量“盲区”。

发明内容

针对现有方法中存在的问题，本发明提出一种空地结合的潮间带一体化测绘方法，机载激光探测与测量***结合拖曳一体化测量***，解决了潮间带常见的测量问题，具有良好的效果。

本发明采用以下的技术方案：

一种空地结合的潮间带一体化测绘方法，包括测量装置，测量装置包括无人机和载体车，无人机上搭载激光探测与测量***，载体车上设置有激光扫描仪、单波束测深仪、惯性导航***和卫星定位***；

测绘方法包括以下步骤：

步骤1：原始数据采集下载后，检查各测量仪器观测数据的完整性、正确性，进行观测数据文件的格式转换，进行各测量仪器高精度时间同步，各测量仪器数据的时、空融合，通过点云数据预处理，形成标准LAS数据文件；

步骤2：进行点云时空融合，完成从仪器坐标系到地心地固坐标系的转换；

步骤3：通过获取单波束点云与激光扫描仪点云、激光扫描仪点云与机载LIDAR点云数据之间的点、线、面等几何特征来建立对应关系，求解旋转矩阵和平移量进行点云匹配。

优选地，所述步骤1包括以下子步骤：

步骤1.1：利用硬件同步控制器进行各测量仪器高精度时间同步，各测量仪器数据的时、空融合，硬件同步控制器的时间保持与卫星定位***的时间同步；硬件同步控制器控制各测量仪器的触发及监测其反馈信号，信号的传输都采用差分方式；

步骤1.2：利用IE进行基准站和流动站数据结算，输出POS数据；

步骤1.3：利用VSursPROCESS软件进行点云数据预处理，形成标准LAS数据文件。

优选地，所述步骤2包括以下子步骤：

步骤2.1：从激光扫描仪坐标系到载体坐标系转换；

根据标定的公共点计算坐标系转换需要的的6参数：l _x、l _y、l _z、ω、

κ；

l _x、l _y、l _z分别为坐标转换过程中点坐标沿载体坐标系x轴、y轴、z轴的平移量；

ω、

κ分别为坐标转换过程中点坐标绕载体坐标系x轴、y轴、z轴的旋转角度；

设激光扫描仪坐标系下点坐标为

在载体坐标系下的坐标为

则有，

其中，

代表从激光扫描仪坐标系到载体坐标系转换的旋转矩阵，R _x(ω)、

R _z(κ)分别为绕载体坐标系x轴、y轴、z轴旋转的旋转矩阵；

步骤2.2：从载体坐标系到站心坐标系转换；

惯导坐标系前进方向为y轴、向右为x轴、向上为z轴；

惯导记录姿态角：侧滚角Roll、俯仰角Pitch、偏航角Heading；

侧滚角Roll：惯导x轴与水平方向之间的夹角，载体右侧向下为正；

俯仰角Pitch：惯导y轴与水平方向之间的夹角，载体向上为正；

偏航角Heading：惯导前进方向，即xy平面与正北方向之间的夹角，顺时针为正；

设Roll、Pitch、Heading分别为r、p、y；设点在站心坐标系下的坐标为

载体坐标系下坐标向站心坐标系转换；

①先绕z轴旋转y；

②再绕x轴旋转p；

③最后绕y轴旋转r；

则有，

其中，

代表从载体坐标系到站心坐标系转换的旋转矩阵，R _x(p)、R _y(r)、R _z(y)分别为绕站心坐标系x轴、y轴、z轴旋转的旋转矩阵；

步骤2.3：从站心坐标系到地心地固坐标系转换；

站心坐标系原点在WGS84下的经纬度分别为L和B；

设扫描点在地心地固坐标系下的坐标为

站心坐标系下的坐标转换为WGS84坐标系下的坐标：

①先绕x轴旋转

②再绕z轴旋转

③最后将站心坐标系原点平移到WGS84坐标系原点；

则有，

其中，

代表从站心坐标系到地心地固坐标系转换的旋转矩阵；

其中，N代表卯酉圈曲率半径，h代表大地高，a、b代表椭球长、短半径；

得

为站心坐标系原点在地心地固坐标系下的空间直角坐标；

最后得，

优选地，所述步骤3包括：

首先介绍一下所需要的理论基础：

平面拟合原理

已知平面方程：a′x+b′y+c′z＝d     (6)

且a′ ²+b′ ²+c′ ²＝1，d≥0，(x y z)是平面上的任意点；

其中，(a′ b′ c′)代表垂直于平面且远离坐标原点方向的方向余弦；

同样的，它也表示垂直于平面远离坐标原点的单位法向量的分量；

d表示了平面和坐标原点间的垂直距离；

最小二乘算法基本原理

最小二乘法的思想就是估算一个最佳数学模型，求得最佳模型参数使估算模型点的值和实际观察值的差值的平方和最小；

在二维数据中，假设给出模型函数g(x _i)，g(x _i)与真实值(x _i,y _i)的误差可用三种形式表述：

①误差最大值，即

其中n为点总个数；

②误差绝对值之和，即

③误差平方和，即

最小二乘算法的原理便是寻找一个最佳参数模型，对于给定的数据(x _i,y _i)(i＝0,1...n)，误差平方和

最小；

最小二乘平面拟合算法

基于最小二乘法的平面拟合算法是在三维点云数据的基础上进行的拟合算法，以公式(6)为三维模型，确定a′,b′,c′,d四个参数使三维模型的估算值和测量值之间的差值平方和E ²最小；由于E ²是非负数，因此它存在极小值，E ²对每个参数的偏导数为零，利用偏导数为零构建的方程组便可求解出a′,b′,c′,d，并使估算值和测量值之间的差值平方和E ²最小；

假设点云数据的点集为{p _i|p _i∈oxyz,i＝1,2,3...,N}，其中x _i和y _i设为无误差变化的自变量，z _i是包含误差的因变量，则z和x、y的函数相关性可以假设为函数表达式：

z＝f(x,y；p,q,r)＝p+qx+ry    (7)

且其中的p,q,r可以通过最小二乘算法得到；

根据公式(6)、(7)，令d＝c′p，a′＝-qc′，b′＝-rc′且

则公式(7)可以描述为平面方程；

根据上述定义f函数的公式，假如自变量的值为x _i和y _i，被测量值z应该为p+qx _i+ry _i，然而因为误差的存在测量值z为z _i，则第i个点的测量误差为：

e _i＝f(x _i, _i；p,q,r)-z _i   (8)

根据最小二乘的原理，p,q,r应该满足公式(8)的平方和最小，即E ²最小：

若使得E ²最小，应满足：

即：

有：

根据公式(11)可以求解出p,q,r，同样的，公式(6)中a′,b′,c′,d四个参数也就可通过d＝c′p，a′＝-qc′，b′＝-rc′，

求出；

求解旋转矩阵

在上述算法的计算过程中，提取3对以上对应的平面特征量后，需要求取其各自的平面法向量；

假设求解的一对平面为a ₁x+b ₁y+z＝d ₁和a ₂x+b ₂y+z＝d ₂，则其法向量对可表示为P＝(a ₁,b ₁,-1)、Q＝(a ₂,b ₂,-1)，法向量的旋转矩阵R满足P＝RQ，采用罗德里格旋转法求解；上述P,Q矩阵，其夹角为：

P,Q矩阵的叉乘为：

若已知单位向量

利用法向量将其旋转θ角度后，根据罗德里格旋转法求解其三维旋转矩阵为：

即可求出

的三维变换矩阵C ₁；

求解平移量

利用三维变换矩阵C ₁可以对点云数据进行旋转，相应的平面特征量对相互平行，在两片点云数据中寻找一对以上的特征点，或利用对应面的的质心坐标的差值，计算点云数据的平移量即：

本发明具有的有益效果是：

本发明提供的一种空地结合的潮间带一体化测绘方法，集成了机载激光探测与测量***和拖曳测量***，可以获取潮间带区域水下地形和水上地形数据，解决了潮间带地形测量问题。两者能完美覆盖测量“盲区”，实现潮间带区域一体化无缝测量。

附图说明

图1为本发明提供的潮间带载体车外业采集流程图。

图2为本发明提供的机载激光探测与测量***外业采集流程图。

图3为本发明提供的单波束数据格式转换流程图。

图4为本发明提供的数据解算流程图。

图5为本发明提供的单波束测深仪数据、激光扫描仪数据、机载激光探测与测量***数据空间整合流程图。

具体实施方式

下面结合附图和具体实施例对本发明的具体实施方式做进一步说明：

结合图1至图5，一种空地结合的潮间带一体化测绘方法，包括测量装置，测量装置包括无人机和载体车，无人机上搭载激光探测与测量***，载体车上设置有激光扫描仪、单波束测深仪、惯性导航***和卫星定位***。

测绘方法包括以下步骤：

步骤1：原始数据采集下载后，检查各测量仪器观测数据的完整性、正确性，进行观测数据文件的格式转换，进行各测量仪器高精度时间同步，各测量仪器数据的时、空融合，通过点云数据预处理，形成标准LAS数据文件。

各测量仪器观测数据包括激光扫描仪数据、激光探测与测量***数据、单波束测深仪数据、惯性导航***数据和卫星定位***数据等。

具体的，步骤1包括以下子步骤：

步骤1.1：如图1和图2所示，分别为载体车和机载激光探测与测量***数据采集流程图。图3为单波束数据格式转换流程图。

利用硬件同步控制器进行各测量仪器高精度时间同步，各测量仪器数据的时、空融合，它作用是为***中的激光扫描仪、单波束测深仪等仪器提供相应的同步信号，并记录相应的时间，为后期的数据处理提供一个时间同步基准，硬件同步控制器的时间保持与卫星定位***的时间同步；硬件同步控制器控制各测量仪器的触发及监测其反馈信号，考虑到环境因素 复杂，为保证***的高可靠性，信号的传输都采用差分方式；

步骤1.2：如图4所示，利用IE进行基准站和流动站数据结算，输出POS数据；

步骤1.3：利用VSursPROCESS软件进行点云数据预处理，形成标准LAS数据文件。

步骤2：进行点云时空融合，完成从仪器坐标系到地心地固坐标系的转换。

步骤2包括以下子步骤：

步骤2.1：从激光扫描仪坐标系到载体坐标系转换；

根据标定的公共点计算坐标系转换需要的的6参数：l _x、l _y、l _z、ω、

κ；

l _x、l _y、l _z为坐标转换过程中点坐标沿载体坐标系x轴、y轴、z轴的平移量；

ω、

κ为坐标转换过程中点坐标绕载体坐标系x轴、y轴、z轴的旋转角度；

设激光扫描仪坐标系下点坐标为

在载体坐标系下的坐标为

则有，

其中，

代表从激光扫描仪坐标系到载体坐标系转换的旋转矩阵，R _x(ω)、

R _z(κ)分别为绕载体坐标系x轴、y轴、z轴旋转的旋转矩阵。

步骤2.2：从载体坐标系到站心坐标系转换；

惯导坐标系前进方向为y轴、向右为x轴、向上为z轴；

惯导记录姿态角：侧滚角Roll、俯仰角Pitch、偏航角Heading。

侧滚角Roll：惯导x轴与水平方向之间的夹角，载体右侧向下为正；

俯仰角Pitch：惯导y轴与水平方向之间的夹角，载体向上为正；

偏航角Heading：惯导前进方向，即xy平面与正北方向之间的夹角，顺时针为正；

设Roll、Pitch、Heading分别为r、p、y；设点在站心坐标系下的坐标为

载体坐标系下坐标向站心坐标系转换；

①先绕z轴旋转y；

②再绕x轴旋转p；

③最后绕y轴旋转r；

则有，

其中，

代表从载体坐标系到站心坐标系转换的旋转矩阵，R _x(p)、R _y(r)、R _z(y)分别为绕站心坐标系x轴、y轴、z轴旋转的旋转矩阵。

步骤2.3：从站心坐标系到地心地固坐标系转换；

站心坐标系原点在WGS84下的经纬度分别为L和B；

设扫描点在地心地固坐标系下的坐标为

站心坐标系下的坐标转换为WGS84坐标系下的坐标：

①先绕x轴旋转

②再绕z轴旋转

③最后将站心坐标系原点平移到WGS84坐标系原点；

则有，

代表从站心坐标系到地心地固坐标系转换的旋转矩阵。

其中，N代表卯酉圈曲率半径，h代表大地高，a、b代表椭球长、短半径。

得

为站心坐标系原点在地心地固坐标系下的空间直角坐标；

最后得，

步骤3：通过获取单波束点云与激光扫描仪点云、激光扫描仪点云与机载LIDAR点云数据之间的点、线、面等几何特征来建立对应关系，求解旋转矩阵和平移量进行点云匹配。

步骤3包括：

首先介绍一下所需要的理论基础：

平面拟合原理

已知平面方程：

a′x+b′y+c′z＝d   (6)

且a′ ²+b′ ²+c′ ²＝1，d≥0，(x y z)是平面上的任意点；

其中(a′ b′ c′)代表垂直于平面且远离坐标原点方向的方向余弦；

同样的，它也表示垂直于平面远离坐标原点的单位法向量的分量；

d表示了平面和坐标原点间的垂直距离。

最小二乘算法基本原理

最小二乘法的思想就是估算一个最佳数学模型，求得最佳模型参数使估算模型点的值和 实际观察值的差值的平方和最小；

在二维数据中，假设给出模型函数g(x _i)，g(x _i)与真实值(x _i,y _i)的误差可用三种形式表述：

①误差最大值，即

其中n为点总个数；

②误差绝对值之和，即

③误差平方和，即

最小二乘算法的原理便是寻找一个最佳参数模型，对于给定的数据(x _i,y _i)(i＝0,1...n)，误差平方和

最小；

最小二乘平面拟合算法

基于最小二乘法的平面拟合算法是在三维点云数据的基础上进行的拟合算法，以公式(6)为三维模型，确定a′,b′,c′,d四个参数使三维模型的估算值和测量值之间的差值平方和E ²最小；由于E ²是非负数，因此它存在极小值，E ²对每个参数的偏导数为零，利用偏导数为零构建的方程组便可求解出a′,b′,c′,d，并使估算值和测量值之间的差值平方和E ²最小；

假设点云数据的点集为{p _i|p _i∈oxyz,i＝1,2,3...,N}，其中x _i和y _i设为无误差变化的自变量，z _i是包含误差的因变量，则z和x、y的函数相关性可以假设为函数表达式：

z＝f(x,y；p,q,r)＝p+qx+ry    (7)

且其中的p,q,r可以通过最小二乘算法得到；

根据公式(6)、(7)，令d＝c′p，a′＝-qc′，b′＝-rc′且

则公式(7)可以描述为平面方程；

根据上述定义f函数的公式，假如自变量的值为x _i和y _i，被测量值z应该为p+qx _i+ry _i，然而因为误差的存在测量值z为z _i，则第i个点的测量误差为：

e _i＝f(x _i,y _i；p,q,r)-z _i   (8)

根据最小二乘的原理，p,q,r应该满足公式(8)的平方和最小，即E ²最小：

若使得E ²最小，应满足：

即：

有：

根据公式(11)可以求解出p,q,r，同样的，公式(6)中a′,b′,c′,d四个参数也就可通过d＝c′p，a′＝-qc′，b′＝-rc′，

求出。

求解旋转矩阵

在上述算法的计算过程中，提取3对以上对应的平面特征量后，需要求取其各自的平面法向量；

假设求解的一对平面为a ₁x+b ₁y+z＝d ₁和a ₂x+b ₂y+z＝d ₂，则其法向量对可表示为P＝(a ₁,b ₁,-1)、Q＝(a ₂,b ₂,-1)，法向量的旋转矩阵R满足P＝RQ，采用罗德里格旋转法求解；上述P,Q矩阵，其夹角为：

P,Q矩阵的叉乘为：

若已知单位向量

利用法向量将其旋转θ角度后，根据罗德里格旋转法求解其三维旋转矩阵为：

即可求出

的三维变换矩阵C ₁；

求解平移量

利用三维变换矩阵C ₁可以对点云数据进行旋转，相应的平面特征量对相互平行，在两片点云数据中寻找一对以上的特征点，或利用对应面的的质心坐标的差值，计算点云数据的平移量即：

当然，上述说明并非是对本发明的限制，本发明也并不仅限于上述举例，本技术领域的技术人员在本发明的实质范围内所做出的变化、改型、添加或替换，也应属于本发明的保护范围。

Claims (4)

1. 一种空地结合的潮间带一体化测绘方法，其特征在于，包括测量装置，测量装置包括无人机和载体车，无人机上搭载激光探测与测量***，载体车上设置有激光扫描仪、单波束测深仪、惯性导航***和卫星定位***；

   测绘方法包括以下步骤：

   步骤1：原始数据采集下载后，检查各测量仪器观测数据的完整性、正确性，进行观测数据文件的格式转换，进行各测量仪器高精度时间同步，各测量仪器数据的时、空融合，通过点云数据预处理，形成标准LAS数据文件；

   步骤2：进行点云时空融合，完成从仪器坐标系到地心地固坐标系的转换；

   步骤3：通过获取单波束点云与激光扫描仪点云、激光扫描仪点云与机载LIDAR点云数据之间的点、线、面等几何特征来建立对应关系，求解旋转矩阵和平移量进行点云匹配。
2. 根据权利要求1所述的一种空地结合的潮间带一体化测绘方法，其特征在于，所述步骤1包括以下子步骤：

   步骤1.1：利用硬件同步控制器进行各测量仪器高精度时间同步，各测量仪器数据的时、空融合，硬件同步控制器的时间保持与卫星定位***的时间同步；硬件同步控制器控制各测量仪器的触发及监测其反馈信号，信号的传输都采用差分方式；

   步骤1.2：利用IE进行基准站和流动站数据结算，输出POS数据；

   步骤1.3：利用VSursPROCESS软件进行点云数据预处理，形成标准LAS数据文件。
3. 根据权利要求1所述的一种空地结合的潮间带一体化测绘方法，其特征在于，所述步骤2包括以下子步骤：

   步骤2.1：从激光扫描仪坐标系到载体坐标系转换；

   根据标定的公共点计算坐标系转换需要的的6参数：l _x、l _y、l _z、ω、

   

   κ；

   l _x、l _y、l _z分别为坐标转换过程中点坐标沿载体坐标系x轴、y轴、z轴的平移量；

   ω、

   

   κ分别为坐标转换过程中点坐标绕载体坐标系x轴、y轴、z轴的旋转角度；

   设激光扫描仪坐标系下点坐标为

   

   在载体坐标系下的坐标为

   

   则有，

   

   其中，

   

   代表从激光扫描仪坐标系到载体坐标系转换的旋转矩阵，R _x(ω)、

   

   R _z(κ)分别为绕载体坐标系x轴、y轴、z轴旋转的旋转矩阵；

   

   步骤2.2：从载体坐标系到站心坐标系转换；

   惯导坐标系前进方向为y轴、向右为x轴、向上为z轴；

   惯导记录姿态角：侧滚角Roll、俯仰角Pitch、偏航角Heading；

   侧滚角Roll：惯导x轴与水平方向之间的夹角，载体右侧向下为正；

   俯仰角Pitch：惯导y轴与水平方向之间的夹角，载体向上为正；

   偏航角Heading：惯导前进方向，即xy平面与正北方向之间的夹角，顺时针为正；

   设Roll、Pitch、Heading分别为r、p、y；设点在站心坐标系下的坐标为

   

   载体坐标系下坐标向站心坐标系转换；

   ①先绕z轴旋转y；

   ②再绕x轴旋转p；

   ③最后绕y轴旋转r；

   则有，

   

   其中，

   

   代表从载体坐标系到站心坐标系转换的旋转矩阵，R _x(p)、R _y(r)、R _z(y)分别为绕站心坐标系x轴、y轴、z轴旋转的旋转矩阵；

   

   步骤2.3：从站心坐标系到地心地固坐标系转换；

   站心坐标系原点在WGS84下的经纬度分别为L和B；

   设扫描点在地心地固坐标系下的坐标为

   

   站心坐标系下的坐标转换为WGS84坐标系下的坐标：

   ①先绕x轴旋转

   

   ②再绕z轴旋转

   

   ③最后将站心坐标系原点平移到WGS84坐标系原点；

   则有，

   

   其中，

   

   代表从站心坐标系到地心地固坐标系转换的旋转矩阵；

   

   

   其中，N代表卯酉圈曲率半径，h代表大地高，a、b代表椭球长、短半径；

   

   

   得

   

   

   为站心坐标系原点在地心地固坐标系下的空间直角坐标；

   最后得，

   
4. 根据权利要求1所述的一种空地结合的潮间带一体化测绘方法，其特征在于，所述步骤3包括：

   首先介绍一下所需要的理论基础：

   平面拟合原理

   已知平面方程：a′x+b′y+c′z＝d           (6)

   且a′ ²+b′ ²+c′ ²＝1，d≥0，(x y z)是平面上的任意点；

   其中，(a′ b′ c′)代表垂直于平面且远离坐标原点方向的方向余弦；

   同样的，它也表示垂直于平面远离坐标原点的单位法向量的分量；

   d表示了平面和坐标原点间的垂直距离；

   最小二乘算法基本原理

   最小二乘法的思想就是估算一个最佳数学模型，求得最佳模型参数使估算模型点的值和实际观察值的差值的平方和最小；

   在二维数据中，假设给出模型函数g(x _i)，g(x _i)与真实值(x _i,y _i)的误差可用三种形式表述：

   ①误差最大值，即

   

   其中n为点总个数；

   ②误差绝对值之和，即

   

   ③误差平方和，即

   

   最小二乘算法的原理便是寻找一个最佳参数模型，对于给定的数据(x _i,y _i)(i＝0,1…n)，误差平方和

   

   最小；

   最小二乘平面拟合算法

   基于最小二乘法的平面拟合算法是在三维点云数据的基础上进行的拟合算法，以公式(6)为三维模型，确定a′,b′,c′,d四个参数使三维模型的估算值和测量值之间的差值平方和E ²最小；由于E ²是非负数，因此它存在极小值，E ²对每个参数的偏导数为零，利用偏导数为零构建的方程组便可求解出a′,b′,c′,d，并使估算值和测量值之间的差值平方和E ²最小；

   假设点云数据的点集为{p _i|p _i∈oxyz,i＝1,2,3...,N}，其中x _i和y _i设为无误差变化的自变量，z _i是包含误差的因变量，则z和x、y的函数相关性可以假设为函数表达式：

   z＝f(x,y；p,q,r)＝p+qx+ry      (7)

   且其中的p,q,r可以通过最小二乘算法得到；

   根据公式(6)、(7)，令d＝c′p，a′＝-qc′，b′＝-rc′且

   

   则公式(7) 可以描述为平面方程；

   根据上述定义f函数的公式，假如自变量的值为x _i和y _i，被测量值z应该为p+qx _i+ry _i，然而因为误差的存在测量值z为z _i，则第i个点的测量误差为：

   e _i＝f(x _i,y _i；p,q,r)-z _i        (8)

   根据最小二乘的原理，p,q,r应该满足公式(8)的平方和最小，即E ²最小：

   

   若使得E ²最小，应满足：

   

   即：

   

   有：

   

   其中i＝1…n   (12)

   根据公式(11)可以求解出p,q,r，同样的，公式(6)中a′,b′,c′,d四个参数也就可通过d＝c′p，a′＝-qc′，b′＝-rc′，

   

   求出；

   求解旋转矩阵

   在上述算法的计算过程中，提取3对以上对应的平面特征量后，需要求取其各自的平面法向量；

   假设求解的一对平面为a ₁x+b ₁y+z＝d ₁和a ₂x+b ₂y+z＝d ₂，则其法向量对可表示为P＝(a ₁,b ₁,-1)、Q＝(a ₂,b ₂,-1)，法向量的旋转矩阵R满足P＝RQ，采用罗德里格旋转法求解；上述P,Q矩阵，其夹角为：

   

   P,Q矩阵的叉乘为：

   

   若已知单位向量

   

   利用法向量将其旋转θ角度后，根据罗德里格旋转法求解其三维旋转矩阵为：

   

   即可求出

   

   的三维变换矩阵C ₁；

   求解平移量

   利用三维变换矩阵C ₁可以对点云数据进行旋转，相应的平面特征量对相互平行，在两片点云数据中寻找一对以上的特征点，或利用对应面的的质心坐标的差值，计算点云数据的平移量即：

   

Images