[发明专利]一种基于一体化标定参数的图像空间投影拼接方法

权利要求书

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轴的旋转参数标定，标定结果包括代表旋转轴方向的单位向量和绕轴旋转点，旋转云台绕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轴旋转轴旋转角度的序列{^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轴旋转轴的标零；

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轴的旋转角度，^LX⁰、^LY⁰、^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坐标系中任意点，至滑轨坐标系中的转换为结果坐标；

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

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