多维度环境信息采集平台及其标定方法

1.本发明属于导航与定位领域，具体涉及一种基于微型固态激光雷达和摄像头环状阵列的头戴式环境信息采集平台及其标定方法。


背景技术：

2.近年来，随着技术发展，环境感知的方法逐步成熟，但相关技术主要应用于智能汽车和机器人导航等方面，而适用于人体的方法和设备相对较少。为了实现人体视角下室内外环境下感知与导航的功能，进而达到行人出行环境感知与导航、增强现实(ar)的环境感知等目的，需要构建全方位的环境信息采集平台，并设计合适的算法进行标定。
3.目前，环境感知所借助的设备集中于激光雷达和视觉两个方面，且主要使用于机器人导航和自动驾驶领域，例如：公开号为cn 113160330a的中国专利申请提出了一种基于端到端的相机与激光雷达标定方法，提升了相机与激光雷达联合标定之精度，改善了传统卷积神经网络方法的误差问题，提出了新的特征融合方式，可应用于在自动驾驶等场景。
4.公开号为cn113409399a的中国专利申请提出了一种双相机联合标定方法、系统及装置，采用一个内参已知的标准的左相机对待标定的右相机进行标定，在标定的过程中对右相机的内参进行联合优化，在标定方法简单的前提下具有较高精度。
5.公开号为cn110322519a的中国专利申请提出了一种用于激光雷达与相机联合标定的标定装置及标定方法，通过构建特殊形状的标定板，并通过“拟合插值法”实现多次测量数据的处理，进而完成标定。
6.适用于人体的方法和设备，相关的研究和专利数量仍然较少。


技术实现要素：

7.本发明所解决的技术问题是：为探索人体视角下环境感知的方法，提出一种基于“微型固态激光雷达—广角相机”阵列的多维度环境信息采集平台及其标定方法，用于出行场景下的周围环境感知。
8.本发明为解决以上技术问题，采用如下技术方案：首先，本发明提出一种多维度环境信息采集平台，包括：贴合人体头部形状的环状支架、3~10个微型固态激光雷达，以及标定模块，其中微型固态激光雷达等角度均匀分布于环状支架上，分别与标定模块相连接；每个微型固态激光雷达均安装有单目相机，形成“微型固态激光雷达—单目相机”环状阵列对，每一对摄像头视野和雷达的fov相同；其中，所述标定模块包括：相邻点云配准单元，用于对相邻的微型固态激光雷达两两进行点云拼合，得到所有相邻两个微型固态激光雷达的外参矩阵，实现其外参标定；全局点云拼合优化单元，用于拼合所有微型固态激光雷达的点云，针对每个外参矩阵设定修正矩阵并求解外参修正矩阵，优化点云误差，实现所有固态激光雷达的联合标
定，将每个微型固态激光雷达的点云整合至同一球坐标系中；雷达相机联合标定单元，分别针对每一对摄像头和微型固态激光雷达进行外参标定，确定摄像头在坐标系中的位置，完成所有摄像头和微型固态激光雷达的标定。
9.其次，本发明提出一种多维度环境信息采集平台的标定方法，包括如下步骤：步骤s1、利用二分法改进的cpd算法对相邻的微型固态激光雷达两两进行点云拼合，从而得到所有相邻两个微型固态激光雷达的外参矩阵，实现其外参标定；步骤s2、拼合所有微型固态激光雷达的点云，针对每个外参矩阵设定修正矩阵，利用牛顿-拉夫逊迭代法求解外参修正矩阵，优化点云误差，实现所有固态激光雷达的联合标定，使得每个微型固态激光雷达的点云可以整合至同一球坐标系中；步骤s3、利用梯度下降优化的pnp算法分别针对每一对摄像头和微型固态激光雷达进行外参标定，从而确定摄像头在坐标系中的位置，完成所有摄像头和微型固态激光雷达的标定。
10.作为本发明所提出的标定方法进一步的细化方案，步骤s1包括：步骤s101、根据设备的安装的具体情况，以几何方法预估相邻微型固态激光雷达点云数据重叠角度的上下界，并在此范围内，按照最大重叠角度、配准误差小于阈值的标准进行二分迭代，求解最大重叠角度；步骤s102、取重叠角度中值，截取该角度范围内的点云，并在截取的两片点云上利用cpd算法实现点集配准；步骤s103、根据二分法的原则，在配准误差小于阈值时降低重叠角度上界，在配准误差大于阈值时提高下界，当上下界差值小于预设值（如1%）时停止迭代，否则回到步骤s102继续迭代；步骤s104、以最后一次迭代的时cpd算法配准的结果为准，求取两两相邻微型固态激光雷达之间的外参矩阵。
11.作为本发明所提出的标定方法进一步的细化方案，所述步骤s2具体包括：步骤201、构建外参矩阵，优化点云拼合的误差：为每个外参矩阵设定待求的一个4
×
4的修正矩阵，修正后点云中的超过预设比例（至少95%）的点p满足，且修正矩阵的行列式值在0.95~1.05之间（含0.95和1.05），其中n表示微型固态激光雷达的个数；步骤s202、构建矩阵方程组，其中，为点云中点的个数，并将矩阵方程组拆解为线性方程组，其中为修正矩阵中各元素；步骤s203、判断点云中点的个数与微型固态激光雷达的个数之比：（1）当满足在3倍的以内时，随机选取其中个方程，采用牛顿-拉夫逊迭代法求解修正矩阵中各元素；（2）当满足大于3倍的时，构造误差函数，
用梯度下降法求解误差函数的最小值；上述步骤s203（1）、（2）所述两种迭代方法的迭代初值均为单位矩阵时的值；步骤s204、根据步骤s201至步骤s203求出的修正矩阵，构建一球坐标系，计算各微型固态激光雷达在该坐标系中的坐标。
12.作为本发明所提出的标定方法进一步的细化方案，所述步骤s3具体包括：步骤s301、对于一对微型固态激光雷达和单目相机摄像头，通过在无过度曝光的环境中，在距离采集平台3~6米位置处放置具有标定板。（标定板的特征为：形状为正方形，呈黑白棋盘格形状，部分方格被挖空，部分白色方格带有aruco图案，且该图案位于所在方格的正中心，标定板每个小方格的边长为微型固态激光雷达分辨率的10倍以上。）用已标定内参的摄像头拍摄标定板，检测aruco和棋盘格图案，从而计算出图像中标定板的棋盘格交点位置；用微型固态激光雷达扫描标定板，根据边缘和中空部分，检测标定板上各方格的顶点位置；将标定板置于不同位置，重复拍摄计算，得到多个点对；
13.步骤s302：采用梯度下降优化的pnp算法，计算微型固态激光雷达和单目相机的外参矩阵，实现联合标定；步骤s303：对完成每一对微型固态激光雷达和单目相机重复s301和s302，完成标定。
14.最后，本发明提出一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现本发明所述的标定方法的步骤。
15.本发明采用以上技术方案，与现有技术相比能够达到如下技术效果：本发明提出的环境信息采集平台，能够全方位采集人体周围环境点云信息和图像信息，克服单一前向视角的信息缺失，具有更好的效果。
16.本发明提出的针对上述采集平台的标定方法，能够快速准确的完成上述采集平台的内外参标定。
17.本发明利用固态激光雷达阵列和广角相机联合获取环境信息并加以校正，能够克服单一传感器的不足，提高检测的精确性。
附图说明
18.此处所说明的附图用来提供对本技术的进一步理解，构成本技术的一部分，本技术的示意性实施例及其说明用于解释本技术，并不构成对本技术的不当限定。在附图中：图1是多维度信息采集硬件平台示意图。其中，(a)为硬件平台传感器布局示意图，图中数字1至8为设备编号，其中1、3、5、7设备可以只安装微型固态激光雷达也可以完整安装微型固态激光雷达和单目相机，而2、4、6、8设备完整安装微型固态激光雷达和单目相机；(b)为佩戴效果示意图；(c)为单个设备中完整安装微型固态激光雷达和单目相机的局部放大示意图。
19.图2是点云重合范围示意图。
20.图3是标定板结构和背景墙示意图。
21.图4是改进pnp算法涉及坐标系示意图。
22.图5是整体流程图。
实施方式
23.为使本技术的目的、技术方案和优点更加清楚，下面将具体结合具体实施例及相应的附图对本技术技术方案进行清楚、完整地描述。显然，所描述的实施例仅是本技术一部分实施例，而不是全部的实施例。基于本技术中的实施例，本领域普通技术人员在没有做出创造劳动前提下所获得的所有其他实施例，都属于本技术保护的范围：以下结合附图，详细说明本技术各实施例提供的技术方案：本技术实施例构建了一种基于“微型固态激光雷达—单目相机”阵列的多维度环境信息采集平台，并设计其标定方法，所示平台和方法包括：步骤1：构建一贴合头部形状的环状支架，作为环境信息采集平台的设备载体；该环状支架呈圆环形状，轻质且刚性，确保附着于支架的器件不会相对移动；为适应行人出之用途，环状支架贴近人体侧有柔性材料包裹作为缓冲，同时可以通过松紧带固定于头部，使得在行走过程中支架和附着的器件不会相对头部滑动。
24.将微型固态激光雷达和单目相机一上一下间隔约1cm水平固定，构成“微型固态激光雷达—单目相机”设备对，再将8个设备对以45
°
角的间隔均匀分布于环状支架上，且各设备对的中心应当处于同一水平面。所有设备对选用的单目相机和微型固态激光雷达相同，且微型固态激光雷达的水平fov至少90度，竖直fov至少60度。将8个设备对顺时针依次编号为1~8，在单目相机视野足够开阔时（如畸变矫正后的水平视角大于130度），则其中1、3、5、7设备对可以只安装微型固态激光雷达，以节约成本；在单目相机视野不够开阔时（如畸变矫正后的水平视角小于130度），应当完整安装微型固态激光雷达和单目相机。2、4、6、8设备对总是完整安装微型固态激光雷达和单目相机。该实施例可以获得全方位的环绕视场点云数据和图像数据，硬件结构如如图1所示。
25.步骤2：将采集平台置于室内，在距离采集平台约5米处放置若干物体，围成一圈。物体应当落在微型固态激光雷达的垂直fov内。用8个微型固态激光雷达扫描一次，各得到一片点云。接下来，对相邻的微型固态激光雷达两两进行点云拼合。分析相邻微型固态激光雷达点云数据的重叠情况，对于两个水平fov为90度的相邻激光雷达，其重叠范围应当小于45度，如图2所示。当检测到的障碍物充分远（可以认为是大于10倍）时，其理想重叠范围应为45度。截取其重叠范围的点云，并在重叠点云上利用二分法改进的cpd算法实现点集配准。所谓改进的cpd算法，即在标准cpd算法前进行预处理，并适当调整其迭代步骤，具体流程如下：子步骤1：设置二分法边界l=35
°
，r=45
°
和迭代误差阈值t。该边界应当根据实际情况确定，例如，当雷达个数不为8，或者水平fov不是90
°
时，r调整为对应的理想重叠角度；再如，当标定场地较小，场景中物体距离采集平台较近，点云实际重叠范围较小，则l应当设置的更小。
26.子步骤2：取二分中值m=(l+r)/2，裁剪相邻m
°
点云，将其中一片点云以合适方向旋转90
°
。所谓合适方向和旋转角度也需要根据雷达个数和水平fov确定。
27.子步骤3：采用标准cpd算法，得到点云配准情况，记录配准误差tm。
28.子步骤4：如果tm》t，r置为m，否则l置为m。当|r-l|足够小时，停止迭代，取最后一步外参矩阵作为相邻两个激光雷达的外参矩阵；否则回到子步骤2继续迭代。
29.本实施例的合理性在于：
（1）对于一般的激光slam方法的定位，点云配准常用当前检测到的点云在点云地图中进行匹配，通常点云地图远大于点云；而本发明在标定过程中进行点云配准，多个微型固态激光雷达同时扫描同一场景，且重合视角较大。而具体重合视角的范围与标定物品的摆放有一定关系。如果所取重叠点云范围太小，则点云中点的数量较少，可能引起误差；而若所取重叠点云范围太大，则点云不能完全重合，配准误差大。因此通过二分法，在缩小配准误差至可接受范围的前提下，尽可能取范围较大的点云。
30.（2）由于重叠部分点云本身是高度相似的，对点云进行预先旋转可以处于更好的迭代初值，在保证精度的前提下，减少迭代求解次数，从而提高实际运行cpd算法时的速度。
31.重复上述步骤8次，可以得到两两相邻微型固态激光雷达之间的外参矩阵。
32.步骤3：拼合所有微型固态激光雷达的点云，并通过回环检测优化点云误差，实现所有固态激光雷达的联合标定，使得每个微型固态激光雷达的点云可以整合至同一球坐标系中。具体采用如下修正算法：假定在前述步骤中，实施例针对1-2，2-3，
…
，7-8，8-1两两标定了外参矩阵，分别记为,在理想情况下，应当有
ꢀꢀ
（1）其中，e表示单位矩阵。
33.式(1)表明，即1号设备对上的雷达检测到的某一点旋转一圈后回到原来的位置。如果顺次拼合1-2，2-3，
…
，7-8的点云，显然8-1两个设备对的点云应当部分重叠，但由于拼合顺序的原因，误差累计导致点云重叠误差较大。
34.设在实际情况下，
ꢀꢀ
（2）
35.则
ꢀꢀ
（3）接着，需要对各矩阵进行修正，使得
ꢀꢀ
（4）其中，是修正后的矩阵。
36.可以将式(4)的条件近似表示成：只要满足修正矩阵满足对于任意，式（5）恒成立：
ꢀꢀ
（5）其中为激光雷达有效检测距离，表示某一点到激光雷达原点的值，为略大于0的某一较小值。
37.8个修正矩阵共计8*16=128个变量，每取一个点p可以产生4个方程，因此需要选取128/4=32个点来求解式（5），即
ꢀꢀ
（6）其中，为修正矩阵中各元素。
38.当为定值时，考虑采用牛顿-拉夫逊法的进行迭代求解，各修正矩阵的初值设置为单位矩阵，求解过程中如某次迭代越界，则本次不做更新，沿用上述值，迭代将在次数过多或误差足够小的情况下停止。如果迭代次数过多，求解失败，则放大重试，直至超过设定的上界。通过此方法可以相对平均的将误差分配在8个设备对之间。
39.由于点云中点数很多，为充分利用这些点，也可以构造误差函数
ꢀꢀ
（7）用梯度下降法求解式(7)的最小值。迭代初值亦为单位矩阵时的值。
40.上述“修正算法”可以归纳为如下步骤：子步骤1：取32个点（牛顿-拉夫逊法）或所有点（梯度下降法），构造方程组，设置步长；子步骤2：设置修正矩阵的初值为单位矩阵；子步骤3：进行牛顿-拉夫逊迭代求解，或者构造误差函数使用梯度下降法求解；子步骤4：如果求解成功，则结束；否则，回到步骤2。
41.步骤4：分别针对每一对摄像头和微型固态激光雷达进行外参标定，从而确定摄像头在步骤3所述的坐标系中的位置，从而完成所有摄像头和微型固态激光雷达的标定；为了完成该部分的标定，本实施例采用特定形状的标定板。参考图3所示，以斜条纹部分为背景墙，不构成标定板的一部分；标定板形状为正方形，呈棋盘格形状，点阵背景图案的格子和无背景图案的格子并非实际图样，仅用于表明这两种格子应当填充不同的图案；标定板中部分方格被挖空（即标定板中间的斜条纹部分），从斜条纹部分的位置可以透过标定板看到背景墙；部分方格带有aruco/apriltag图案，且该图案位于所在方格的正中心。为了减小误差，标定板每个小方格的边长应当在微型固态激光雷达分辨率的10倍以上。使用时，将标定板置于背景墙前约1至2米的位置。
42.用微型固态激光雷达扫描标定板，通过中空部分和边界，分析标定板的方格顶点；再用单目相机拍摄标定板，通过棋盘格图案和aruco/apriltag图案，识别标定板的方格顶点。例如，标定板上的方格数为7
×
7，中空格数格，则可以产生至少40对标志点对（每个中空格4个点对，标志板自身四个角也产生4个点对，即4
×
9+4=40个对点对），再将标定板置于不同位置，重复8至10次上述单目相机拍摄标定板的步骤，可以得到320~400个点对。当方格数较多（例如21
×
21，含中空格），重复拍摄次数也较多（例如达到25~30次），则产生的点对也较多，可以超过10000对点对。
43.对上述点对应用改进的pnp算法，可以得到相机和雷达的外参矩阵，其原理如下：pnp算法采用直接线性变换（dlt）计算外参矩阵，在这种模式下，一对点产生两个线性方程，为计算外参矩阵需要求12个未知数，因此需要6对点来求解。通常情况下，标定不只产生6对点，则使用最小二乘法拟合最优解。所涉及坐标系如图4所示。
44.本实施例所用标定板每拍摄一帧即会产生数十以至于数百个标定点，拍摄标定图片可能会有几十至几百帧，在这种情况下，标定点对的数量可能很大（例如超过10000对点
对），使用最小二乘法拟合时在求解逆矩阵的过程中可能效率较低，因此采用梯度下降法最小化误差函数以求解外参矩阵，可以实现效率的提升。
45.本实施例采用特定的电子设备存储和处理执行上述步骤的程序，所用设备和程序也应一并受到保护。本实施例的整体流程如图5所示。
46.上述仅为本发明的优选实施方法，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。本实施例中未明确的各组成部分均可用现有技术加以实现。

技术特征：
1.一种多维度环境信息采集平台，其特征在于：包括贴合人体头部形状的环状支架、n个微型固态激光雷达，以及标定模块，所述n个微型固态激光雷达等角度均匀分布于环状支架上，分别与标定模块相连接；每个微型固态激光雷达均安装有单目相机，形成“微型固态激光雷达—单目相机”环状阵列对；其中，所述标定模块包括：相邻点云配准单元，用于对相邻的微型固态激光雷达两两进行点云拼合，得到所有相邻两个微型固态激光雷达的外参矩阵，实现其外参标定；全局点云拼合优化单元，用于拼合所有微型固态激光雷达的点云，针对每个外参矩阵设定修正矩阵并求解外参修正矩阵，优化点云误差，实现所有固态激光雷达的联合标定，将每个微型固态激光雷达的点云整合至同一球坐标系中；雷达相机联合标定单元，分别针对每一对摄像头和微型固态激光雷达进行外参标定，确定摄像头在坐标系中的位置，完成所有摄像头和微型固态激光雷达的标定。2.根据权利要求1所述的多维度环境信息采集平台，其特征在于，n的取值范围是3~10。3.根据权利要求1所述的多维度环境信息采集平台，其特征在于，环状阵列对中，每一设备对的摄像头视野和雷达的fov相同。4.根据权利要求1所述的多维度环境信息采集平台，其特征在于，环状支架贴近人体侧设置有海绵材料包裹作为缓冲。5.一种基于权利要求1所述的多维度环境信息采集平台的标定方法，其特征在于，包括如下步骤：步骤s1、利用二分法改进的cpd算法对相邻的微型固态激光雷达两两进行点云拼合，从而得到所有相邻两个微型固态激光雷达的外参矩阵，实现其外参标定；步骤s2、拼合所有微型固态激光雷达的点云，针对每个外参矩阵设定修正矩阵，利用牛顿-拉夫逊迭代法求解外参修正矩阵，优化点云误差，实现所有固态激光雷达的联合标定，使得每个微型固态激光雷达的点云可以整合至同一球坐标系中；步骤s3、利用梯度下降优化的pnp算法分别针对每一对摄像头和微型固态激光雷达进行外参标定，从而确定摄像头在坐标系中的位置，完成所有摄像头和微型固态激光雷达的标定。6.根据权利要求5所述的标定方法，其特征在于，步骤s1包括：步骤s101、根据设备的安装的具体情况，以几何方法预估相邻微型固态激光雷达点云数据重叠角度的上下界，并在此范围内，按照最大重叠角度、配准误差小于阈值的标准进行二分迭代，求解最大重叠角度；步骤s102、取重叠角度中值，截取该角度范围内的点云，并在截取的两片点云上利用cpd算法实现点集配准；步骤s103、根据二分法的原则，在配准误差小于阈值时降低重叠角度上界，在配准误差大于阈值时提高下界，当上下界差值小于预设值时停止迭代，否则回到步骤s102继续迭代；步骤s104、以最后一次迭代的时cpd算法配准的结果为准，求取两两相邻微型固态激光雷达之间的外参矩阵。7.根据权利要求5所述的标定方法，其特征在于，所述步骤s2具体包括：步骤201、构建外参矩阵，优化点云拼合的误差：为每个外参矩阵设定待求的一个
4
×
4的修正矩阵，修正后点云中的超过预设比例的点p满足，且修正矩阵的行列式值在0.95~1.05之间，其中n表示微型固态激光雷达的个数；步骤s202、构建矩阵方程组，其中，为点云中点的个数，并将矩阵方程组拆解为线性方程组，其中为修正矩阵中各元素；步骤s203、判断点云中点的个数与微型固态激光雷达的个数之比：（1）当满足在3倍的以内时，随机选取其中个方程，采用牛顿-拉夫逊迭代法求解修正矩阵中各元素；（2）当满足大于3倍的时，构造误差函数，用梯度下降法求解误差函数的最小值；上述（1）、（2）两种迭代方法的迭代初值均为单位矩阵时的值；步骤s204、根据步骤s201至步骤s203求出的修正矩阵，构建一球坐标系，计算各微型固态激光雷达在该坐标系中的坐标。8.根据权利要求5所述的标定方法，其特征在于，所述步骤s3具体包括：步骤s301、对于一对微型固态激光雷达和单目相机摄像头，通过在无过度曝光的环境中，在距离采集平台一定范围位置处放置标定板支架，用已标定内参的摄像头拍摄标定板，检测aruco和棋盘格图案，从而计算出图像中标定板的棋盘格交点位置；用微型固态激光雷达扫描标定板，根据边缘和中空部分，检测标定板上各方格的顶点位置；将标定板置于不同位置，重复拍摄计算，得到多个点对；步骤s302：采用梯度下降优化的pnp算法，计算微型固态激光雷达和单目相机的外参矩阵，实现联合标定；步骤s303：对完成每一对微型固态激光雷达和单目相机重复s301和s302，完成标定。9.根据权利要求8所述的标定方法，其特征在于，所述标定板的特征包括：标定板形状为正方形，呈黑白棋盘格形状，部分方格被挖空，部分白色方格带有aruco图案，且该图案位于所在方格的正中心，标定板每个小方格的边长为微型固态激光雷达分辨率的10倍以上。10.一种电子设备，包括使用的传感器、存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如权利要求5-9任一项所述的标定方法的步骤。

技术总结
本发明公开了一种多维度环境信息采集平台及其标定方法，采集平台由多个“微型固态激光雷达—单目相机”设备对构成，均匀安装于可穿戴的环状支架上。为标定上述采集平台中各设备参数，通过改进的CPD点集配准算法拼合点云数据，并通过求解修正矩阵降低拼合误差，标定各微型固态激光雷达之间的外参，再通过标定板和改进的PnP算法，标定每一对微型固态激光雷达和单目相机之间外参，最终完成所有设备之间的标定。本发明能够较为准确的获取全方位点云和视觉信息，提高了环状设备阵列的标定准确度。度。度。


技术研发人员：方铭宇 钱伟行 朱煜彬 赵杰 陆晅 张浩诚 包琪玮 沈皓轩 蔡洁萱
受保护的技术使用者：南京师范大学
技术研发日：2023.06.15
技术公布日：2023/7/22