CN113050074B - 无人驾驶环境感知中相机与激光雷达标定系统及标定方法 - Google Patents

Abstract

本发明公开了一种无人驾驶环境感知中相机与激光雷达标定系统及标定方法，基于一种定制标定板的传感器系统，分别对相机和激光雷达所采集的数据进行处理，得到两传感器各自坐标下标定板上镂空圆圆心的三维坐标，再基于改进的ICP(迭代最近点)算法对两组点集进行配准，解算相机与激光雷达之间的相对位姿关系，得到标定结果。本发明提供一种无人驾驶环境感知中相机与激光雷达标定系统及算法，其通过改进的算法使得软件系统或者算法具有较强的鲁棒性，提升了标定精度，为相机与激光雷达数据融合做准备。

Description

无人驾驶环境感知中相机与激光雷达标定系统及标定方法

技术领域

本发明涉及无人驾驶领域。更具体地说，本发明涉及一种无人驾驶环境感知中相机与激光雷达标定系统及标定方法。

背景技术

在无人驾驶中，车辆必须实时的感知周围的环境，其任务包括目标检测及分类、多目标追踪、场景理解。在目标检测和场景理解领域，障碍物检测、车道线检测、行人识别、交通标志识别、红绿灯识别等是首要需要解决的任务。相机作为一种视觉传感器，已成熟应用在目标检测领域，自从2012年Alexnet卷积神经网络在图片分类问题上的突破，基于深度学习的目标检测网络如雨后春笋般涌现，RCNN、MS-CNN、YOLO、FastRCNN、DCNN等一系列2D目标检测网络相继提高了检测准确度和速度。然而，相机具有一些局限，采集的数据是RGB图像的像素阵列，没有深度信息(距离信息)，双目相机可以完成测距工作，但是算法不够成熟，其测量误差与雷达等专业测距设备差距太大。相机本身视场角也有限，摄像头受外界条件的影响也比较大，如光线的强度。激光雷达能精确的采集带有距离信息的3D点云数据，不受光照影响，并且感知距离能达到150m，但在角分辨度上远远不如相机，在复杂场景下，如小物体和远距离物体，相机可以清楚的识别目标，但激光雷达采集到的目标物体点是比较少的，难以聚类，这不利于使用点云数据实现目标检测。且容易受雨雪、沙尘、雾霾等影响。毫米波雷达受自身性能所限，无法反馈高度信息。它可以探测到物体所在的位置，但无法探测它的高度，如路边存在一个较高的指路牌，毫米波雷达能探测代此处存在障碍物，却不知道指路牌是悬挂于道路上方。

由于自动驾驶车辆在单一传感器的环境感知任务中遇到越来越大的挑战和需要实现更多的功能，多传感器融合方案受到越来越多研究人员的青睐。多传感器融合技术能解决单一传感器在自动驾驶中的局限。通过多传感器数据融合，传感器之间能取长补短，提升整个感知系统的效果。激光雷达测距具有精度高的特点，而且可以提供360度全视野的信息，由于传感器各自的特殊性，它们适合作为同一感知系统的一部分。多传感器深度融合可以实现将3D点云数据映射到图像上，即图像上的某些像素也具有了深度信息，可以帮助感知系统进行基于图像的分割或目标检测。传感器信息融合技术的基本原理与人的大脑综合处理信息的过程一样，各传感器之间进行多层次、多空间的信息互补，优化组合处理，最终产生对环境信息的一致性解释。信息融合的最终目标是基于各传感器获得的分离观测信息，通过信息多级别、多方面的组合而导出更多有效信息。传感器融合利用了多个传感器相互协同操作的优势，而且也综合处理其他信息源的数据来提高整个感知系统的智能化。多传感器联合标定是多传感器数据融合的前提，是移动机器人、无人驾驶技术等研究领域的核心问题。

专利CN202010084392.X提供了一种基于球体标定物的标定方法，通过获取图像中球心的二维坐标和点云中球心的三维坐标，构成一组相互对应的2D-3D位置数据对，再使用pnp方法求解旋转矩阵和平移向量。该方法使用一个球体，由于至少需要4对相互对应的2D-3D位置数据对，需要人为摆放标定物4次，得到4张图像数据和4帧点云数据再分别进行处理，不仅增加了工作量，还会人为引入误差，且2D-3D对应点集求解是一个PNP问题，此类问题求解的结果通常不稳定。

专利CN201910639761.4中，采用了一种基于正方形棋盘格标定物的标定方法，首先对双目相机进行标定，再对双目图像进行校正，得到棋盘格角点的深度值，再利用相机内参矩阵得到角点三维坐标，最终生成了标定板的点云图，点云处理中，也是利用RANSAC算法来拟合标定板平面方程，通过将所有角点投影到拟合平面上，再提取拟合平面上标定板两条对角线上的角点来拟合直线方程，得到棋盘格4个顶点三维坐标。激光雷达则是通过相邻交边确定标定板顶点坐标，最后利用ICP算法来求解相机与激光雷达之间的相对位姿关系。该方法涉及相机标定、图像矫正、生成点云等，不仅增加了工作量，且在双目标定的过程中容易出错，导致得到的角点深度值误差大，其次，由于受光照影响大，再三角测量时易导致三维坐标计算出错。另外，由于采用传统的ICP算法配准，在数据包含包含噪点的情况下，配准结果不够精确。

由此可知，现有的标定系统其标定方式、计算方法复杂，且在对坐标标定算法进行计算时，其标定配准的精度达不到要求，将二维与三维，三维与三维两幅图进行配准时，现在的标定方法通常会产生投影，不能满足使用要求和测量要求。

发明内容

本发明的一个目的是解决至少上述问题和/或缺陷，并提供至少后面将说明的优点。

为了实现根据本发明的这些目的和其它优点，提供了一种无人驾驶环境感知中相机与激光雷达标定系统及标定方法，包括：

步骤一、基于三维激光雷达、RGB-D相机、镂空标定板搭建车载的传感器系统，其中标定板上设置有四个镂空圆；

步骤二、基于相机与镂空标定板的配合，通过计算得到标定板上各镂空位置的中心在三维空间下的多个第一组圆心坐标；

步骤三、基于三维激光雷达与镂空标定板的配合，通过计算得到标定板各镂空位置在三维状态下对应的多个第二组圆心坐标；

步骤四、基于标定板上的镂空圆个数，获得多对一一对应的圆心坐标数据对；

步骤五、利用改进的ICP算法解算数据对之间的位置关系，完成二者的标定。

优选的是，在步骤一中，所述三维激光雷达被配置为采用VLP-16线激光雷达，所述RGB-D相机被配置为采用MYNT EYE深度相机；

其中，所述标定板被配置在传感器系统正前方1.5～7m，且标定板能全部在相机的取景图像中，同时至少有2行激光束能扫描到4个镂空圆的边缘；

镂空标定板后面设置有与其大小一致的黄色背景板。

优选的是，在步骤二中，第一组圆心坐标的获取方式被配置为包括：

S21、基于单目采集的RGB图像数据转换成LAB图像，对B通道进行阈值分割，滤除黄色以外的所有颜色；

S22、对图像进行二值化处理、candy边缘检测、最小二乘法对圆进行拟合，以得到标定板镂空圆圆心对应的二维像素坐标；

S23、调用相机自带的深度图数据，将彩色图与深度图对齐，得到与二维像素坐标对应的深度值，并基于以下公式得到圆心在三维空间下的第一组圆心坐标：

x＝(u-c_x)*z/f_x

y＝(v-c_y)*z/f_y

其中，(u，v)为二维像素坐标，(x，y，z)为二维像素坐标对应的第一圆心坐标，d为深度，depth_scale为尺度因子，f_x和f_y为焦距，c_x和c_y为相机主点。

优选的是，在步骤三中，第二组圆心坐标的获取方式被配置为包括：

S31、使用直通滤波滤除标定板周围点云，使用RANSAC算法拟合标定板平面方程；

S32、通过指定一个表示相邻点云深度差的一个量值的方法，过滤掉标定板上的连续点，保留不连续点，去除不属于标定板镂空圆边缘的点云；

S33、将镂空圆边缘上的点映射到二维平面上拟合圆，得圆心二维坐标；

S34、将圆心二维坐标映射到三维空间中，以得到第二组三维坐标。

优选的是，在步骤五中，所述改进的ICP算法被配置为在ICP算法中加入最大相关熵准则(MCC)，其被配置为包括：

S51、基于MCC的局部相似度量函数，建立稀疏ICP算法模型；

S52、对基于MCC的稀疏ICP算法模型进行求解。

优选的是，最大相关熵准则论证方式被配置为包括：

S510、将两个随机变量的局部相似性表示为：V_σ(A,B)＝E[k_σ(A-B)]，其中k_σ(.)是一种内核函数，E是数学期望；

S511、两个有限集数据的相关熵通过/>进行估计，其中，高斯核函数k_σ(.)为：/>

S512、对于任意两个相同维度的向量和/>将之作差得到/>其中e_j＝a_j-b_j，则e_j的最大相关熵为/>在内核宽度σ决定的小范围邻域内，相关熵可以看作是衡量两个随机变量相似度的方法，则相关熵越大，两个变量间的相似度越高，当V_σ(A,B)取得最大值的时候，A,B之间的误差E就最小。

优选的是，基于MCC的稀疏ICP算法模型数学表达式如下所示：

s.t.R^TR＝I,det(R)＝1。

优选的是，稀疏ICP算法模型的求解方式被配置为包括：

S521、对t求偏导：

得到：

其中，w表示迭代时用指数项部分的(E_k-1,R_k-1,t_k-1)代替(E_k,R_k,t_k)；

S522、令：

则含参数R,E的目标函数变为：

S523、令：

N＝Q+E；

则含参数R的目标函数为：

通过半方(HQ)技术，将上式转换为：

其中：

则最大化f(R)等价于：

F(R)＝-tr(H)；

其中：

通过SVD分解H得：

R＝VDU^T；

此时求解稀疏矩阵E，则只含E的目标函数如下：

其中：

S524、令：

RX+t-P＝M；

则等价于：

其中，B是一个以b为对角元素的对角矩阵，上式等价于：

其中：

将上述问题看成一个子问题，有：

解得：

其中：

即A中的每个元素都是：

本发明至少包括以下有益效果：本发明无人驾驶环境感知中相机与激光雷达标定系统及算法，其通过改进的算法使得软件系统或者算法具有较强的鲁棒性，提升了标定精度，为相机与激光雷达数据融合做准备，经仿真实验和实际标定(也可称为配准)，结果表明对比传统ICP算法该方法能够有效提升配准结果的精度。

本发明的其它优点、目标和特征将部分通过下面的说明体现，部分还将通过对本发明的研究和实践而为本领域的技术人员所理解。

附图说明

图1为标定系统流程图；

图2为标定板示意图；

图3为相机视角的标定板示意图；

图4相机获取的标定板镂空圆圆心二维点；

图5为基于ROS的相机自带的深度图；

图6为激光雷达获取的标定板镂空圆圆心三维点；

图7为改进的ICP算法在公共数据集上的二维配准结果；

图8为改进的ICP算法在公共数据集上的三维配准结果；

图9为传统ICP算法在公共数据集上的二维配准结果；

具体实施方式

下面结合附图对本发明做进一步的详细说明，以令本领域技术人员参照说明书文字能够据以实施。

本发明提出了一种RGB-D相机与三维激光雷达系统外参标定的方法，采用的是一个具有4个镂空圆的标定板，该方法能简化标定流程且提高标定精度。

本发明原理是针对图像数据获取镂空圆的圆心像素坐标，通过ROS下的同步机制，得到圆心像素坐标对应的深度值，最后计算出对应的相机坐标系下的三维坐标。针对激光雷达点云数据，经处理同样得到激光雷达坐标系下镂空圆心三维坐标，然后对其进行配准，求解相对位姿关系。

本发明具体步骤如下：

1、搭建传感器系统，固定相机和激光雷达，放置标定板在合理位置。

2、相机获取标定板镂空圆圆心像素坐标；

3、获取镂空圆圆心像素坐标对应的深度值；

4、计算相机坐标系下镂空圆圆心的三维坐标；

5、计算激光雷达坐标系下镂空圆圆心的三维坐标；

6、采用基于最大相关熵准则的稀疏ICP算法对两点集进行配准，求解相机与激光雷达之间的相对位姿关系；

实施例：

1、搭建传感器系统，固定相机和激光雷达，放置标定板在合理位置。

首先，固定相机与激光雷达，标定板为红色的亚克力板，板上有4个相互对称的镂空圆，标定板后面有一个同样大小的黄色背景完整无镂空亚克力板，将该标定装置放置在多传感器系统正前方1.5～7.5m，且垂直于水平面。

其次，调整标定装置离多传感器系统之间的距离和他们之间的相对高度，使标定装置在相机视野中能完全显示处来，且至少有2束激光能扫描到每个镂空圆上。

2、相机获取标定板镂空圆圆心像素坐标。

如图2，由于标定板有4个镂空圆，在黄色背景板的重叠上，图像上呈现的是红色板面和黄色圆，使用相机检测图像中的黄色圆，将RGB图像转换到LAB空间，将LAB三个通道分离出来，B通道对应的是黄色，则对B通道进行阈值分割，设定其范围为(150，240)，可滤除黄色以外所有背景，得到黄色的圆。再对图像进行二值化处理，得到黑白图像，利用candy边缘检测算法检测圆的边缘，最后利用最小二乘法拟合圆，求得4个圆心的像素坐标。

3、相机获取标定板镂空圆圆心三维坐标。

如图3-5，由于RGB-D相机自带环境深度信息，不需要再进行相机的标定、图像校正、匹配、三角测量等，直接通过ROS的话题机制，订阅相机的深度图节点，再利用ROS的同步机制，同时引入彩色图数据和深度图数据，目的在于对齐彩色图和深度图(如图5)，具体来说，首先基于单目采集RGB图像数据，然后将彩色图像转换到LAB颜色空间，将LAB三通道单独分离出来，由于B通道对黄色敏感，故对B通道进行阈值分割，设定其阈值范围为(160，250)，可滤除黄色以外所有颜色。再对图像进行二值化处理，得到黑白图像，进行candy边缘检测，最后使用最小二乘法对圆进行拟合，得到标定板镂空圆圆心的二维坐标。由于MYNTEYE相机自带深度值，将彩色图与深度图对齐，则可得到彩色图中像素坐标对应的深度值，经过如下公式得到相机坐标系下如图4所示的圆心三维坐标：

x＝(u-c_x)*z/f_x

y＝(v-c_y)*z/f_y；

其中，(u,v)为像素坐标，(x,y,z)为像素坐标对应的三维坐标，d为深度，depth_scale为尺度因子，一般默认为1000.0，f_x和f_y为焦距，c_x和c_y为相机主点。

4、激光雷达获取标定板镂空圆圆心三维坐标。

首先，使用直通滤波对镂空标定板周围x、y、z 3个方向上的无关数据进行滤除，只保留了标定上的点云，使用样本一致性算法拟合标定板平面方程，通过设定一个离标定板距离远近的阈值a，保留a范围内的点云，此处阈值a设为1cm，即得到了标定板上面的点云。

其次通过为每个点云人为附加一个深度不连续值，通过比较同一束激光上相邻点云直接的深度不连续值，过滤掉连续的点云。然后使用RANSAC算法找到标定板点云中的直线，过滤掉能拟合成直线的点云，此时只剩下镂空圆边缘的点云了，可拟合圆，得圆心二维坐标，再将圆心二维坐标返投到三维空间中，得圆心三维坐标；保证至少两束激光能扫描到镂空圆的边缘，因为至少需要3个点才能拟合圆。最终得到4组一一对应的3D-3D圆心数据对，即如图6所示的圆心三维坐标。

5、采用改进的ICP算法对两组点集配准，求解的结果用于配准，相机与激光雷达标定就是求解相机与激光雷达之间的位置关系，本专利就是通过点配准这种方法来标定，配准结果好，说明标定的好。

由于传统ICP算法以欧式距离为度量函数，对噪声敏感，本发明在传统ICP算法上加入了最大相关熵准则并利用L1范数的稀疏性来减小点集配准间的误差，提高配准鲁棒性。其中最大相关熵准则(MCC)是一种局部相似度量函数，包括：

1)最大相关熵准则

2)基于最大相关熵准则的稀疏ICP算法模型

3)模型求解

将两个随机变量的局部相似性表示为：V_σ(A,B)＝E[k_σ(A-B)]，其中k_σ(.)是一种内核函数，E是数学期望；

S511、两个有限集数据的相关熵通过/>进行估计，其中，高斯核函数k_σ(.)为：/>

S512、对于任意两个相同维度的向量和/>将之作差得到/>其中e_j＝a_j-b_j，则e_j的最大相关熵为/>在内核宽度σ决定的小范围邻域内，相关熵可以看作是衡量两个随机变量相似度的方法，则相关熵越大，两个变量间的相似度越高，当V_σ(A,B)取得最大值的时候，A,B之间的误差E就最小。

稀疏ICP算法模型数学表达式如下所示：

s.t.R^TR＝I,det(R)＝1；

其中，R是旋转矩阵，t是平移向量，e是误差，其中对e进行L1范数约束，代表稀疏性，E为稀疏矩阵。

稀疏ICP算法模型的求解方式被配置为包括：

S521、对t求偏导：

得到：

其中，w表示迭代时用指数项部分的(E_k-1,R_k-1,t_k-1)代替(E_k,R_k,t_k)；

S522、令：

则含参数R，E的目标函数变为：

S523、令：

N＝Q+E；

则含参数R的目标函数为：

通过半方(HQ)技术，将上式转换为：

其中：

则最大化f(R)等价于：

F(R)＝-tr(H)；

其中：

通过SVD分解H得：

R＝VDU^T；

此时求解稀疏矩阵E，则只含E的目标函数如下：

其中：

S524、令：

RX+t-P＝M；

则等价于：

其中，B是一个以b为对角元素的对角矩阵，上式等价于：

其中：

将上述问题看成一个子问题，有：

解得：

其中：

即A中的每个元素都是：

模型求解求解后的结果(是一个矩阵)就是两组三维圆心坐标的空间位姿关系。也就是求解相机与激光雷达之间的相对位姿关系，得：

其中，另外R是旋转矩阵，t是平移向量，e是误差，其中对e进行L1范数约束，代表稀疏性。E为稀疏矩阵，这三个个量就是要求解的量，R、t就是两个点集对应的位姿关系，也就是相机与激光雷达之间的位姿关系，为了简化数学过程，其中P、N、M、ω_i、y_i是中间变量，x_i和p_c(i)分别代表两组点集，c_i代表两点集中的点一一对应，N_p代表每组点集中的点个数。

6、为验证改进的ICP算法的性能，首先进行该算法的仿真实验，通过与传统ICP算法、CPD算法做对比，不同算法耗时比较如表一所示：

表一

由表一可以看出，改进的算法比传统ICP算法慢，这是因为我们的算法中包含了指数项的计算，但仍然具有较快的速度。

表二

而表二可以看出，与传统的ICP算法相比，本发明算法中旋转和平移两参数的误差较小，这表明我们的算法在噪声点集配准中具有较高的精度。

在仿真实验中，采用如图7的配准样图，采用改进的ICP算法进行点集配准，由图8的对比图可知，其重合度很高(重叠、配准)，几乎没有任何间隙，而相对于如图9中现有技术的ICP配准效果所产生的较大间隙而言，可知本发明改进的ICP算法配准精度得到显著提高。

7、为验证此标定方法在真实环境中的可行性和标定结果的优劣，进行实验验证。标定板如图2所示，参数为120cm×80cm×4mm，镂空圆半径为12cm，相机为小觅深度版，激光雷达为Velodyne16线激光雷达。分别采集10帧同一时刻的图像数据和点云数据。

8、利用两传感器获得的标定板镂空圆心的三维坐标，进行真实场景的相机与激光雷达标定，采用改进的基于最大相关熵准则的稀疏ICP算法进行配准，获得相机与激光雷达之间的相对位姿关系，

得到旋转矩阵R为：

平移向量t为：

采用重投影误差来量化相机与激光雷达标定结果，值越小代表标定结果越好，越大代表标定结果不好。得到10组标定结果的重投影误差为0.01263像素；而采用传统的ICP算法进行配准，10组标定结果的重投影误差为1.20185像素。由此可知，本发明的方法，其标定效果更为优异。

以上方案只是一种较佳实例的说明，但并不局限于此。在实施本发明时，可以根据使用者需求进行适当的替换和/或修改。

这里说明的设备数量和处理规模是用来简化本发明的说明的。对本发明的应用、修改和变化对本领域的技术人员来说是显而易见的。

尽管本发明的实施方案已公开如上，但其并不仅仅限于说明书和实施方式中所列运用。它完全可以被适用于各种适合本发明的领域。对于熟悉本领域的人员而言，可容易地实现另外的修改。因此在不背离权利要求及等同范围所限定的一般概念下，本发明并不限于特定的细节和这里示出与描述的图例。

Claims (4)

1.一种无人驾驶环境感知中相机与激光雷达标定方法，其特征在于，包括：

步骤一、基于三维激光雷达、RGB-D相机、镂空标定板搭建车载的传感器系统，其中标定板上设置有四个镂空圆；

步骤二、基于相机与镂空标定板的配合，通过计算得到标定板上各镂空位置的中心在三维空间下的多个第一圆心坐标；

步骤三、基于三维激光雷达与镂空标定板的配合，通过计算得到标定板各镂空位置的中心在三维空间下对应的多个第二圆心坐标；

步骤四、基于标定板上的镂空圆个数，获得多对一一对应的圆心坐标数据对；

步骤五、利用改进的ICP算法解算数据对之间的位置关系，完成二者的标定；

在步骤五中，所述改进的ICP算法被配置为在ICP算法中加入最大相关熵准则MCC，其被配置为包括：

S51、基于MCC的局部相似度量函数，建立稀疏ICP算法模型；

S52、对基于MCC的稀疏ICP算法模型进行求解；

基于MCC的稀疏ICP算法模型数学表达式如下所示：

s.t.R^TR＝I,det(R)＝1；

上式中，R表示旋转矩阵、t表示平移向量，xi、Pc(i)分别代表根据步骤二、三获得的两组点集，Np代表每组点集中的点个数，σ表示内核宽度，c(i)代表两点集中的点一一对应；

基于MCC的稀疏ICP算法模型的求解方式被配置为包括：

S521、对t求偏导：

得到：

其中，ω_i表示迭代时用指数项部分的(E_k-1,R_k-1,t_k-1)代替(E_k,R_k,t_k)；

S522、令：

则含参数R,E的目标函数变为：

S523、令：

N＝Q+E；

则含参数R的目标函数为：

通过半方技术，将上式转换为：

其中：

则最大化等价于：

F(R)＝-tr(H)；

其中：

通过SVD分解H得：

R＝VDU^T；

此时求解稀疏矩阵E，则只含E的目标函数如下：

其中：

S524、令：

RX+t-P＝M；

则等价于：

其中，B是一个以b为对角元素的对角矩阵，上式等价于：

其中：

有：

解得：

其中：

即A中的每个元素都是：

2.如权利要求1所述的无人驾驶环境感知中相机与激光雷达标定方法，其特征在于，在步骤一中，所述三维激光雷达被配置为采用VLP-16线激光雷达，所述RGB-D相机被配置为采用MYNT EYE深度相机；

其中，所述标定板被配置在传感器系统正前方1.5～7m，且标定板能全部在相机的取景图像中，同时至少有2行激光束能扫描到4个镂空圆的边缘；

镂空标定板后面设置有与其大小一致的黄色背景板。

3.如权利要求2所述的无人驾驶环境感知中相机与激光雷达标定方法，其特征在于，在步骤二中，第一圆心坐标的获取方式被配置为包括：

S21、基于单目采集的RGB图像数据转换成LAB图像，对B通道进行阈值分割，滤除黄色以外的所有颜色；

S22、对图像进行二值化处理、candy边缘检测、最小二乘法对圆进行拟合，以得到标定板镂空圆圆心对应的二维像素坐标；

S23、调用相机自带的深度图数据，将彩色图与深度图对齐，得到与二维像素坐标对应的深度值，并基于以下公式得到圆心在三维空间下的第一圆心坐标：

x＝(u-c_x)*z/f_x

y＝(v-c_y)*z/f_y

其中，(u，v)为二维像素坐标，(x，y，z)为二维像素坐标对应的第一圆心坐标，d为深度，depth_scale为尺度因子，f_x和f_y为焦距，c_x和c_y为相机主点。

4.如权利要求1所述的无人驾驶环境感知中相机与激光雷达标定方法，其特征在于，在步骤三中，第二圆心坐标的获取方式被配置为包括：

S31、使用直通滤波滤除标定板周围点云，使用RANSAC算法拟合标定板平面方程；

S32、通过指定一个表示相邻点云深度差的一个量值的方法，过滤掉标定板上的连续点，保留不连续点，去除不属于标定板镂空圆边缘的点云；

S33、将镂空圆边缘上的点映射到二维平面上拟合圆，得圆心二维坐标；

S34、将圆心二维坐标映射到三维空间中，以得到第二圆心坐标。