CN112927133A - 一种基于一体化标定参数的图像空间投影拼接方法 - Google Patents

Abstract

本发明公开了一种基于一体化标定参数的图像空间投影拼接方法，通过三维标定板，标定相机内参数；根据2D‑LiDAR单次扫描标定板板的点云数据确定2D‑LiDAR和相机之间的旋转矩阵和平移矩阵，根据旋转后相机图像中关键点坐标与采集末端零位时图像关键点坐标对比，结合相机与2D‑LiDAR的外参数，实现旋转云台的旋转参数的标定；使得标定结果处于旋转云台一体化坐标系中，完成旋转图像的空间投影及拼接。本发明提高了标定的效率和准确性，解决了传统的多传感器与运动机构需分步标定的问题；将所有标定结果，包括相机图像与激光点云，在基于旋转云台一体化坐标系内进行表示，并将其运动学方程用于旋转图像的空间投影及拼接方法。

Description

一种基于一体化标定参数的图像空间投影拼接方法

技术领域

本发明属于图像处理领域，尤其涉及一种基于一体化标定参数的图像空间投影拼接方法。

背景技术

获取室外真实场景的三维模型是计算机视觉、视觉导航、机器人等领域的一个重要研究课题，具有广阔的应用场景：如机器人导航，无人驾驶，移动道路测量车和城市或地图3D数字化等。对于大型场景的空间测量，单一传感器存在不可避免的局限性，2D-LiDAR与相机联合测量技术结合了两种类型传感器的优点，能同时获取纹理丰富的图像和几何结构信息。

在利用2D-LiDAR与相机联合测量技术进行空间测量前，需要对其几何安装参数进行标定。

目前，大多数相关工作主要是利用特定的标定板来计算两传感器间精准的相对位姿(旋转矩阵和平移矢量)，该标定板一般是三角板或由三角板组成的V型板等等，通过特定的结构提取激光点云和相机图像中的角点，从而完成标定。但是，激光点云存在稀疏性，会影响点云数据和图像数据中角点的准确性，从而影响标定的精确性。

此外，对建筑物，如桥梁、外墙面等进行外观检测时，由于摄像头经常需要调整角度和转向，导致拍摄得到的图片会出现倾斜、扭曲等，导致需要将图像拼接形成建筑的整体图形时，需要根据各图像边缘的相似度寻找到邻近的图片，然后将图片边缘、扭曲、拉伸或缩减等将邻近图片拼接在一起。其缺点在于，处理流程繁琐，耗费时间长，且当建筑有大量相似的部分时，很容易使得图片拼接错误，而且缺少部分图片时，即无法经图片拼接。

名词解释：

2D-LiDAR：二维激光雷达。

ChArUco标定板：棋盘格与ArUco二维码相结合的标定板。

发明内容

为解决上述问题，本发明公开了一种基于一体化标定参数的图像空间投影拼接方法，本发明采用特殊的U型三维标定板，仅需采集一次标定数据，即实现多传感器几何参数的自动标定，提出采集末端的旋转云台一体化坐标系，确定了旋转云台的运动学方程，形成了图像与2D激光点云的有效融合，以实现多自由度旋转图像的快速精确拼接。

为实现上述目的，本发明的技术方案为：

一种基于一体化标定参数的图像空间投影拼接方法，包括如下步骤：

步骤一、建立一体化标定系统；

所述一体化标定系统包括标定板和多自由度采集末端，多自由度采集末端包括旋转云台，旋转云台包括2D-LiDAR和至少两个相机；旋转云台固定连接在Y轴旋转轴，Y轴旋转轴轴接连接有Z轴旋转轴；所述标定板包括ChArUco标定板，ChArUco标定板一侧固定有矩形侧板，另一侧固定有等腰直角三角形侧板，等腰直角三角形侧板的一个直角边为ChArUco标定板的侧边；矩形侧板外侧固定有第一边侧板；等腰直角三角形侧板的斜面上固定有第二边侧板；矩形侧板和等腰直角三角形侧板所在平面均与ChArUco标定板所在平面垂直设置；第一边侧板所在平面与ChArUco标定板坐在平面平行，第二边侧板所在平面与等腰直角三角形侧板所在平面垂直设置；

步骤二、获取2D-LiDAR单次扫描标定板的点云数据，提取标定板的角点信息，根据角点信息确定多自由度采集末端在各个旋转自由度时的零点位置；

步骤三、驱动Y轴旋转轴和Z轴旋转轴预设轨迹旋转，并采集相机和2D-LiDAR数据形成完整的标定数据集；

步骤四、

提取完整的标定数据集中每张图像中的ArUco特征点和ChArUco角点坐标序列，所述ArUco特征点包括二维码的ID号和二维码四个顶点的平面坐标；根据采集末端不同旋转角度时相机图像中相应二维码四个顶点的平面坐标信息，标定相机内参；

步骤五、在获取2D-LiDAR扫描标定板的点云数据，提取标定板角点，计算各标定板角点在相应相机图像中的平面坐标信息，结合各个标定板角点的空间和平面坐标信息，确定2D-LiDAR和相机之间的旋转矩阵和平移矩阵；

步骤六：获取2D-LiDAR绕多自由度采集末端的Z轴旋转轴和Y轴旋转轴旋转得到的点云数据和相机图像，进行多自由度采集末端的旋转云台的Y轴和Z轴旋转参数的联合标定；

步骤七：将Z轴旋转轴和Y轴旋转轴标定结果在2D-LiDAR坐标系内表示；

步骤八：多自由度采集末端采集目标物的图像，并将采集到图像的像素点均转化到2D-LiDAR坐标系内，完成目标物图像的拼接。

进一步的改进，所述步骤二包括如下步骤：

相机朝向ChArUco标定板，2D-LiDAR朝向与矩形侧板所在平面平行的方向，采集2D-LiDAR扫描标定板的激光点云；对激光点云进行稀疏激光均匀化采样，激光滤波，得到均匀连续的激光点云；将均匀连续的激光点云转换成图像，对图像进行形态学标定板角点取激光点云清晰连续的骨架，获得获取图像中激光点云的轮廓，当轮廓中包含的点的个数大于或等于预设值时，则为有效有效激光轮廓，否则为无效激光轮廓，重新采集2D-LiDAR扫描标定板的激光点云；

根据有效激光轮廓，进行多边形拟合后，提取有效激光轮廓中关于标定板的多边形轮廓线段，通过外扩矩阵确定标定板角点位置：

式1中，K₁与K₂分别表示多边形轮廓上任一线段的起始点和终止点的坐标；

K₃和K₄分别表示线段K₁K₂经过外扩后对应的的起始点和终止点的坐标；K₃和K₄作为更新后的标定板角点位置；

然后根据给定的阈值s，计算外扩矩阵四个顶点的坐标，得到获取包含4个顶点的最小外接矩形；s表示此外接矩形高度的二分之一。(最小外接矩阵的二分；

通过给定的阈值s，计算中矩形的4个顶点在2D-LiDAR坐标系中的坐标值，并获取包含4个顶点的最小外接矩形，以判断激光点云在哪个矩形内，将所有激光点云分配到对应的矩形内，然后将矩形内的激光点拟合得到直线，根据两两直线的交点得到有效激光轮廓的角点，分别为p0、p1、p2、p3、p4、p5，其中p1、p2、p3、p4即标定板角点；

其中，d＝3δ，s＝2δ，δ表示2D-LiDAR的测量误差；

驱动多自由度采集末端的Y轴旋转轴旋转，采集序列激光剖面，并提取处于ChArUco标定板上p2p3连线与2D-LiDAR坐标系中X轴方向的夹角，取夹角最小的位置为Y轴的零点，记录Y轴为零点时对应的Y轴机械角度θ，并驱动Y轴旋转至Y轴的零点；然后驱动Z轴旋转轴旋转，采集序列激光剖面，并提取线段p2p3的长度，取p2p3长度最小时的位置为Z轴的零点，Z轴的零点对应的Z轴机械角度β，将{θ,β}角度作为多自由度采集末端的零点位置，即完成多自由度采集末端的零点位置标定。

进一步的改进，所述步骤三包括如下步骤：

将多自由度采集末端的Y轴旋转轴与Z轴旋转轴均置于零点位置，然后分两步进行标定采集相机和2D-LiDAR数据形成完整的标定数据集：

保持Z轴旋转轴处于Z轴的零点位置不动，驱动多自由度采集末端的Y轴旋转轴转动，每次转动1±0.5°；每次Y轴旋转轴转动后，多自由度采集末端上的两个相机拍摄图像并保存，记录2D-LiDAR一帧点云数据，同时记录多自由度采集末端在Y轴自由度和Z轴自由度的旋转角度，作为一组标定数据；在每个旋转位置保存对应的一组标定数据，作为Y轴转动的标定数据集；

然后保持Y轴旋转轴处于Y轴的零点位置不动，驱动多自由度采集末端的Z轴旋转轴转动，每次转动1±0.5°，将转动过程中每组相机图像、激光点云和多自由度采集末端的姿态数据保存下来，作为Z轴转动的标定数据集；

Y轴转动的标定数据和Z轴转动的标定数据集形成完整的标定数据集。

进一步的改进，所述步骤四包括如下步骤：

提取Y轴转动的标定数据中每张图像中的ArUco特征点，所述ArUco特征点包括二维码的ID号和二维码四个顶点的平面坐标；同时提取出提取出每张图像的ChArUco角点坐标序列；基于Y轴旋转轴旋转的多个位置，图像坐标系上的角点序列和相对应的世界坐标序列，根据棋盘格标定方法标定出相机的内参数矩阵K和畸变参数矩阵D，完成相机内参的标定:

式2中，f_x和f_y分别表示x与y方向上像素与实际距离的比例；u₀，v₀分别为图像中心在像素平面即图像坐标系的u、v轴坐标，u为图像宽度方向为方向，v为图像高度方向；k₁，k₂，k₃表示镜头的径向畸变参数；m₁,m₂表示镜头的切向畸变参数。

进一步的改进，所述步骤五包括如下步骤：

根据Y轴转动的标定数据集，获取激光点云数据，提取有效激光轮廓中的标定板角点，即得到标定板角点p1、p2、p3和p4在2D-LiDAR坐标系中的空间坐标

根据得到的激光轮廓的角点坐标，通过标定板结构的几何约束，设ChArUco标定板处于标定板世界坐标系Z＝0平面，于是通过

以及标定板的几何约束得到所述标定板角点在标定板世界坐标系的坐标

即有：

式3中，d为二维码边缘与ChArUco标定板边缘的距离，w为ChArUco标定板的边长；

以相机图像中提取的二维码四个顶点作为控制点，结合标定的相机内参数及畸变参数，通过相机标定方法标定出相机外参，即相机坐标系与标定板世界坐标系之间的旋转矩阵R_W2C与平移矩阵T_W2C，然后确定标定板角点在图像坐标系中的实际坐标：

式4中，^WX_i,^WY_i,^WZ分别为标定板角点在标定板世界坐标系中的X、Y、Z分量；

分别表示标定板角点在相机坐标系中的X、Y、Z分量；

分别表示标定板角点在图像中的u轴和v轴坐标值；

得到标定板角点在图像坐标系中的理论坐标信息：

式5中，R_L2C和T_L2C分别表示2D-LiDAR与相机间的旋转矩阵和平移矩阵；^LX_i,^LY_i,^LZ_i分别为标定板角点的X、Y、Z分量；^CX_i,^CY_i,^CZ_i分别表示标定板角点在相机坐标系中对应的理论坐标值；^Iu_i,^Iv_i分别表示标定板角点在图像中的理论像素坐标值；

至此，定义损失函数为：

式6中，Δu和Δv分别表示像素点在u和v方向的偏差；

采用最小二乘法，迭代求解得到2D-LiDAR与相机间旋转矩阵和平移矩阵。进一步的改进，所述步骤六包括如下步骤：

根据完整的标定数据集，进行旋转云台绕Z轴和Y轴的旋转参数标定，标定结果包括代表旋转轴方向的单位向量和绕轴旋转点，旋转云台绕Z轴和Y轴的标定结果最后都统一到同一个坐标系2D-LiDAR坐标系下：

首先，设绕Y轴旋转轴旋转的旋转向量就是2D-LiDAR坐标系中Y轴(0,1,0)，通过由粗到细的搜索方法求解绕得到Y轴旋转轴的旋转点，同样通过由粗到细的搜索方法得到Z轴旋转轴的旋转点；利用相机图像中的ArUco特征点作为外部控制点，将旋转时相机图像的ArUco特征点，与旋转到零点角度时相机图像的ArUco特征点之间的偏差作为误差函数，利用最小二乘法，迭代求解误差函数最小时的旋转轴方向与旋转点；求解得到旋转云台的Y轴、Z轴的旋转参数，作为迭代求解时Y、Z轴的旋转轴向量和旋转点的初始值。

进一步的改进，所述由粗到细的搜索方法包括如下步骤：

一)获取Z轴旋转轴旋转不同角度的一系列激光点云数据，读取激光的单次点云数据，记录Z轴旋转轴第i次旋转的角度θ_i；对第i次转台绕Z轴旋转后采集到的的原始点云数据，进行稀疏激光均匀化上采样和滤波处理，得到有效的点云数据，提取得到标定板角点p2、p3；最后得到标定板角点p2、p3与Z轴旋转轴旋转角度的序列

其中，i表示第i次旋转，j表示p2或p3点，

则表示第i次旋转时p2或p3点在2D-LiDAR坐标系中的坐标值；

二)将2D-LiDAR绕多自由度的旋转简化为空间上一点绕两向量旋转，存在如下关系：

式7中，θ为Z轴旋转轴旋转的角度，

表示Z轴旋转轴旋转的方向向量，Sz表示Z轴旋转轴的旋转点，

则表示旋转云台绕

方向旋转θ角度对应的旋转矩阵，P₁表示2D-LiDAR坐标系内任意一点，P₁绕

轴旋转θ角度对应点为P₀；r0、r1、r2...r8分别表示旋转矩阵

的各元素X_AZ,Y_AZ,Z_AZ,分别表示绕Z轴旋转点S_Z在2D-LiDAR坐标系的X、Y、Z值，^Ln_x，^Ln_y，^Ln_z分别表示旋转轴向量

在2D-LiDAR坐标系的X、Y、Z轴分量；

确定点P₀绕Y轴旋转轴

轴旋转α后的几何关系：

式8中，α表示旋转云台绕Y轴旋转的角度，

表示绕Y轴旋转的方向向量，S_Y则表示绕Y轴旋转点，

表示旋转云台绕

方向旋转α角度对应的旋转矩阵；P₂则表示P₀在绕

旋转α角度后的对应点；

将绕Z轴旋转轴旋转后的标定板角点p2、p3还原到旋转零角度的公式即为：

公式9中，θ₀表示在Z轴旋转轴旋转标定为零角度时，传感器记录的实际角度，而θ_i则表示在第i次Z轴旋转轴旋转时所记录的角度值；

分别表示Z轴旋转轴第i次旋转后，标定板第j个角点在2D-LiDAR坐标系中的坐标值，

分别表示标定板角点在还原至绕Z轴旋转零角度后，在2D-LiDAR坐标系中的坐标值；

将所有采集末端绕Z轴旋转采集的激光点云数据均还原到绕Z轴旋转零角度，即形成点云数据；

三)设旋Z轴旋转轴旋转轴向量为

计算Z轴旋转轴的旋转点，根据由粗到细的搜索策略，先利用循环迭代的方法进行粗略的求解：

A.初始时，设旋Z轴旋转轴的旋转点为S_Z(0,0,0)；

B.Z轴旋转轴旋转采集到的所有激光点云数据，均还原到Z轴旋转轴旋转角度为零时的状态；

C.根据还原后的点云数据，计算得到包含还原后的点云数据的最小外接矩阵，记最小外接矩阵的四个顶点为{^Lq_k}(k＝1,2,3,4)；

D.将四个顶点连线，形成四条线段{l_k}(k＝1,2,3,4)，随后给定阈值d和s，得到l_k线段各自的外扩矩形；

E.依次判断激光点云落在哪个外扩矩形内，并形成每个外扩矩形的激光点集；根据每个矩形内点集拟合直线，并计算矩形内所有点到对应直线的距离累加值；

F.计算四个外扩矩形的顶点到拟合直线距离累加值之和，如果小于上一次迭代的累加值之和，则根据阈值Δ_y将旋转点S_Z的Y分量加上Δ_y；Δ_y＝1-3mm；

G.若迭代次数超过设定阈值次数则结束，否则回到步骤B运行循环；

迭代过程停止之后，得到粗略求解的采集末端绕Z轴旋转点，记为

其中

表示

点在2D-LiDAR坐标系中的X分量，

表示

点在2D-LiDAR坐标系中的Y分量，

表示

点在2D-LiDAR坐标系中的Z分量；

将

作为细化搜索的迭代初值，设定一个绕Z轴旋转轴旋转点Y分量的迭代阈值d_y，d_y＝0.1*Δ_y，同时将细化搜索时旋转点Y值搜索范围控制在

之内；于是按照步骤A至步骤F的迭代过程，细化搜索得到旋转云台绕Z轴旋转轴的旋转点，最终结果得到的旋转点记为

其中

表示

点在2D-LiDAR坐标系中的X分量，

表示

点在2D-LiDAR坐标系中的Y分量，

表示

点在2D-LiDAR坐标系中的Z分量；

根据步骤一)至步骤三)，同样得到旋转云台绕Y轴的旋转点，记为

其中

表示

点在2D-LiDAR坐标系中的X分量，

表示

点在2D-LiDAR坐标系中的Y分量，

表示

点在2D-LiDAR坐标系中的Z分量；完成对Z轴旋转轴和Y轴旋转轴的标零。

进一步的改进，包括如下步骤：

a.在对Z轴旋转轴和Y轴旋转轴标零之后，记录下标零时Z轴旋转轴和Y轴旋转轴的传感器角度值，记为θ₀，α₀，运用图像处理方法提取此时相机图像中ArUco特征点的像素坐标，记为：

其中j表示旋转轴在零角度时，相机图像中第j个ArUco二维码，k则表示第j个ArUco二维码的第k个顶点，LC表示左相机图像，RC则表示右相机图像；

表示在旋转零角度时，左相机图像中，第j个ArUco二维码的第k个顶点在图像坐标系中的u轴坐标，

表示左相机图像中，第j个ArUco二维码的第k个顶点在图像坐标系中的v轴坐标；

分别表示右相机图像中相应点的u轴和v轴坐标；

根据Y轴转动的标定数据和Z轴转动的标定数据集，当第i次Z轴旋转轴和Y轴旋转轴旋转角度分别α_i,θ_i,提取相机图像ArUco特征点：

i表示第i次旋转。

根据左相机的内参与外参，将左相机的图像坐标转化为标定板世界坐标：

式11中，i表示第i次旋转，j表示图像中的第j个ArUco二维码，k则表示二维码的第k个顶点；R_W2C与T_W2C分别表示左相机的旋转矩阵和平移矩阵，K^LC为左相机内参数矩阵；

为左相机图像ArUco特征点的像素坐标，

分别为特征点在左相机坐标系中X、Y、Z轴的坐标，

分别为特征点在标定板世界坐标系中X、Y、Z轴对应坐标值；

因图像中ArUco二维码的四个顶点世界坐标均已知，故求得

根据2D-LiDAR与相机间关系标定结果，得到ArUco二维码各顶点在2D-LiDAR坐标系中的空间坐标表示：

式12中，R_L2LC与T_L2LC分别表示2D-LiDAR与左相机之间的旋转矩阵和平移矩阵，

表示第i次旋转时，左相机图像的第j个ArUco二维码的第k个顶点在2D-LiDAR坐标系中的对应坐标值；

同理根据根据右相机的内参与外参，将右相机的图像坐标转化为2D-LiDAR坐标；在2D-LiDAR坐标系下，旋转云台绕Y轴的旋转轴为

绕轴旋转点为[X_AY,Y_AY,Z_AY]^T,将在2D-LiDAR坐标系下的ArUco特征点还原到绕Y轴旋转轴零角度时的坐标，坐标表示为：

式13中，α₀表示Y轴旋转轴在零角度时，传感器记录的旋转角度值，α_i则为第i次旋转时传感器记录的旋转角度值；

分别对应着在2D-LiDAR坐标系下相应特征点还原至Y轴旋转轴在零角度时的坐标；

同样的，在2D-LiDAR坐标系下，旋转云台绕Z轴的旋转轴为

绕轴旋转点为[X_AZ,Y_AZ,Z_AZ]^T，则ArUco二维码各顶点还原到绕Z轴旋转零角度，坐标表示为：

式14中，θ₀表示旋转云台绕Z轴旋转零角度时，传感器记录的旋转角度值，θ_i则为第i次旋转时传感器记录的旋转角度值；

对应着相应特征点还原至2D-LiDAR坐标系下绕Z轴旋转轴在零角度时的坐标；利用相机内参数得到ArUco特征点还原至Z轴旋转轴和Y轴旋转轴均在零角度时的像素坐标，称之为理论像素坐标；具体推导公式如下：

式15中，

即为ArUco特征点在Z轴旋转轴和Y轴旋转轴均在零角度时的理论像素坐标，

为对应的理论2D-LiDAR坐标值；

关注理论像素坐标，与Z轴旋转轴和Y轴旋转轴均在零角度时所提取的ArUco特征点像素坐标，建立误差函数如下：

式16中，Δ_Lu表示左相机图像在图像坐标系u方向的像素差，Δ_Lv表示左相机图像在图像坐标系v方向的像素差，Δ_Ru和Δ_Rv则表示右相机图像的相应差值，Δ_u和Δ_v则反映两个相机在u和v方向的整体像素偏差；根据式16，利用最小二乘法迭代求解旋转云台绕Z轴和Y轴的旋转参数，将Z轴旋转轴和Y轴旋转轴均在零点时的数据作为初始值，即：

最终解得旋转云台绕Z轴和Y轴的旋转参数，包括旋转轴

及其对应的绕轴旋转点S_Y,S_Z；

将所有在2D-LiDAR坐标系下的点按照如下公式还原到Z轴旋转轴和Y轴旋转轴均在零角度时的状态：

式17中，^LX,^LY,^LZ表示在2D-LiDAR坐标系下表示的任意一点的X、Y、Z轴坐标，α和θ则表示此时传感器记录的旋转云台绕Y轴和Z轴的旋转角度，^LZ⁰,^LZ⁰，^LZ⁰则表示将所述任意一点还原至旋转平台绕Y、Z轴旋转零角度时的坐标值；

b.将旋转云台的旋转参数结果转换到旋转云台一体化坐标系内，其中Y轴旋转轴通过基座滑动连接有直线滑轨；

滑轨坐标系O_sld与基座坐标系O_base之间旋转和平移均已知：

式18中，M_base2sld(d)表示滑轨坐标系与基座坐标系之间的平移矩阵，X_sld,Y_sld,Z_sld表示滑轨坐标系中点的X、Y、Z轴的坐标值，而X_base,Y_base,Z_base代表滑轨坐标系中的点在基座坐标系中的对应位置坐标；

基座坐标系O_base与云台偏转坐标系O_yaw之间旋转和平移均已知：

式19中，R_yaw2base和t_yaw2base分别表示基座坐标系与云台偏转坐标系之间的旋转和平移矩阵，X_base,Y_base,Z_base分别表示基座坐标系中点的X、Y、Z轴坐标值，而X_yaw,Y_yaw,Z_yaw则代表基座坐标系中的点在绕Z轴旋转的偏转坐标系中的对应位置坐标；

云台偏转坐标系O_yaw与旋转云台一体化坐标系O_mot之间旋转和平移也已知：

式20中，R_mot2yaw和t_mot2yaw分别表示偏转坐标系与旋转云台一体化坐标系之间的旋转和平移矩阵，X_yaw,Y_yaw,Z_yaw表示云台偏转坐标系中点的X、Y、Z轴坐标值，

则表示云台偏转坐标系的点在旋转云台Y、Z轴旋转角度均为零角度时，在一体化运动坐标中的对应坐标值；

还原旋转后的旋转云台一体化坐标系中点表示为：

式21中，X_mot,Y_mot,Z_mot表示在旋转云台一体化坐标系中点的X、Y、Z轴坐标值；β为旋转云台绕旋转云台一体化坐标系Z轴旋转的传感器记录角度，β₀表示Z轴旋转零角度时传感器记录角度，α与α₀分别表示旋转云台一体化坐标系Y轴旋转的相应角度；

与

分别表示旋转云台一体化坐标系Y轴和Z轴的旋转矩阵；

在旋转云台一体化坐标系中

与所标定旋转云台三个方向的旋转轴

完全重合，其中，

分别为基于2D-LiDAR坐标系的y轴和z轴旋转方向单位向量，且

的交点与旋转云台一体化坐标系原点O_mo_t重合，计算

最小二乘交点：S_AYAZ＝[X_AYAZ,Y_AYAZ,Z_AYAZ]^T，有如下推导：

式22中，[X_LS,Y_LS,Z_LS]分别表示2D-LiDAR坐标系内任一点的X、Y、Z轴坐标值，在旋转云台一体化坐标系中的对应的X、Y、Z轴坐标为[X_mot,Y_mot,Z_mot]；R_LS2mot即为2D-LiDAR坐标系何旋转云台一体化坐标系之间的旋转矩阵，t_LS2mot为平移矩阵；X_AYAZ\Y_AYAZ和_-Z_AYAZ分别表示

最小二乘交点S_AYAZ在2D-LiDAR坐标系中的X、Y、Z轴坐标；

综合式18-22，即有2D-LiDAR坐标系到滑轨坐标系的转换公式，表述如下：

X_sld、Y_sldZ_sld分别表示2D-LiDAR坐标系中任意一点[X_LS，Y_LS，Z_LS]，在还原至旋转云台旋转零角度时，对应滑轨坐标系中点的X、Y、Z轴坐标；

将相机图像坐标中任意点以及2D-LiDAR坐标系中任意点，至滑轨坐标系中的转换为结果坐标。

进一步的改进，所述步骤八包括如下步骤：

Ⅰ.将左右相机的视场统一为2D-LiDAR坐标系中的虚拟单目相机模式；

以H_L，W_L分别表示左相机图像在纵向和横向的像素点个数，H_R，W_R分别表示右相机图像在横向和纵向的像素点个数，左右相机t₀～t₄点在相机图像平面坐标系中的坐标分别为：

其中，

分别表示左相机图像中的四个角点，以及这些点在图像坐标系中u、v轴坐标位置，

则表示左相机图像中心点在图像坐标系中u、v轴坐标位置；同样的

与

则表示右相机图像中相应点在图像坐标系中u、v轴坐标位置；

将图像点转换到相机坐标系中，随后转换到2D-LiDAR坐标系中，按照如下公式求得左右相机图像四个顶点与中心点在2D-LiDAR坐标系内坐标位置：

式25中，

分别表示前述左相机

在图像平面中的u、v轴像素坐标，

则分别表示右相机

在图像平面中的u、x轴像素坐标；

分别表示左、右相机内参的逆矩阵；

分别表示左、右相机与2D-LiDAR之间旋转矩阵的逆矩阵；T_LS2LC,T_LS2RC分别表示左、右相机与2D-LiDAR之间的平移矩阵；

表示左相机

点在2D-LiDAR坐标系中对应点的X、Y、Z轴坐标，而

则表示右相机

点在2D-LiDAR坐标系中对应点的X、Y、Z轴坐标；

左相机坐标系原点O_LC和右相机坐标系原点O_RC，在2D-LiDAR坐标系中对应点^LCO_LS与^RCO_LS，其转换坐标计算如下：

式26中，

分别为左相机坐标系原点在2D-LiDAR坐标系中的X、Y、Z轴坐标，

为右相机坐标系原点在2D-LiDAR坐标系中X、Y、Z轴坐标；根据

和

与左、右相机坐标系原点在2D-LiDAR坐标系中相连所形成射线，射线与2D-LiDAR坐标中Z_LS＝h平面的交点；Z_LS表示2D-LiDAR坐标系的Z轴，h表示相机拍摄平面与2D-LiDAR坐标系原点的垂直距离；Z_LS＝h平面即虚拟单目相机平面；

射线与Z_LS＝h平面的交点记为：

左相机的交点

的X、Y与Z轴坐标计算如下：

同理得到右相机的相关交点

计算得到射线与Z_LS＝h平面的交点在2D-LiDAR坐标系中具体位置后，得到O_E,T₅,T₆,T₇,T₈在2D-LiDAR坐标系位置；

O_E表示左相机图像中心点与左相机坐标系原点O_LC连线的直线与右相机图像中心点与右相机坐标系原点O_RC连线的直线，在2D-LiDAR坐标系中的最小二乘交点；T₅为

和

连线在2D-LiDAR坐标系Y_LS＝0平面的交点；T₆为

和

的连线中点在2D-LiDAR坐标系Y_LS＝0平面的交点；T₇为

和

的连线在2D-LiDAR坐标系X_LS＝0平面的交点；T₈为

和

的连线在2D-LiDAR坐标系X_LS＝0平面的交点；

O_E在2D-LiDAR坐标系中位置，通过两条直线的最小二乘交点确定。所述两条直线分别过左右相机的原点与T₀点，具体计算方法：

式28中，X,Y,Z分别表示需要计算的T₀在2D-LiDAR坐标X、Y、Z轴的坐标值坐标值；完成基于旋转云台一体化坐标系的虚拟单目相机视场的关键点计算；

Ⅱ.旋转图像的空间投影及拼接

由于相机在拍摄时，相平面与被拍摄平面并非平行，需要将图像投影到被拍摄物体的平面，随后将左右相机的图像进行拼接，以实现在三维模型表面纹理贴图等目的；

2.1.单相机旋转图像的空间投影变换

相机图像从各自的相机坐标系，投影虚拟单目相机平面：

计算左、右相机相平面四个角点在2D-LiDAR坐标系的对应坐标：

和

以及左、右相机坐标系原点在2D-LiDAR坐标系中的对应空间坐标：^LCO_LS和^RCO_LS；

此时传感器记录的旋转云台绕Y轴旋转角度α和绕Z轴旋转角度θ，根据式17，将左、右相机相平面角点和左、右相机坐标系原点在2D-LiDAR坐标系的位置还原到旋转云台在旋转零角度状态时的对应坐标，分别记为

^LCO_LS-0和^RCO_LS-0；

得到左、右相机相平面角点和坐标系原点的连线，在还原至旋转零角度后的2D-LiDAR坐标系Z＝h平面的交点位置；对于左相机交点位置分别为：

计算公式如下：

式29中，

表示

点坐标值的XYZ分量，

则分别表示左相机相平面四个角点还原至旋转零角度后的点

的XYZ分量，^LCO_LS-0(X)，^LCO_LS-0(Y)，^LCO_LS-0(Z)则分别表示左相机坐标系原点在还原至旋转零角度后的^LCO_LS-0XYZ分量；

左相机相平面四个角点和左相机坐标系原点在还原至旋转零角度后的点在2D-LiDAR坐标系中的最大和最小X、Y坐标值，并作为透视变换后图像尺寸的变换依据：

式30中，

表示在

四个点中，X分量最大值，

表示X分量的最小值，

和

分别为Y分量的最大值和最小值；

同样得到右相机相平面四个角点和右相机坐标系原点在还原至旋转零角度后的点在2D-LiDAR坐标系中的最大和最小X、Y坐标值；

2.2.设定透视变换后的图像尺寸与原图像尺寸相同，则原图像四个顶点对应的透视变换后图像坐标，对于左相机按如下公式进行计算：

式31中，

与

分别表示原左相机图像四个角点在透视变换后图像中的像素坐标；

根据四个控制点在原始图像和透视变换后图像中的坐标位置，计算透视变换矩阵，即得到左相机图像的透视变换结果；同理可得右相机图像的透视变换结果；

2.3.旋转图像的拼接

将左右相机的图像都变换到同一图像上，再对两个变换后的图像进行权重相加形成一张拼接后的图，于是有：

式32中，

和

分别表示在拼接图像四个角点与Z＝h平面交点在X轴方向的最大值和最小值，

和

表示拼接图像四个角点与Z＝h平面交点在Y轴方向的最大值和最小值；

设定拼接后图像纵向的像素个数为H＝H_L，则横向的像素个数为：

式33中，H_L表示左相机原始图像在纵向的像素点数，W为新拼接图像在横向的像素点数；

得到左相机图像四个顶点在新拼接图像中的像素坐标：

在式34中，

和

分别表示左相机图像的四个角点在新拼接图像中的像素坐标；

同样得到右相机图像的四个角点在新拼接图像中的像素坐标；

将左相机图像和右相机图像通过加权相加的方法拼接得到新拼接图像。

进一步的改进，将多组旋转云台在不同旋转角度位置拍摄的图像投影到Z_LS＝h平面，得到对应的新拼接图像；在世界坐标系内旋转新拼接图像至旋转云台处于零点时的位置，实现旋转图像在3D模型中的还原贴图。

附图说明

图1为本发明实施例中采集末端结构示意图；

图2为本发明实施例中采集末端及其各运动关节坐标系示意图；

图3为本发明实施例中标定板示意图；

图4为本发明实施例中旋转矩形求解标定板角点示意图；

图5为本发明实施例中ChArUco板定板图；

图6为本发明实施例中相机图像ArUco检测结果图；

图7为本发明实施例中相机图像ChArUco关键角点检测结果图；

图8为本发明实施例中标定板几何约束示意图；

图9为本发明实施例中相机与2D-LiDAR关系标定结果校验图；

图10为本发明实施例中空间点绕Y轴和Z轴旋转简图；

图11为本发明实施例中2D-LiDAR坐标系中的虚拟单目相机关键点示意图；

图12为本发明实施例中左相机图像透视变换效果图；

图13为本发明实施例中右相机图像透视变换效果图；

图14为为本发明实施例中左相机图像透视变换效果图和右相机图像透视变换效果图合并的效果图；

图15为本发明实施例中激光点云标定板角点绕Z轴还原结果图；

图16为本发明实施例中激光点云标定板角点绕Y轴还原结果图；

图17为本发明实施例中多旋转图像的投影与拼接应用图；

图18为本发明实施例中一体化自动标定流程图。

其中，1相机，22D-LiDAR，3标定板，4Z轴旋转轴，5Y轴旋转轴，6基座，7直线滑轨，31ChArUco标定板，32矩形侧板，33等腰直角三角形侧板，34第一边侧板，35第二边侧板。

具体实施方式

以下结合附图及实施例对本发明做进一步说明。

图18是本发明实施例提供的一种2D-LiDAR和相机的多自由度采集末端的联合标定方法的流程图。所述标定方法包括：

步骤一：获取2D-LiDAR单次扫描标定板的点云数据，提取标定板结构角点信息，根据角点信息确定多自由度采集末端在各个旋转自由度时的零点位置；

步骤二：根据后续各项标定需求，驱动采集末端按照特定轨迹运动，并采集相机和2D-LiDAR数据形成完整的标定数据集；

步骤三：根据相机获取的图像，提取出图像中ChArUco关键角点坐标信息，根据采集末端不同旋转角度时相机图像中相应二维码特征点的平面坐标信息，计算相机内参；

步骤四：在获取激光扫描U型标定板的点云数据后，提取U型标定板角点，根据标定板特定结构计算各角点在相应相机图像中的平面坐标信息，结合各个角点的空间和平面坐标信息，确定2D-LiDAR和相机之间的旋转矩阵和平移矩阵；

步骤五：获取2D-LiDAR绕采集末端Y轴和Z轴旋转得到的点云数据和相机图像，采用由粗到细的搜索策略，实现旋转云台的Y轴和Z轴旋转参数的联合标定；

步骤六：采集末端标定结果在其一体化的运动坐标系内表示；旋转云台一体化坐标系内关键点计算及运用；

步骤七：旋转后，左右相机图像的投影变换；投影变换后多图像的拼接。

下面进行详细说明：

(1)采集末端零点位置标定：

图3中的U型标定板中间为ChArUco标定板(如图5所示)，一侧为两个互相垂直且尺寸已知的矩形板，另一侧为一块直角边与ChArUco标定板边长相同的等腰直角三角形板，有一块矩形板与其斜边相接。

将如图3所示U型标定板放置在竖直支架上，并将处于机械零点状态的采集末端设备放置在支架大概中心位置之下，采集2D-LiDAR扫描标定板的激光点云。

由于2D-LiDAR采样的角度以及激光与标定板轮廓之间的角度变化，得到的点云数据有的区域比较稀疏，影响后续提取激光轮廓关键点，同时有效的激光点也减少了，需要对激光点云进行稀疏激光均匀化上采样，激光滤波，得到均匀连续的激光点云。将均匀连续的激光点云转换成图像，对图像进行膨胀、二值化和细化等形态学处理，提取点云清晰连续的骨架。获取图像中点云的轮廓，根据轮廓中包含的点的个数判断该轮廓是否为有效激光轮廓。

根据得到的有效激光轮廓，通过直线拟合提取轮廓中的角点。在直线拟合过程中，提出了可旋转矩形的思想如图4。其中，d和s是我们给定的距离值，计算外扩矩阵：

公式1中，K₁与K₂表示经过前述图像处理后，计算得到的初始U型标定板轮廓关键点(如图3所示)，K₃和K₄则表示经过外扩后U型标定板轮廓关键点的更新位置。

随后通过给定的阈值s，计算图4中矩形的4个顶点，并获取包含这4个顶点的最小外接矩阵。通过求解出的矩形方程，来判断激光点云在哪个矩形内，由此将所有激光点云以此分配带其所属矩形，随后将矩形内的激光点拟合得到直线，根据两两直线的交点确定相应的角点，分别为p₀,p₁,p₂,p₃,p₄,p₅。

驱动采集末端设备首先绕其俯仰Y轴(如图2中的Ymot所示)旋转某一正负角度范围，采集序列激光剖面，并提取线段p2p3(如图3中所示)的角度，取线段角度最小的点为Y轴的零点，记录该帧点云对应的Y轴机械角度θ，并驱动Y轴旋转至该角度；然后绕偏转Z轴(如图2中Zmot所示)旋转某一正负角度范围，采集序列激光剖面，并提取线段p2p3的长度，取长度最小的点为Z轴的零点，并记录该帧点云对应的Z轴机械角度β，将{θ,β}角度作为采集末端的零点位置，即完成采集末端的零点位置标定。

(2)标定数据采集

根据(1)中所述零点标定方法，将采集末端的俯仰自由度(Y轴)与偏转自由度(Z轴)均置于零位置后，分两步进行标定所需要数据的采集。

保持偏转自由度(Z轴)处于零位置不动，驱动采集末端绕俯仰自由度(Y轴)转动，每次转动约1°。每次绕Y轴转动后，控制采集末端上的左右相机拍摄图像并保存，记录2D-LiDAR一帧点云数据，同时记录采集末端在俯仰自由度和偏转自由度的旋转角度，以此作为一组标定数据。在每个旋转位置按照此方式保存对应的一组标定数据，以此为绕Y轴转动的标定数据集。

类似的，保持俯仰自由度(Y轴)处于零位置不动，驱动采集末端绕偏转自由度(Z轴)转动，每次转动约1°。将过程中每组相机图像、激光点云和采集末端姿态数据保存下来，作为绕Z轴转动的标定数据集。

(3)相机内参标定

本发明采用的是棋盘格与ArUco二维码相结合的ChArUco标定板(如图5)。

进行相机内参标定前，需要对要标定相机所拍摄的图像进行一系列处理：获取(2)所述绕Y轴转动的标定数据集中，由需标定相机所拍摄的标定板图像；提取每张图像中ArUco特征点(如图6所示)，包括其ID号和二维码四个顶点的平面坐标；进一步的，根据之前所述ArUco特征点，提取出图像的ChArUco角点坐标序列(如图7中绿色圆圈所示结果)；基于采集末端在绕Y轴旋转的多个位置时，图像坐标系上的角点序列和相对应的世界坐标序列，根据棋盘格标定方法标定出相机的内参数矩阵K和畸变参数矩阵D:具体

公式2中，f_x,f_y分别表示x与y方向上像素与实际距离的比例，也叫做x轴和y轴上的归一化焦距；u₀,v₀分别为图像中心在像素平面的位置；k₁,k₂,k₃则表示镜头的径向畸变参数；p₁,p₂表示镜头的切向畸变参数。

(4)2D-LiDAR与相机的外参标定

根据(2)所述采集末端绕Y轴旋转所采集到的数据集，获取激光点云数据，按照(1)所述方法提取激光轮廓中的标定板角点，即可得到所述角点(图3中所示p1、p2、p3和p4)在2D-LiDAR坐标系中的空间坐标

根据得到的所述激光轮廓的角点坐标，通过标定板结构的几何约束(图8所示)。假定标定板处于U型标定板世界坐标系Z＝0平面，于是通过前述

以及标定板的几何约束，即可得到所述标定板角点在世界坐标系的坐标

即有：

公式3中，d为标定二维码边缘与标定板边缘的距离，w为标定板的边长。

通过前述的相机图像中提取的ChArUco关键角点作为控制点，结合之前标定的相机内参数及其畸变参数，通过相机标定方法^[1]标定出相机外参，即相机坐标系与U型标定板世界坐标系之间的旋转矩阵R_W2C与平移矩阵T_W2C，然后确定所述U型标定板角点在图像坐标系中的实际坐标：

公式4中，^WX_i,^WY_i,^WZ对应着公式3中所观测到的标定板角点坐标X、Y、Z分量；

表示观测点在相机坐标系中坐标值；

表示观测点在图像中的像素坐标值。

同样的，可以推导上述标定板角点在图像坐标系中的理论坐标信息：

公式5中，R_L2C和T_L2C分别表示激光与相机间旋转矩阵和平移矩阵；^LX_i,^LY_i,^LZ_i对应着公式3中标定板角点的点云数据

的X、Y、Z分量；^CX_i,^CY_i,^CZ_i表示标定板角点在相机坐标系中对应的理论坐标值；^Iu_i,^Iv_i表示其在图像中的理论像素坐标值。。

至此，定义损失函数为：

采用最小二乘法，迭代求解激光与相机间旋转矩阵和平移矩阵。根据求解结果，将激光点云数据转换到对应相机的图像坐标系上，可以直观看到校验结果，如图9所示，图中绿色与蓝色的点线即为激光点云数据。

(5)旋转轴参数标定

根据(2)中采集到的绕Y轴和Z轴旋转标定数据集，分别用来进行旋转云台的Y轴和Z轴旋转参数标定。需要特别指出的是，标定结果包括代表旋转轴方向的单位向量和绕轴旋转点，Y轴和Z轴的旋转参数标定结果最后都统一到同一个坐标系下。

首先，根据旋转云台的结构，初步假设绕Y轴旋转的旋转向量就是2D-LiDAR坐标系中Y轴(0,1,0)，通过由粗到细的搜索方法求解绕Y轴的旋转点，对于Z轴也是如此；随后，利用相机图像中的ArUco特征点作为外部控制点，将旋转时相机图像的ArUco特征点，与旋转零角度时相机图像的ArUco特征点之间的偏差作为误差函数，利用最小二乘法，迭代求解误差函数最小时的旋转轴与旋转点。而前述由粗到细搜索方法求解旋转云台的Y轴、Z轴的旋转参数，则作为迭代求解时Y、Z轴的旋转轴向量和旋转点的初始值。

①采用由粗到细的搜索方法计算旋转轴参数初值

获取转台绕Z轴旋转不同角度的一系列激光点云数据，读取激光的单次点云数据，记录转台第i次旋转的角度θ_i。按照(1)中所述方法与步骤，对第i次转台绕Z轴旋转后采集到的的原始点云数据，进行稀疏激光均匀化上采样和滤波处理，得到有效合理的点云数据，提取激光轮廓的关键点(如图3所示，仅需要p2p3)。最后得到关于标定板结构的关键角点与采集末端绕Z轴旋转角度的序列

这里i表示第i次旋转，j表示p2或p3点，

则表示第i次旋转时p2或p3点从激光点云获取的坐标(2D-LiDAR坐标系下)。

接下来，将对旋转云台的旋转运动带来空间坐标的变化进行说明。将所述2D-LiDAR绕多自由度的旋转简化抽象为空间上一点绕两向量旋转，所述旋转过程的几何模型如图10所示，存在如下关系：

公式7中，θ为旋转云台绕Z轴旋转的角度，

表示绕Z轴旋转的方向向量，S_Z表示绕Z轴旋转点，

则表示旋转矩阵。P₁表示任意一点，其绕

轴旋转θ后对应点为P₀。

类似的，可确定点P₀绕采集末端

轴旋转α后的几何关系：

公式8中，α表示旋转云台绕Y轴旋转的角度，

表示绕Y轴旋转的方向向量，S_Y则表示绕Y轴旋转点，

表示其对应的旋转矩阵。P₂则表示P₀在绕

旋转α后的对应点。

于是将绕Z轴旋转后的U型标定板角点(如图3所示，仅需要关注p2p3)还原到旋转零角度的公式即为：

公式9中，θ₀表示在旋转云台绕Z轴旋转标定为零角度时，传感器记录的实际角度，而θ_i则表示在第i次绕Z轴旋转时所记录的角度值；

表示旋转云台第i次绕Z轴旋转后，U型标定板第j个关键角点在2D-LiDAR坐标系中的坐标值，

则表示这些关键角点在还原至绕Z轴旋转零角度后的坐标值。

依照上述方法，将所有采集末端绕Z轴旋转采集的激光点云数据均还原到绕Z轴旋转零角度，即形成如图15所示的点云数据。

假设旋转云台绕Z轴的旋转轴向量为

只需要计算绕Z轴旋转点即可。接下来，根据由粗到细的搜索策略，先利用循环迭代的方法进行粗略的求解：

1)初始时，假设旋转云台绕Z轴的旋转点为S_Z(0,0,0)；

2)按照之前所述公式，将采集末端绕Z轴旋转采集到的所有激光点云数据，均还原到绕Z轴旋转角度为零时的状态；

3)根据还原后的点云数据，计算得到包含这些点云数据的最小外接矩阵，记此矩阵的四个顶点为{^Lq_k}(k＝1,2,3,4)，如图15；

4)将上个步骤中四个顶点连线，形成四条线段{l_k}(k＝1,2,3,4)，随后给定阈值d和s，按照(1)中计算标定板角点相同的方法，获得如图4所示l_k线段各自的外扩矩形；

5)依次判断激光点云落在哪个外扩矩形内，并形成每个外扩矩形的激光点集。根据每个矩形内点集拟合直线，并计算矩形内所有点到其对应直线的距离累加值；

6)计算四个外扩矩形的点到拟合直线距离累加值之和，如果其小于上一次迭代的累加值之和，则根据阈值Δ_y将旋转点S_Z的Y分量加上Δ_y；

7)若迭代次数超过一定阈值，则结束，否则回到2)继续。

上述迭代过程停止之后，可以得到粗略求解的采集末端绕Z轴旋转点，记为

进一步的，将

作为细化搜索的迭代初值，设定一个绕Z轴旋转点Y分量的迭代阈值d_y，d_y远小于上述迭代中的Δ_y。同时将细化搜索时旋转点Y值搜索范围控制在

之内。于是按照上述粗略搜索的迭代过程，进一步细化搜索采集末端绕Z轴的旋转点，最终结果为

对于旋转云台绕Y轴的旋转轴向量与旋转点，其计算方法与Z轴类似。同样获取标定板的关键角点与绕Y轴旋转的序列

类似的，i表示第i次旋转，j表示图3中所示的p1、p2、p3或p4点，

则表示第i次旋转时U型标定板角点从激光点云获取的坐标(2D-LiDAR坐标系下)。如图16所示，设定其初始旋转轴向量和旋转点分别为：

S_Y(0,0,0)，这里

S_Y含义与公式8中相同。

采用与绕Z轴标定计算时同样的公式，计算激光点云还原到绕Y轴零角度时的坐标，如图16所示，随后进行粗略搜索得到的绕Y轴旋转点，记为

进一步细化搜索，最终旋转云台绕Y轴旋转点的结果，记为

②基于图像控制点的旋转参数优化

根据①所述，在对Y，Z轴标零之后，记录下此时的旋转云台绕Y轴和Z轴的传感器角度值，记为θ₀，α₀，运用图像处理方法提取此时相机图像中ArUco特征点(ArUco的四个顶点)的像素坐标，记为：

其中j表示旋转轴在零角度时，相机中第j个ArUco二维码，k则表示此二维码的第k个顶点，LC表示左相机图像，RC则表示右相机图像。

根据(2)所述采集的标定数据集，当第i次绕Y轴和Z轴角度分别α_i,θ_i,提取相机图像ArUco特征点：

与前述类似，旋转零角度时特征点表示类似，这里i表示第i次旋转。

以左相机为例，根据左相机的内参与外参，可以将图像坐标转化为世界坐标：

公式11中，i表示第i次旋转，j表示图像中的第j个ArUco二维码，k则表示二维码的第k个顶点；R_W2C与T_W2C表示左相机的外参数(旋转矩阵和平移矩阵)，K^LC为左相机内参数矩阵；

为左相机图像ArUco特征点的像素坐标，

是这些特征点在左相机坐标系中的坐标表示，

则为特征点在U型标定板世界坐标系中的对应坐标值。

因图像中ArUco二维码的四个顶点世界坐标均已知，故可从上述公式求得

进一步的，根据(4)所述2D-LiDAR与相机间关系标定结果，可以推导出上述ArUco二维码各顶点在2D-LiDAR坐标系中的空间坐标表示：

公式12中，R_L2LC与T_L2LC分别表示2D-LiDAR与左相机之间的旋转矩阵和平移矩阵，

表示第i次旋转时，左相机图像的第j个ArUco二维码的第k个顶点在2D-LiDAR坐标系中的对应坐标值，其他符号含义与公式11中相同。

在2D-LiDAR坐标系下，旋转云台绕Y轴的旋转轴为

绕轴旋转点为[X_AY,Y_AY,Z_AY]^T,与①中公式9类似，可将ArUco特征点(在2D-LiDAR坐标系下)还原到绕Y轴旋转零角度，其坐标表示为：

公式13中，α₀表示旋转云台绕Y轴旋转零角度时，传感器记录的旋转角度值，α_i则为第i次旋转时传感器记录的旋转角度值；

对应着相应特征点还原至绕Y轴旋转零角度时的坐标(2D-LiDAR坐标系下)，其余符号含义与公式8中相同。

同样的，在2D-LiDAR坐标系下，旋转云台绕Z轴的旋转轴为

绕轴旋转点为[X_AZ,Y_AZ,Z_AZ]^T，则ArUco二维码各顶点还原到绕Z轴旋转零角度，其坐标表示为：

公式14中，θ₀表示旋转云台绕Z轴旋转零角度时，传感器记录的旋转角度值，θ_i则为第i次旋转时传感器记录的旋转角度值；

对应着相应特征点还原至绕Z轴旋转零角度时的坐标(2D-LiDAR坐标系下)，其余符号与公式13中相同。

最后，利用相机内参数得到ArUco特征点还原至，旋转云台各自由度旋转零角度时的像素坐标，称之为理论像素坐标。具体推导公式如下：

公式15中，

即为ArUco特征点在旋转云台各自由度旋转零角度时的理论像素坐标，

为对应的理论2D-LiDAR坐标值，其余符号含义与公式12和公式14中相同。

仅关注理论像素坐标，与实际旋转云台各自由度旋转零角度时所提取的ArUco特征点像素坐标(公式10中定义)，建立误差函数如下：

公式16中，Δ_Lu表示左相机图像在图像坐标系u方向的像素差，Δ_Lv表示左相机图像在图像坐标系v方向的像素差，类似的，Δ_Ru和Δ_Rv则表示右相机图像的相应差值，Δ_u和Δ_v则反映两个相机在u和v方向的整体像素偏差。其余符号与前述公式中意义相同。

根据公式16，利用最小二乘法迭代求解Y轴与Z轴旋转参数，将①中结果作为初始值，即：

最终解得旋转云台绕Z轴和Y轴的旋转参数，包括旋转轴

及其对应的绕轴旋转点S_Y,S_Z。

经过采集末端旋转后，所有在2D-LiDAR坐标系下的点均可按照如下公式还原到采集末端处于零位时的状态：

公式17中，^LX,^LY,^LZ表示在2D-LiDAR坐标系下表示的任意一点，α和θ则表示此时传感器记录的旋转云台绕Y轴和Z轴的旋转角度，^LZ⁰,LZ⁰，^LZ⁰则表示将此任意点还原至旋转平台绕Y、Z轴旋转零角度时的坐标值(2D-LiDAR坐标系下)。

按照上述结果与公式17，将(2)中所述激光点云数据还原到Y轴和Z轴旋转零角度时的状态，还原后的激光点云数据能够很好的契合在一起，并且能正确的反应出U型标定板的几何结构，以此证明标定方法和结果的正确性和准确性。

(6)旋转图像空间投影及拼接应用

提出旋转云台一体化坐标系的目的，是将相机的视场、激光测量距离与范围，均在一个坐标系中进行表示，同时也要将，在2D-LiDAR坐标系中表示的采集末端旋转参数，转换到此坐标系中。

具体步骤可以描述为：计算出2D-LiDAR与旋转云台一体化坐标系之间的旋转和平移关系，将旋转参数转换到旋转云台一体化坐标系中；计算左、右相机视场范围关键点，设置为虚拟单目相机视场的关键点，并将这些关键点在2D-LiDAR坐标系中的空间位置转换到采集末端的旋转云台一体化坐标系中；最后将前述结果用于旋转图像的空间投影及拼接。

①采集末端各自由度旋转参数到旋转云台一体化坐标系的转换

前述采集末端各自由度旋转参数的标定方案均是基于2D-LiDAR坐标系下的，需要将结果转化到旋转云台一体化坐标系下，以便于后续的虚拟单目相机关键点计算，以及旋转图像的拼接处理。

为此，需要将旋转云台的旋转参数结果转换到旋转云台一体化坐标系内(如图2中O_mot坐标系)。在认为采集机构的加工精度足够的前提下，按照设计图纸，滑轨坐标系O_sld与基座坐标系O_base之间旋转和平移均已知：

公式18中，M_base2sld(d)表示滑轨坐标系与基座坐标系之间的平移矩阵(与基座滑动位移d相关)，X_sld,Y_sld,Z_sld表示滑轨坐标系中点的坐标值，而X_base,Y_base,Z_base代表这些点在基座坐标系中的对应位置坐标。

基座坐标系O_base与云台偏转坐标系O_yaw之间旋转和平移均已知：

公式19中，R_yaw2base和t_yaw2base分别表示基座坐标系与云台偏转坐标系之间的旋转和平移矩阵(通过已知设计尺寸计算得到)，X_base,Y_base,Z_base表示基座坐标系中点的坐标值，而X_yaw,Y_yaw,Z_yaw则代表这些点在偏转(即绕Z轴旋转)坐标系中的对应位置坐标。

云台偏转坐标系O_yaw与旋转云台一体化坐标系O_mot之间旋转和平移也已知：

公式20中，R_mot2yaw和t_mot2yaw分别表示偏转坐标系与旋转云台一体化坐标系之间的旋转和平移矩阵(通过已知设计尺寸计算得到)，X_yaw,Y_yaw,Z_yaw表示云台偏转坐标系中点的坐标值，

则表示这些点在旋转云台Y、Z轴旋转角度均为零角度时，在一体化运动坐标中的对应坐标值。

在旋转云台一体化坐标系中，认为所有旋转均是绕原点进行旋转，即旋转云台绕Y轴旋转的旋转轴为旋转云台一体化坐标系的Y轴方向，旋转点为旋转云台一体化坐标系原点；旋转云台绕Z轴旋转的旋转轴为旋转云台一体化坐标系的Z轴方向，旋转点也是旋转云台一体化坐标系的原点。于是还原旋转后的旋转云台一体化坐标系中点可表示为：

公式21中，X_mot,Y_mot,Z_mot表示在旋转云台一体化坐标系中点的坐标值，

含义见公式20；β对应着此时旋转云台绕旋转云台一体化坐标系Z轴旋转的传感器记录角度，β₀表示Z轴旋转零角度时传感器记录角度，同理α与α₀分别表示旋转云台一体化坐标系Y轴旋转的相应角度；

与

分别表示旋转云台一体化坐标系Y轴和Z轴的旋转矩阵，矩阵具体形式见公式7。

因为旋转云台一体化坐标系中定义的

应与之前所标定旋转云台旋转轴

完全重合(

即为之前标定基于2D-LiDAR坐标系的归一化旋转方向向量)，且

的交点应与旋转云台一体化坐标系原点O_mot重合，计算

最小二乘交点：S_AYAZ＝[X_AYAZ,Y_AYAZ,Z_AYAZ]^T，有如下推导：

公式22中，[X_LS,Y_LS,Z_LS]表示2D-LiDAR坐标系内任一点的坐标值，其在旋转云台一体化坐标系中的对应坐标[X_mot,Y_mot,Z_mot]；R_LS2mot即为上述两个坐标系之间的旋转矩阵，t_LS2mot为平移矩阵，由之前标定的Y、Z轴旋转方向向量及其最小二乘交点确定。

综合公式18-22，即有2D-LiDAR坐标系到滑轨坐标系的转换公式，可以表述如下：

结合之前所述相机内参以及2D-LiDAR与相机间关系的标定结果(见公式5)，可计算在采集末端进行多自由度旋转后，相机图像坐标中任意点以及2D-LiDAR坐标系中任意点，至滑轨坐标系中的转换结果坐标。

同样的，对基于旋转云台一体化坐标系的旋转云台Y、Z轴旋转参数标定结果，运用激光点云数据进行校验，其结果原图并无差别。进一步的，随机旋转Y轴和Z轴，利用2D-LiDAR对办公室环境进行扫描，最后根据基于旋转云台一体化坐标系的旋转参数对激光点云数据进行还原。

②虚拟单目相机视场的关键点计算

根据4.4中所述2D-LiDAR与相机间参数的标定结果，可以将左右相机的视场统一为2D-LiDAR坐标系中的虚拟单目相机模式，以便于后续的左右相机图像拼接，如图11所示。

图中

表示左相机图像中的四个顶点在左相机坐标系中的位置，与左相机坐标系原点O_LC连线的直线，在2D-LiDAR坐标系Z_LS＝h平面的角点位置，而

则表示右相机图像的四个顶点与此平面的相交位置。O_E则表示左相机图像中心点与左相机坐标系原点O_LC连线的直线，与左相机图像中心点与左相机坐标系原点O_RC连线的直线，在2D-LiDAR坐标系中的最小二乘交点。取T₅为

和

连线在2D-LiDAR坐标系Y_LS＝0平面的交点，T₆为

和

的连线中点在2D-LiDAR坐标系Y_LS＝0平面的交点，T₇为

和

的连线在2D-LiDAR坐标系X_LS＝0平面的交点，T₈为

和

的连线在2D-LiDAR坐标系X_LS＝0平面的交点，T₀则表示2D-LiDAR坐标系Z轴与Z_LS＝h交点，即T₀(0,0,h)。

上述O_E,T₀,T₅,T₆,T₇,T₈点集，即为在2D-LiDAR坐标系下，用来表示左右相机视场等信息的虚拟单目相机视锥体关键点。并且由于左右相机与2D-LiDAR之间为刚体结构，故这些点在2D-LiDAR坐标系中的相对位置不会随着采集末端的旋转或运动而改变。下面将叙述如何计算这些点在2D-LiDAR坐标系中的位置：

以H_L,W_L分别表示左相机图像在纵向和横向的像素点个数，H_R,W_R分别表示右相机图像在横向和纵向的像素点个数，左右相机T₀～T₄点在相机图像平面坐标系中的坐标分别为：

图像点转换到相机坐标系中，随后将其转换到2D-LiDAR坐标系中，按照如下公式即可求得左右相机图像内顶点各自在2D-LiDAR坐标系内位置：

公式25中，

表示前述左相机T₀～T₄在图像平面中的像素坐标，

则表示右相机T₀～T₄在图像平面中的像素坐标；

分别表示左、右相机内参的逆矩阵；

分别表示左、右相机与2D-LiDAR之间旋转矩阵的逆矩阵；T_LS2LC,T_LS2RC分别表示左、右相机与2D-LiDAR之间的平移矩阵。

左、右相机坐标系原点在2D-LiDAR坐标系中转换坐标计算如下：

公式26中，

为左相机坐标系原点在2D-LiDAR坐标系中的表示，

为右相机坐标系原点在2D-LiDAR坐标系中的表示，其余符号含义与公式25中相同。

根据上述各点与相机坐标系原点在2D-LiDAR坐标系中相连所形成射线，与2D-LiDAR坐标中Z_LS＝h平面的交点。以左相机为例，其交点坐标为：

公式27中符号含义，见公式25、26。

进一步的，T₅,T₆,T₇,T₈在2D-LiDAR坐标系位置，可通过上述各点取中间点来计算，这里不再赘述。同样O_E在2D-LiDAR坐标系位置可以通过两条直线的最小二乘交点确定，这两条直线分别过左右相机的原点与T₀点，即：

公式28中，X,Y,Z表示需要计算的交点坐标值，其余符号见公式25、26，注意T₀点坐标与左、右相机原点坐标的区别。

综上，完成了T₅,T₆,T₇,T₈与O_E在2D-LiDAR坐标系中位置的计算，结合公式22，即可将这些点转换到旋转云台一体化坐标系中。最终，实现了基于旋转云台一体化坐标系的虚拟单目相机视场的关键点计算，进一步的可以用T₅,T₆,T₇,T₈与O_E来计算虚拟单目相机的垂直于水平方向视场角度等，以达到采集末端的一体化处理效果。

(7)旋转图像的空间投影及拼接

由于相机在拍摄时，其相平面与被拍摄平面并非平行，需要将图像投影到被拍摄物体的平面，随后将左右相机的图像进行拼接，以实现在三维模型表面纹理贴图等目的。

①单相机旋转图像的空间投影变换

本节所述相机图像变换的原理，即将相机图像从其各自的相机坐标系，投影到(6)中②所述的虚拟单目相机平面，下面描述其具体方法：

根据(6)-②中方法，计算左、右相机相平面四个角点在2D-LiDAR坐标系的对应坐标：

和

以及左、右相机坐标系原点在2D-LiDAR坐标系中的对应空间坐标：^LCO_LS和^RCO_LS。

此时采集末端传感器记录的旋转云台绕Y轴旋转角度α和绕Z轴旋转角度θ，根据公式17，可将左、右相机相平面角点在2D-LiDAR坐标系的位置还原到旋转云台在旋转零角度状态时的对应坐标，记为

^LCO_LS-0和^RCO_LS-0。

随后计算这些角点和坐标系原点的连线，在还原至旋转零角度后的2D-LiDAR坐标系Z＝h平面的交点位置。以左相机为例，记这些交点为

其计算公式如下：

公式29中，

表示前述

点坐标值的XYZ分量，

则表示左相机相平面四个角点还原至旋转零角度后的点

的XYZ分量，^LCO_LS-0(X)，^LCO_LS-0(Y)，^LCO_LS-0(Z)则表示左相机坐标系原点在还原至旋转零角度后的^LCO_LS-0XYZ分量。

获取这些点在2D-LiDAR坐标系中的最大和最小X、Y坐标值，并以其作为透视变换后图像尺寸的变换依据：

公式30中，

表示在

四个点中，X分量最大值，

表示X分量的最小值，

和

分别为Y分量的最大值和最小值。

右相机的最大最小X、Y坐标计算方法类似。设定透视变换后的图像尺寸与原图像尺寸相同，则原图像四个顶点对应的透视变换后图像坐标，以左相机为例，可以按如下公式来计算：

公式31中，

与

分别表示原左相机图像四个角点在透视变换后图像中的像素坐标，其余符号含义详见公式29、30。

按照上述步骤，得到了四个控制点在原始图像和透视变换后图像中的坐标位置，计算透视变换矩阵，即可得到左右相机图像的透视变换结果(如图12和图13所示)。

②旋转图像的拼接

进一步的，采集末端上安装左右两个相机，是为了弥补单相机拍摄视场不足，很多场景下需要将左右相机的图像拼接到一起，形成(6)-②类似单目相机的图像。于是需要将左右相机的图像都变换到同一图像上，再对俩变换后的图像进行权重相加形成一张拼接后的图。于是有：

公式32中，

和

表示在拼接图像四个角点与Z＝h平面交点在X轴方向的最大值和最小值，类似的，

和

表示Y轴方向的最大值和最小值，其他符号含义如公式30中所述。

设定拼接后图像纵向的像素个数为H＝H_L，则横向的像素个数为：

公式33中，H_L表示左相机原始图像在纵向的像素点数，W为新拼接图像在横向的像素点数，其余符号见公式32。

按照与之前同样的方法(见公式31)，可以得到左相机图像四个顶点在新拼接图像中的像素坐标：

在公式34中，

和

表示左相机图像的四个角点在新拼接图像中的像素坐标，

和

含义见公式30-，其他符号公式32中含义相同。经过变换后的左相机图像如图14的左上角区域图所示。

右相机的变换原理与左相机一致，采用透视变换计算即可得到左右相机图像的透视变换到拼接图像的结果，如图14的右上角区域图所示。

随后将两张通过加权相加的方法，实现其拼接，最终结果如图14下部区域图所示，左右相机两张图像拼接在一起，左右相机图像重叠部分重合效果较好。

更进一步的，可以将多组在不同旋转角度位置的图像，按照①与②中方法，根据2D-LiDAR获取的激光点云数据所反映的观测场景真实情况，以提取出不同的相机拍摄平面位置，将相机图像投影到这些平面，并进行拼接，最终实现旋转图像在3D模型中的正确贴图，如图17所示，左右相机拍摄的混泥土桥桥底图像能正确贴合在桥底的多平面模型上。

综上所述，本发明实例仅需多自由度采集末端一次观测特制的U型标定板就可以完成相机内参数、2D-LiDAR与相机关系参数以及采集末端各个旋转轴参数的标定，提高了外参数标定的效率和准确性；通过旋转云台的转动，实现了多传感器几何参数的标定，确定了旋转云台的运动学方程，形成了图像与2D激光点云的有效融合。

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

Claims (10)

1.一种基于一体化标定参数的图像空间投影拼接方法，其特征在于，包括如下步骤：

步骤一、建立一体化标定系统；

所述一体化标定系统包括标定板和多自由度采集末端，多自由度采集末端包括旋转云台，旋转云台包括2D-LiDAR和至少两个相机；旋转云台固定连接在Y轴旋转轴，Y轴旋转轴轴接连接有Z轴旋转轴；所述标定板包括ChArUco标定板，ChArUco标定板一侧固定有矩形侧板，另一侧固定有等腰直角三角形侧板，等腰直角三角形侧板的一个直角边为ChArUco标定板的侧边；矩形侧板外侧固定有第一边侧板；等腰直角三角形侧板的斜面上固定有第二边侧板；矩形侧板和等腰直角三角形侧板所在平面均与ChArUco标定板所在平面垂直设置；第一边侧板所在平面与ChArUco标定板坐在平面平行，第二边侧板所在平面与等腰直角三角形侧板所在平面垂直设置；

步骤二、获取2D-LiDAR单次扫描标定板的点云数据，提取标定板的角点信息，根据角点信息确定多自由度采集末端在各个旋转自由度时的零点位置；

步骤三、驱动Y轴旋转轴和Z轴旋转轴预设轨迹旋转，并采集相机和2D-LiDAR数据形成完整的标定数据集；

步骤四、

提取完整的标定数据集中每张图像中的ArUco特征点和ChArUco角点坐标序列，所述ArUco特征点包括二维码的ID号和二维码四个顶点的平面坐标；根据采集末端不同旋转角度时相机图像中相应二维码四个顶点的平面坐标信息，标定相机内参；

步骤五、在获取2D-LiDAR扫描标定板的点云数据，提取标定板角点，计算各标定板角点在相应相机图像中的平面坐标信息，结合各个标定板角点的空间和平面坐标信息，确定2D-LiDAR和相机之间的旋转矩阵和平移矩阵；

步骤六：获取2D-LiDAR绕多自由度采集末端的Z轴旋转轴和Y轴旋转轴旋转得到的点云数据和相机图像，进行多自由度采集末端的旋转云台的Y轴和Z轴旋转参数的联合标定；

步骤七：将Z轴旋转轴和Y轴旋转轴标定结果在2D-LiDAR坐标系内表示；

步骤八：多自由度采集末端采集目标物的图像，并将采集到图像的像素点均转化到2D-LiDAR坐标系内，完成目标物图像的拼接。

2.如权利要求1所述的基于一体化标定参数的图像空间投影拼接方法，其特征在于，所述步骤二包括如下步骤：

相机朝向ChArUco标定板，2D-LiDAR朝向与矩形侧板所在平面平行的方向，采集2D-LiDAR扫描标定板的激光点云；对激光点云进行稀疏激光均匀化采样，激光滤波，得到均匀连续的激光点云；将均匀连续的激光点云转换成图像，对图像进行形态学标定板角点取激光点云清晰连续的骨架，获得获取图像中激光点云的轮廓，当轮廓中包含的点的个数大于或等于预设值时，则为有效有效激光轮廓，否则为无效激光轮廓，重新采集2D-LiDAR扫描标定板的激光点云；

根据有效激光轮廓，进行多边形拟合后，提取有效激光轮廓中关于标定板的多边形轮廓线段，通过外扩矩阵确定标定板角点位置：

式1中，K₁与K₂分别表示多边形轮廓上任一线段的起始点和终止点的坐标；

K₃和K₄分别表示线段K₁K₂经过外扩后对应的的起始点和终止点的坐标；K₃和K₄作为更新后的标定板角点位置；

然后根据给定的阈值s，计算外扩矩阵四个顶点的坐标，得到获取包含4个顶点的最小外接矩形；s表示此外接矩形高度的二分之一；

通过给定的阈值s，计算中矩形的4个顶点在2D-LiDAR坐标系中的坐标值，并获取包含4个顶点的最小外接矩形，以判断激光点云在哪个矩形内，将所有激光点云分配到对应的矩形内，然后将矩形内的激光点拟合得到直线，根据两两直线的交点得到有效激光轮廓的角点，分别为p0、p1、p2、p3、p4、p5，其中p1、p2、p3、p4即标定板角点；

其中，d＝3δ，s＝2δ，δ表示2D-LiDAR的测量误差；

驱动多自由度采集末端的Y轴旋转轴旋转，采集序列激光剖面，并提取处于ChArUco标定板上p2p3连线与2D-LiDAR坐标系中X轴方向的夹角，取夹角最小的位置为Y轴的零点，记录Y轴为零点时对应的Y轴机械角度θ，并驱动Y轴旋转至Y轴的零点；然后驱动Z轴旋转轴旋转，采集序列激光剖面，并提取线段p2p3的长度，取p2p3长度最小时的位置为Z轴的零点，Z轴的零点对应的Z轴机械角度β，将{θ,β}角度作为多自由度采集末端的零点位置，即完成多自由度采集末端的零点位置标定。

3.如权利要求2所述的基于一体化标定参数的图像空间投影拼接方法，其特征在于，所述步骤三包括如下步骤：

将多自由度采集末端的Y轴旋转轴与Z轴旋转轴均置于零点位置，然后分两步进行标定采集相机和2D-LiDAR数据形成完整的标定数据集：

保持Z轴旋转轴处于Z轴的零点位置不动，驱动多自由度采集末端的Y轴旋转轴转动，每次转动1±0.5°；每次Y轴旋转轴转动后，多自由度采集末端上的两个相机拍摄图像并保存，记录2D-LiDAR一帧点云数据，同时记录多自由度采集末端在Y轴自由度和Z轴自由度的旋转角度，作为一组标定数据；在每个旋转位置保存对应的一组标定数据，作为Y轴转动的标定数据集；

然后保持Y轴旋转轴处于Y轴的零点位置不动，驱动多自由度采集末端的Z轴旋转轴转动，每次转动1±0.5°，将转动过程中每组相机图像、激光点云和多自由度采集末端的姿态数据保存下来，作为Z轴转动的标定数据集；

Y轴转动的标定数据和Z轴转动的标定数据集形成完整的标定数据集。

4.如权利要求3所述的基于一体化标定参数的图像空间投影拼接方法，其特征在于，所述步骤四包括如下步骤：

提取Y轴转动的标定数据中每张图像中的ArUco特征点，所述ArUco特征点包括二维码的ID号和二维码四个顶点的平面坐标；同时提取出提取出每张图像的ChArUco角点坐标序列；基于Y轴旋转轴旋转的多个位置，图像坐标系上的角点序列和相对应的世界坐标序列，根据棋盘格标定方法标定出相机的内参数矩阵K和畸变参数矩阵D，完成相机内参的标定:

式2中，f_x和f_y分别表示x与y方向上像素与实际距离的比例；u₀，v₀分别为图像中心在像素平面即图像坐标系的u、v轴坐标，u为图像宽度方向为方向，v为图像高度方向；k₁，k₂，k₃表示镜头的径向畸变参数；m₁,m₂表示镜头的切向畸变参数。

5.如权利要求4所述的基于一体化标定参数的图像空间投影拼接方法，其特征在于，所述步骤五包括如下步骤：

根据Y轴转动的标定数据集，获取激光点云数据，提取有效激光轮廓中的标定板角点，即得到标定板角点p1、p2、p3和p4在2D-LiDAR坐标系中的空间坐标{P_i ^L|i＝1,2,3,4}；

根据得到的激光轮廓的角点坐标，通过标定板结构的几何约束，设ChArUco标定板处于标定板世界坐标系Z＝0平面，于是通过{P_i ^L|i＝1,2,3,4}以及标定板的几何约束得到所述标定板角点在标定板世界坐标系的坐标{P_i ^W|i＝1,2,3,4}，即有：

式3中，d为二维码边缘与ChArUco标定板边缘的距离，w为ChArUco标定板的边长；

以相机图像中提取的二维码四个顶点作为控制点，结合标定的相机内参数及畸变参数，通过相机标定方法标定出相机外参，即相机坐标系与标定板世界坐标系之间的旋转矩阵R_W2C与平移矩阵T_W2C，然后确定标定板角点在图像坐标系中的实际坐标：

式4中，^WX_i,^WY_i,^WZ分别为标定板角点在标定板世界坐标系中的X、Y、Z分量；

分别表示标定板角点在相机坐标系中的X、Y、Z分量；

分别表示标定板角点在图像中的u轴和v轴坐标值；

得到标定板角点在图像坐标系中的理论坐标信息：

式5中，R_L2C和T_L2C分别表示2D-LiDAR与相机间的旋转矩阵和平移矩阵；^LX_i,^LY_i,^LZ_i分别为标定板角点的X、Y、Z分量；^CX_i,^CY_i,^CZ_i分别表示标定板角点在相机坐标系中对应的理论坐标值；^Iu_i,^Iv_i分别表示标定板角点在图像中的理论像素坐标值；

至此，定义损失函数为：

式6中，Δu和Δv分别表示像素点在u和v方向的偏差；

采用最小二乘法，迭代求解得到2D-LiDAR与相机间旋转矩阵和平移矩阵。

6.如权利要求5所述的基于一体化标定参数的图像空间投影拼接方法，其特征在于，所述步骤六包括如下步骤：

根据完整的标定数据集，进行旋转云台绕Z轴和Y轴的旋转参数标定，标定结果包括代表旋转轴方向的单位向量和绕轴旋转点，旋转云台绕Z轴和Y轴的标定结果最后都统一到同一个坐标系2D-LiDAR坐标系下：

首先，设绕Y轴旋转轴旋转的旋转向量就是2D-LiDAR坐标系中Y轴(0,1,0)，通过由粗到细的搜索方法求解绕得到Y轴旋转轴的旋转点，同样通过由粗到细的搜索方法得到Z轴旋转轴的旋转点；利用相机图像中的ArUco特征点作为外部控制点，将旋转时相机图像的ArUco特征点，与旋转到零点角度时相机图像的ArUco特征点之间的偏差作为误差函数，利用最小二乘法，迭代求解误差函数最小时的旋转轴方向与旋转点；求解得到旋转云台的Y轴、Z轴的旋转参数，作为迭代求解时Y、Z轴的旋转轴向量和旋转点的初始值。

7.如权利要求6所述的基于一体化标定参数的图像空间投影拼接方法，其特征在于，所述由粗到细的搜索方法包括如下步骤：

一)获取Z轴旋转轴旋转不同角度的一系列激光点云数据，读取激光的单次点云数据，记录Z轴旋转轴第i次旋转的角度θ_i；对第i次转台绕Z轴旋转后采集到的的原始点云数据，进行稀疏激光均匀化上采样和滤波处理，得到有效的点云数据，提取得到标定板角点p2、p3；最后得到标定板角点p2、p3与Z轴旋转轴旋转角度的序列{^LSP_i ^j,θ_i}(j＝2,3)，其中，i表示第i次旋转，j表示p2或p3点，^LSP_i ^j则表示第i次旋转时p2或p3点在2D-LiDAR坐标系中的坐标值；

二)将2D-LiDAR绕多自由度的旋转简化为空间上一点绕两向量旋转，存在如下关系：

r₀＝^Ln_x·^Ln_y·(1-cosθ)+cosθ,r₁＝^Ln_x·^Ln_y·(1-cosθ)+^Ln_z·sinθ

r₂＝^Ln_z·^Ln_x·(1-cosθ)-^Ln_y·sinθ,r₃＝^Ln_x·^Ln_y·(1-cosθ)-^Ln_z·sinθ

r₄＝(^Ln_y)²·(1-cosθ)+cosθ,r₅＝^Ln_y·^Ln_z·(1-cosθ)+^Ln_x·sinθ

r₆＝^Ln_z·^Ln_x·(1-cosθ)+^Ln_y·sinθ,r₇＝^Ln_y·^Ln_z·(1-cosθ)-^Ln_x·sinθ

r₈＝(^Ln_z)²·(1-cosθ)+cosθ

(式7)

式7中，θ为Z轴旋转轴旋转的角度，

表示Z轴旋转轴旋转的方向向量，Sz表示Z轴旋转轴的旋转点，

则表示旋转云台绕

方向旋转θ角度对应的旋转矩阵，P₁表示2D-LiDAR坐标系内任意一点，P₁绕

轴旋转θ角度对应点为P₀；r0、r1、r2...r8分别表示旋转矩阵

的各元素X_AZ,Y_AZ,Z_AZ,分别表示绕Z轴旋转点S_Z在2D-LiDAR坐标系的X、Y、Z值，^Ln_x，^Ln_y，^Ln_z分别表示旋转轴向量

在2D-LiDAR坐标系的X、Y、Z轴分量；

确定点P₀绕Y轴旋转轴

轴旋转α后的几何关系：

式8中，α表示旋转云台绕Y轴旋转的角度，

表示绕Y轴旋转的方向向量，S_Y则表示绕Y轴旋转点，

表示旋转云台绕

方向旋转α角度对应的旋转矩阵；P₂则表示P₀在绕

旋转α角度后的对应点；

将绕Z轴旋转轴旋转后的标定板角点p2、p3还原到旋转零角度的公式即为：

公式9中，θ₀表示在Z轴旋转轴旋转标定为零角度时，传感器记录的实际角度，而θ_i则表示在第i次Z轴旋转轴旋转时所记录的角度值；

^LSP_i ^j(X),^LSP_i ^j(Y),^LSP_i ^j(Z)分别表示Z轴旋转轴第i次旋转后，标定板第j个角点在2D-LiDAR坐标系中的坐标值，

分别表示标定板角点在还原至绕Z轴旋转零角度后，在2D-LiDAR坐标系中的坐标值；

将所有采集末端绕Z轴旋转采集的激光点云数据均还原到绕Z轴旋转零角度，即形成点云数据；

三)设旋Z轴旋转轴旋转轴向量为

计算Z轴旋转轴的旋转点，根据由粗到细的搜索策略，先利用循环迭代的方法进行粗略的求解：

A.初始时，设旋Z轴旋转轴的旋转点为S_Z(0,0,0)；

B.Z轴旋转轴旋转采集到的所有激光点云数据，均还原到Z轴旋转轴旋转角度为零时的状态；

C.根据还原后的点云数据，计算得到包含还原后的点云数据的最小外接矩阵，记最小外接矩阵的四个顶点为{^Lq_k}(k＝1,2,3,4)；

D.将四个顶点连线，形成四条线段{l_k}(k＝1,2,3,4)，随后给定阈值d和s，得到l_k线段各自的外扩矩形；

E.依次判断激光点云落在哪个外扩矩形内，并形成每个外扩矩形的激光点集；根据每个矩形内点集拟合直线，并计算矩形内所有点到对应直线的距离累加值；

F.计算四个外扩矩形的顶点到拟合直线距离累加值之和，如果小于上一次迭代的累加值之和，则根据阈值Δ_y将旋转点S_Z的Y分量加上Δ_y；

Δ_y＝1-3mm；

G.若迭代次数超过设定阈值次数则结束，否则回到步骤B运行循环；

迭代过程停止之后，得到粗略求解的采集末端绕Z轴旋转点，记为

其中

表示

点在2D-LiDAR坐标系中的X分量，

表示

点在2D-LiDAR坐标系中的Y分量，

表示

点在2D-LiDAR坐标系中的Z分量；

将

作为细化搜索的迭代初值，设定一个绕Z轴旋转轴旋转点Y分量的迭代阈值d_y，d_y＝0.1*Δ_y，同时将细化搜索时旋转点Y值搜索范围控制在

之内；于是按照步骤A至步骤F的迭代过程，细化搜索得到旋转云台绕Z轴旋转轴的旋转点，最终结果得到的旋转点记为

其中

表示

点在2D-LiDAR坐标系中的X分量，

表示

点在2D-LiDAR坐标系中的Y分量，

表示

点在2D-LiDAR坐标系中的Z分量；

根据步骤一)至步骤三)，同样得到旋转云台绕Y轴的旋转点，记为

其中

表示

点在2D-LiDAR坐标系中的X分量，

表示

点在2D-LiDAR坐标系中的Y分量，

表示

点在2D-LiDAR坐标系中的Z分量；完成对Z轴旋转轴和Y轴旋转轴的标零。

8.如权利要求7所述的基于一体化标定参数的图像空间投影拼接方法，其特征在于，包括如下步骤：

a.在对Z轴旋转轴和Y轴旋转轴标零之后，记录下标零时Z轴旋转轴和Y轴旋转轴的传感器角度值，记为θ₀，α₀，运用图像处理方法提取此时相机图像中ArUco特征点的像素坐标，记为：

其中j表示旋转轴在零角度时，相机图像中第j个ArUco二维码，k则表示第j个ArUco二维码的第k个顶点，LC表示左相机图像，RC则表示右相机图像；

表示在旋转零角度时，左相机图像中，第j个ArUco二维码的第k个顶点在图像坐标系中的u轴坐标，

表示左相机图像中，第j个ArUco二维码的第k个顶点在图像坐标系中的v轴坐标；

分别表示右相机图像中相应点的u轴和v轴坐标；

根据Y轴转动的标定数据和Z轴转动的标定数据集，当第i次Z轴旋转轴和Y轴旋转轴旋转角度分别α_i,θ_i,提取相机图像ArUco特征点：

i表示第i次旋转。

根据左相机的内参与外参，将左相机的图像坐标转化为标定板世界坐标：

式11中，i表示第i次旋转，j表示图像中的第j个ArUco二维码，k则表示二维码的第k个顶点；R_W2C与T_W2C分别表示左相机的旋转矩阵和平移矩阵，K^LC为左相机内参数矩阵；

为左相机图像ArUco特征点的像素坐标，

分别为特征点在左相机坐标系中X、Y、Z轴的坐标，

分别为特征点在标定板世界坐标系中X、Y、Z轴对应坐标值；

因图像中ArUco二维码的四个顶点世界坐标均已知，故求得

根据2D-LiDAR与相机间关系标定结果，得到ArUco二维码各顶点在2D-LiDAR坐标系中的空间坐标表示：

式12中，R_L2LC与T_L2LC分别表示2D-LiDAR与左相机之间的旋转矩阵和平移矩阵，

表示第i次旋转时，左相机图像的第j个ArUco二维码的第k个顶点在2D-LiDAR坐标系中的对应坐标值；

同理根据根据右相机的内参与外参，将右相机的图像坐标转化为2D-LiDAR坐标；在2D-LiDAR坐标系下，旋转云台绕Y轴的旋转轴为

绕轴旋转点为[X_AY,Y_AY,Z_AY]^T,将在2D-LiDAR坐标系下的ArUco特征点还原到绕Y轴旋转轴零角度时的坐标，坐标表示为：

式13中，α₀表示Y轴旋转轴在零角度时，传感器记录的旋转角度值，α_i则为第i次旋转时传感器记录的旋转角度值；

分别对应着在2D-LiDAR坐标系下相应特征点还原至Y轴旋转轴在零角度时的坐标；

同样的，在2D-LiDAR坐标系下，旋转云台绕Z轴的旋转轴为

绕轴旋转点为[X_AZ,Y_AZ,Z_AZ]^T，则ArUco二维码各顶点还原到绕Z轴旋转零角度，坐标表示为：

式14中，θ₀表示旋转云台绕Z轴旋转零角度时，传感器记录的旋转角度值，θ_i则为第i次旋转时传感器记录的旋转角度值；

对应着相应特征点还原至2D-LiDAR坐标系下绕Z轴旋转轴在零角度时的坐标；利用相机内参数得到ArUco特征点还原至Z轴旋转轴和Y轴旋转轴均在零角度时的像素坐标，称之为理论像素坐标；具体推导公式如下：

式15中，

即为ArUco特征点在Z轴旋转轴和Y轴旋转轴均在零角度时的理论像素坐标，

为对应的理论2D-LiDAR坐标值；

关注理论像素坐标，与Z轴旋转轴和Y轴旋转轴均在零角度时所提取的ArUco特征点像素坐标，建立误差函数如下：

式16中，Δ_Lu表示左相机图像在图像坐标系u方向的像素差，Δ_Lv表示左相机图像在图像坐标系v方向的像素差，Δ_Ru和Δ_Rv则表示右相机图像的相应差值，Δ_u和Δ_v则反映两个相机在u和v方向的整体像素偏差；根据式16，利用最小二乘法迭代求解旋转云台绕Z轴和Y轴的旋转参数，将Z轴旋转轴和Y轴旋转轴均在零点时的数据作为初始值，即：

最终解得旋转云台绕Z轴和Y轴的旋转参数，包括旋转轴

及其对应的绕轴旋转点S_Y,S_Z；

将所有在2D-LiDAR坐标系下的点按照如下公式还原到Z轴旋转轴和Y轴旋转轴均在零角度时的状态：

式17中，^LX,^LY,^LZ表示在2D-LiDAR坐标系下表示的任意一点的X、Y、Z轴坐标，α和θ则表示此时传感器记录的旋转云台绕Y轴和Z轴的旋转角度，^LZ⁰,^LZ⁰，^LZ⁰则表示将所述任意一点还原至旋转平台绕Y、Z轴旋转零角度时的坐标值；

b.将旋转云台的旋转参数结果转换到旋转云台一体化坐标系内，其中Y轴旋转轴通过基座滑动连接有直线滑轨；

滑轨坐标系O_sld与基座坐标系O_base之间旋转和平移均已知：

式18中，M_base2sld(d)表示滑轨坐标系与基座坐标系之间的平移矩阵，X_sld,Y_sld,Z_sld表示滑轨坐标系中点的X、Y、Z轴的坐标值，而X_base,Y_base,Z_base代表滑轨坐标系中的点在基座坐标系中的对应位置坐标；

基座坐标系O_base与云台偏转坐标系O_yaw之间旋转和平移均已知：

式19中，R_yaw2base和t_yaw2base分别表示基座坐标系与云台偏转坐标系之间的旋转和平移矩阵，X_base,Y_base,Z_base分别表示基座坐标系中点的X、Y、Z轴坐标值，而X_yaw,Y_yaw,Z_yaw则代表基座坐标系中的点在绕Z轴旋转的偏转坐标系中的对应位置坐标；

云台偏转坐标系O_yaw与旋转云台一体化坐标系O_mot之间旋转和平移也已知：

式20中，R_mot2yaw和t_mot2yaw分别表示偏转坐标系与旋转云台一体化坐标系之间的旋转和平移矩阵，X_yaw,Y_yaw,Z_yaw表示云台偏转坐标系中点的X、Y、Z轴坐标值，

则表示云台偏转坐标系的点在旋转云台Y、Z轴旋转角度均为零角度时，在一体化运动坐标中的对应坐标值；

还原旋转后的旋转云台一体化坐标系中点表示为：

式21中，X_mot,Y_mot,Z_mot表示在旋转云台一体化坐标系中点的X、Y、Z轴坐标值；β为旋转云台绕旋转云台一体化坐标系Z轴旋转的传感器记录角度，β₀表示Z轴旋转零角度时传感器记录角度，α与α₀分别表示旋转云台一体化坐标系Y轴旋转的相应角度；

与

分别表示旋转云台一体化坐标系Y轴和Z轴的旋转矩阵；

在旋转云台一体化坐标系中

与所标定旋转云台三个方向的旋转轴

完全重合，其中，

分别为基于2D-LiDAR坐标系的y轴和z轴旋转方向单位向量，且

的交点与旋转云台一体化坐标系原点O_mot重合，计算

最小二乘交点：S_AYAZ＝[X_AYAZ,Y_AYAZ,Z_AYAZ]^T，有如下推导：

式22中，[X_LS,Y_LS,Z_LS]分别表示2D-LiDAR坐标系内任一点的X、Y、Z轴坐标值，在旋转云台一体化坐标系中的对应的X、Y、Z轴坐标为[X_mot,Y_mot,Z_mot]；R_LS2mot即为2D-LiDAR坐标系何旋转云台一体化坐标系之间的旋转矩阵，t_LS2mot为平移矩阵；X_AYAZ、Y_AYAZ和Z_AYAZ分别表示

最小二乘交点S_AYAZ在2D-LiDAR坐标系中的X、Y、Z轴坐标；

综合式18-22，即有2D-LiDAR坐标系到滑轨坐标系的转换公式，表述如下：

X_sld、Y_sld、Z_sld分别表示2D-LiDAR坐标系中任意一点[X_LS,Y_LS,Z_LS]，在还原至旋转云台旋转零角度时，对应滑轨坐标系中点的X、Y、Z轴坐标；

将相机图像坐标中任意点以及2D-LiDAR坐标系中任意点，至滑轨坐标系中的转换为结果坐标。

9.如权利要求1所述的基于一体化标定参数的图像空间投影拼接方法，其特征在于，所述步骤八包括如下步骤：

Ⅰ.将左右相机的视场统一为2D-LiDAR坐标系中的虚拟单目相机模式；

以H_L,W_L分别表示左相机图像在纵向和横向的像素点个数，H_R,W_R分别表示右相机图像在横向和纵向的像素点个数，左右相机t₀～t₄点在相机图像平面坐标系中的坐标分别为：

其中，

分别表示左相机图像中的四个角点，以及这些点在图像坐标系中u、v轴坐标位置，

则表示左相机图像中心点在图像坐标系中u、v轴坐标位置；同样的

与

则表示右相机图像中相应点在图像坐标系中u、v轴坐标位置；

将图像点转换到相机坐标系中，随后转换到2D-LiDAR坐标系中，按照如下公式求得左右相机图像四个顶点与中心点在2D-LiDAR坐标系内坐标位置：

式25中，

分别表示前述左相机

在图像平面中的u、v轴像素坐标，

则分别表示右相机

在图像平面中的u、x轴像素坐标；

分别表示左、右相机内参的逆矩阵；

分别表示左、右相机与2D-LiDAR之间旋转矩阵的逆矩阵；T_LS2LC,T_LS2RC分别表示左、右相机与2D-LiDAR之间的平移矩阵；

表示左相机

点在2D-LiDAR坐标系中对应点的X、Y、Z轴坐标，而

则表示右相机

点在2D-LiDAR坐标系中对应点的X、Y、Z轴坐标；

左相机坐标系原点O_LC和右相机坐标系原点O_RC，在2D-LiDAR坐标系中对应点^LCO_LS与^RCO_LS，其转换坐标计算如下：

式26中，

分别为左相机坐标系原点在2D-LiDAR坐标系中的X、Y、Z轴坐标，

为右相机坐标系原点在2D-LiDAR坐标系中X、Y、Z轴坐标；根据

和

与左、右相机坐标系原点在2D-LiDAR坐标系中相连所形成射线，射线与2D-LiDAR坐标中Z_LS＝h平面的交点；Z_LS表示2D-LiDAR坐标系的Z轴，h表示相机拍摄平面与2D-LiDAR坐标系原点的垂直距离；Z_LS＝h平面即虚拟单目相机平面；

射线与Z_LS＝h平面的交点记为：

左相机的交点

的X、Y与Z轴坐标计算如下：

同理得到右相机的相关交点

计算得到射线与Z_LS＝h平面的交点在2D-LiDAR坐标系中具体位置后，得到O_E,T₅,T₆,T₇,T₈在2D-LiDAR坐标系位置；

O_E表示左相机图像中心点与左相机坐标系原点O_LC连线的直线与右相机图像中心点与右相机坐标系原点O_RC连线的直线，在2D-LiDAR坐标系中的最小二乘交点；T₅为

和

连线在2D-LiDAR坐标系Y_LS＝0平面的交点；T₆为

和

的连线中点在2D-LiDAR坐标系Y_LS＝0平面的交点；T₇为

和

的连线在2D-LiDAR坐标系X_LS＝0平面的交点；T₈为

和

的连线在2D-LiDAR坐标系X_LS＝0平面的交点；

O_E在2D-LiDAR坐标系中位置，通过两条直线的最小二乘交点确定。所述两条直线分别过左右相机的原点与T₀点，具体计算方法：

式28中，X,Y,Z分别表示需要计算的T₀在2D-LiDAR坐标X、Y、Z轴的坐标值坐标值；完成基于旋转云台一体化坐标系的虚拟单目相机视场的关键点计算；

Ⅱ.旋转图像的空间投影及拼接

2.1.单相机旋转图像的空间投影变换

相机图像从各自的相机坐标系，投影虚拟单目相机平面：

计算左、右相机相平面四个角点在2D-LiDAR坐标系的对应坐标：

和

以及左、右相机坐标系原点在2D-LiDAR坐标系中的对应空间坐标：^LCO_LS和^RCO_LS；

此时传感器记录的旋转云台绕Y轴旋转角度α和绕Z轴旋转角度θ，根据式17，将左、右相机相平面角点和左、右相机坐标系原点在2D-LiDAR坐标系的位置还原到旋转云台在旋转零角度状态时的对应坐标，分别记为

^LCO_LS-0和^RCO_LS-0；

得到左、右相机相平面角点和坐标系原点的连线，在还原至旋转零角度后的2D-LiDAR坐标系Z＝h平面的交点位置；对于左相机交点位置分别为：

计算公式如下：

式29中，

表示

点坐标值的XYZ分量，

则分别表示左相机相平面四个角点还原至旋转零角度后的点

的XYZ分量，^LCO_LS-0(X)，^LCO_LS-0(Y)，^LCO_LS-0(Z)则分别表示左相机坐标系原点在还原至旋转零角度后的^LCO_LS-0XYZ分量；

左相机相平面四个角点和左相机坐标系原点在还原至旋转零角度后的点在2D-LiDAR坐标系中的最大和最小X、Y坐标值，并作为透视变换后图像尺寸的变换依据：

式30中，

表示在

四个点中，X分量最大值，

表示X分量的最小值，

和

分别为Y分量的最大值和最小值；

同样得到右相机相平面四个角点和右相机坐标系原点在还原至旋转零角度后的点在2D-LiDAR坐标系中的最大和最小X、Y坐标值；

2.2.设定透视变换后的图像尺寸与原图像尺寸相同，则原图像四个顶点对应的透视变换后图像坐标，对于左相机按如下公式进行计算：

式31中，

与

分别表示原左相机图像四个角点在透视变换后图像中的像素坐标；

根据四个控制点在原始图像和透视变换后图像中的坐标位置，计算透视变换矩阵，即得到左相机图像的透视变换结果；同理可得右相机图像的透视变换结果；

2.3.旋转图像的拼接

将左右相机的图像都变换到同一图像上，再对两个变换后的图像进行权重相加形成一张拼接后的图，于是有：

式32中，

和

分别表示在拼接图像四个角点与Z＝h平面交点在X轴方向的最大值和最小值，

和

表示拼接图像四个角点与Z＝h平面交点在Y轴方向的最大值和最小值；

设定拼接后图像纵向的像素个数为H＝H_L，则横向的像素个数为：

式33中，H_L表示左相机原始图像在纵向的像素点数，W为新拼接图像在横向的像素点数；

得到左相机图像四个顶点在新拼接图像中的像素坐标：

在式34中，

和

分别表示左相机图像的四个角点在新拼接图像中的像素坐标；

同样得到右相机图像的四个角点在新拼接图像中的像素坐标；

将左相机图像和右相机图像通过加权相加的方法拼接得到新拼接图像。

10.如权利要求9所述的基于一体化标定参数的图像空间投影拼接方法，其特征在于，将多组旋转云台在不同旋转角度位置拍摄的图像投影到Z_LS＝h平面，得到对应的新拼接图像；在世界坐标系内旋转新拼接图像至旋转云台处于零点时的位置，实现旋转图像在3D模型中的还原贴图。

Images