1.本发明属于自动驾驶领域，具体涉及一种三维感知传感器及其面向建筑物的主动重建方法。
背景技术：2.未知户外场景的探索与主动重建是大多数户外移动机器人的基础技术。户外清洁、灾后救援和国防军事等场景，对于未知环境的探索有很高的时效性或准确性的需求。以自动驾驶技术举例，目前的自动驾驶技术的定位几乎完全依赖于预先建立的高精度地图信息。高精度地图即是利用传感器对未知环境进行探索并采集关键信息，然后通过人工标注、提取高级特征获得。因此，目前的自动驾驶技术在面对未知环境时，其系统的稳定性与鲁棒性会受到较大的挑战，当未知环境中的某些参与物发生了变化，而高精度地图没有对该参与物进行重新采集，会使高精度地图的信息出现误差，进而影响自动驾驶或移动机器人的导航。针对上述问题，学界致力于研究与提升移动机器人在场景未知的户外场景下的主动探索与重建能力，继而实现移动机器人能够主动地探索环境，并探索环境中的未知区域，标记不可探测区域与可行驶区域。
3.发明专利《智能体主动构建环境场景图谱的方法、设备和探索方法》(公开号：cn113111192a)提供了智能体基于视觉信息主动构建环境场景图谱方法、环境探索方法和智能设备。采用环境场景图像及对应的环境场景图谱数据集和采集的智能体探索环境路径训练主动探索模型；基于训练好的主动探索模型生成动作，智能体采用所生成的动作探索环境，探索过程中，获得3d语义点云数据，然后利用3d语义点云数据构建环境场景图谱。
4.发明专利《机器人自主探索建图的方法、设备及存储介质》(公开号：cn106154285a)提供了一种机器人自主探索建图的方法、设备及存储介质，所述方法包括：获取传感器采集的环境数据，利用同步定位与地图构建slam算法，在当前估计的slam初始环境地图上识别所要访问的区域；针对识别出的访问区域，基于主动探索方式进行路径规划，并按照效用最高原则，从已规划的路径中选出探索路径，执行路径探索操作；根据路径探索结果，建立自主探索对应的环境地图；达到了通过机器人的自主探索选择移动路径并建立环境地图的目的，降低了单纯利用slam算法所导致的定位的不确定性，提高了环境地图的构建质量和构建效率，也进一步提高了机器人的自主性。
5.上述专利提到的方法均是面向完整封闭环境的主动探索系统，没有涉及到仅对户外特定建筑物的主动重建。此外，没有考虑通过主动添加点云配准约束的方式来提升整个定位系统的精度。
技术实现要素：6.本发明的目的之一在于提供一种三维感知传感器，通过设计二轴云台来提供激光雷达的俯仰角与偏航角，从而满足主动重建的任务。
7.为实现上述发明目的，本发明采取的技术方案如下：
8.一种三维感知传感器，包括：
9.云台底盘，所述云台底盘上安装转动件，所述转动件可相对所述云台底盘转动；
10.云台支架，所述云台支架安装在所述转动件上端面，以使所述转动件带动所述传感器支架转动，所述云台支架上设置连杆升降机构；
11.驱动电机，所述驱动电机包括第一电机和第二电机，所述第一电机安装在所述云台底盘上用于驱动所述转动件转动，所述第二电机安装在所述云台支架上，所述第二电机用于驱动所述连杆升降机构运动，以使所述连杆升降机构远离所述云台底盘的一端做升降运动；
12.激光雷达，所述激光雷达通过安装板与所述连杆升降机构的上端处的转动块连接，以使所述连杆升降机构驱动所述安装板上的激光雷达做俯仰运动；
13.陀螺仪，所述陀螺仪设置在所述激光雷达的一侧，以使所述陀螺仪与所述激光雷达一起运动；
14.所述第一电机驱动所述云台支架转动以使所述激光雷达具有偏航角的自由度，所述第二电机驱动所述连杆升降机构上端处的转动块转动以使所述激光雷达具有俯仰角的自由度。
15.优选的，所述连杆升降机构包括与所述第二电机的输出轴连接的转动杆和与所述转动杆连接的升降杆，所述升降杆竖向设置，所述转动杆的一端与所述第二电机的输出轴连接，所述转动杆的另一端与所述升降杆的一端连接，所述升降杆的另一端与所述转动块连接，所述转动块与所述云台支架转动连接，以使所述升降杆驱动所述转动块转动，所述安装板安装在所述转动块上，以使所述转动块转动时所述安装板做俯仰运动。
16.优选的，所述激光雷达是livox horizon激光雷达，所述驱动电机为无刷电机，通过第一电机的码盘给出所述激光雷达偏航角的角度，通过所述陀螺仪给出所述激光雷达俯仰角的角度。这样设置，可以使第一电机和第二电机通过正反转控制激光雷达分别做偏航运动和俯仰运动，以分别调节激光雷达的偏航角和俯仰角。同时livox horizon激光雷达相对于传统的机械旋转式激光雷达而言，其扫描图案为非重复式的扫描，livox horizon激光雷达的每帧点云的扫描区域都不同，使得当激光雷达静止放置时，其视场角内的观测的覆盖率会逐步提高，这一特性尤为符合主动重建的任务目标，如果采用传统的机械旋转式激光雷达，则需要在激光雷达扫描某个区域时，主动将激光雷达进行往复摆动，因此带来了因为机构摆动而造成的运动不确定性以及点云的运动畸变，并且需要观测的建筑物部分的覆盖率也不能够保证；而采用livox horizon激光雷达，则只需要在观测的位置固定好三维感知传感器，通过云台支架上的第一电机和第二电机调节好激光雷达的偏航角和俯仰角，然后使激光雷达处于固定状态，随着积分时间增大，点云视场覆盖率会持续增大，直至视场覆盖率接近100％，然后将多帧点云配准就可以获得较好并且稠密的观测结果。
17.本发明的目的之二是提供一种面向建筑物的主动重建方法，从而实现户外场景下建筑物的主动探索与重建技术的视角与路径规划，并能使机器人有效执行基于移动机器人的户外建筑物主动重建，包括如下步骤：
18.步骤s1.通过机器人的激光雷达对建筑物进行扫描，并将扫描区域中获取的每帧点云进行坐标转换，将每帧点云用于更新建筑物的地图，以完成重建建筑物的地图，建筑物重建的地图为栅格地图；
19.步骤s2.对于步骤s1建筑物重建的地图，采用点云更新栅格地图中建筑物的占据栅格和占据栅格特定半径范围内的非占据栅格；
20.步骤s3.每当步骤s2中的每一帧点云被用于更新栅格地图的占据栅格和非占据栅格后，计算该点云的轴对齐包围框，在轴对齐包围框内提取在空间上与建筑物的占据栅格毗邻，并且与未知栅格毗邻的栅格作为边境栅格，并且删除任何与轴对齐包围框有交集的边境栅格聚类，以完成对边境栅格的提取；
21.步骤s4.在步骤s3提取得到边境栅格后，利用区域增长算法对边境栅格进行聚类，使得空间上相连的边境栅格组成边境栅格聚类，通过设置阈值对边境栅格聚类进行筛选，筛选出符合设定阈值范围内的边境栅格聚类；
22.步骤s5.通过步骤s4提取边境栅格并将其根据空间位置关系进行聚类，设边境栅格聚类的集合为c
frt
，机器人对边境栅格聚类进行观测，直到环境中没有边境栅格聚类或者所有边境栅格聚类都不能在机器人的可行驶区域内被看见；对于每一个边境栅格聚类c
frt
，通过在机器人可行驶区域内对边境栅格聚类进行视角采样，从而获得其对应的视角集合vpi；
23.步骤s6.在得到边境栅格聚类的视角集合vpi后，根据边境栅格聚类所能观测到的信息以及边境栅格聚类到当前机器人的位置给视角打分，以获得当前环境下每个边境栅格聚类的最佳观测视角，进而得到最高得分视角集合；
24.步骤s7.在得到最高得分视角集合后，进行旅行商问题求解，求出下一个最佳视角，根据下一个最佳视角完成机器人的路径规划与导航；
25.步骤s8.在完成步骤s7的路径规划后，机器人的主动探索与路径规划模式为规划-执行-观测循环，机器人执行规划好的路径，且在行走过程中，通过定位模块中的激光雷达对建筑物进行扫描，并将建筑物的占据栅格按照一定半径进行膨胀，获取膨胀后的位于占据栅格周围的自由栅格，以补全自由栅格，完成地图更新，同时完成步骤s5的视角采样，以使机器人通过路径规划完成对建筑物的主动探索与地图重建；
26.步骤s1中的所述定位模块包括三维感知传感器和惯性测量单元imu，所述三维感知传感器包括激光雷达和用于调整激光雷达偏航角和俯仰角的驱动电机。
27.优选的，所述步骤s1中所述建筑物重建的地图为八叉树栅格地图，所述步骤s2中的占据栅格为三维占据栅格。
28.优选的，所述步骤s2中用点云更新占据栅格时采用的方法如下：对于机器人采集的当前点云的每一个三维点，计算其到所述三维感知传感器中心的连线所经过的三维栅格，并且增加这些栅格的非占据概率，形成非占据栅格；随后对该三维点所在的栅格，增加其占据的概率，从而完成了一个三维点的地图更新，对于建筑物重建的地图，只更新占据栅格附近的非占据栅格，而保留其他栅格的未知状态。
29.优选的，所述步骤s2还包括占据栅格设定半径内的自由栅格补全，所述自由栅格即为非占据栅格，自由栅格补全方法包括如下步骤：
30.步骤a1.对占据栅格按照设定半径进行膨胀，将膨胀出来的新的栅格设为栅格集合v
dl
；
31.步骤a2.遍历栅格集合v
dl
并找出三维感知传感器视场角之外的栅格集合，标记为v
iv
；
32.步骤a3.对经过步骤a2筛选剩下的栅格进行从三维感知传感器位置出发的光线投射，标记出被建筑物所遮挡的栅格集合，记为v
ocl
；
33.步骤a4.通过删除经过步骤a2和a3标记出来的栅格结合，最终保留的被补全的自由栅格集合表示为：
[0034]vfree
＝{v|v∈v
dl
\(v
iv
∪v_ocl)}。
[0035]
优选的，在所述步骤s4中，当边境栅格聚类的数量小于设定的最小阈值时，则忽略该边境栅格聚类，该最小阈值设置为5，因为数量过小的聚类往往是因为传感器噪声导致，因此本发明将这些聚类忽略；当边境栅格的尺寸超过设定的最大阈值后，该最大阈值设置为2m/栅格的分辨率，对边境栅格进行主成分分析，算出边境栅格形状的长轴的方向，并且在长轴方向上将边境栅格递归地进行拆分，该过程是递归进行的，直到所有的边境栅格聚类都小于设定的最大阈值。通过边境栅格聚类的筛选以及对大尺寸边境栅格进行递归以实现递进地检测与更新边境栅格，从而减少计算量。
[0036]
优选的，在所述步骤s3中，为了使边境栅格只在建筑物的占据栅格附近生成，以使机器人专注于探索建筑物的边界，需要判别某个栅格是否属于目标建筑物，判别栅格是否属于目标建筑物的方法如下：利用并查集算法，给每个栅格添加一个根节0点的信息，在机器人初始化启动时，对其初始点云构建出来的栅格进行归并，并将拥有最多子节点的根节点记为vbd，在之后每次向占据栅格所在的地图中插入点云时，都会将新生成的占据栅格与其邻近栅格进行归并，并更新其根节点的值，通过对占据栅格进行归并后，在空间上与vbd相连的栅格将拥有相同的根节点，而空间上不与vbd相连的栅格则不会拥有相同的根节点，进而通过判断当前占据栅格是否与vbd有相同的根节点，即可判断当前的占据栅格是否是建筑物的一部分，当确定目标建筑物的占据栅格后，即可继续完成对边境栅格的提取。
[0037]
优选的，在所述步骤s5中，以某个边境栅格ci的聚类的中心点为圆心，采取固定的半径步长r
step
与角度步长θstep，在机器人的可行驶区域进行采样，所得到视角集合即为vpi。
[0038]
优选的，在所述步骤s6中，通过覆盖率来评价一个视角能真正地观测到其对应的栅格聚类的程度，一个视角的覆盖率是这个视角下所能看见的对应的边境栅格的数量占原边境栅格数量的比例，基于此，通过视角打分函数选择出的最佳视角为的计算公式为：
[0039][0040]
其中为视角集合中覆盖率最大的视角，f
dist
表示边境栅格聚类中心点到机器人当前位置点之间的距离，d
max
为设置的距离阈值，m为地图坐标系，r为机器人坐标系，p机器人的位置。
[0041]
优选的，在所述步骤s8中，在机器人行走过程中，针对主动重建任务的特性，通过构建位姿因子图的方式，利用当前观测的建筑物点云与已重建的地图做回环检测与修正机器人位姿，利用机器人当前观测点云来修正机器人的位姿的方法如下：
[0042]
步骤b1.获得当前机器人的观测点云，因为livox horizon激光雷达的非重复式扫描的特性，使得在对建筑物进行观测时，单帧点云只能够扫描到雷达视场角内的部分建筑物表面，因此需要在时间上构建一个当前观测的点云子图，使得扫描到的点云尽可能稠密，采用的点云子图形式为哈希栅格地图，哈希栅格地图具体为：对于要插入地图的每个点，根据地图的分辨率获得其三维栅格的索引，随后在哈希表里查找该栅格索引对应的栅格；
[0043]
步骤b2.当机器人运动到下一个视角的半径范围之内时，利用当前观测的点云构建点云子图，当机器人执行到该视角的位置处时，会将当前点云子图与已重建地图进行配准，并将配准的结果作为一元边加入到因子图中，步骤b1中哈希栅格地图对应的哈希子图构建时采用双向配准的策略，首先将激光雷达采集的点云根据当前惯性里程计imu的信息投影到世界坐标系中，得到点云，若当前的哈希子图的点云帧数或者三维点数量小于设定的阈值，则直接向哈希子图中插入世界坐标系中的点云，设以此法插入的点云帧数为k；否则将哈希子图中的所有点云配准到新的点云的坐标系下，并与新获取的点云一同重新加入到哈希子图中；最后，更新哈希子图的位姿信息；当一个子图构建完成后，将前k帧点云从哈希栅格地图中抽取出来，重新与剩下的点云进行配准，以保证点云的一致性。在该算法中，将哈希子图的点云重新配准到新来的点云的坐标系下，是为了构建因子图优化时，对后续里程计更新时能够直接利用最新的位姿。
[0044]
优选的，位姿估计器是fast-lio2，在一些较为空旷的户外场景中，容易因为场景退化的问题导致位姿估计不够准确。而fast-lio2是没有回环检测的，因此当位姿估计失败时，该估计器没有任何策略来将其纠正，因此采用通过构建位姿因子图的方式，利用当前观测的建筑物点云与已构建的地图做回环检测与修正，使得机器人的定位系统更加鲁棒，构建出来的地图更加精确。
[0045]
优选的，通过gtsam算法构建因子图，从而构建优化问题并更新位姿，因子图的构建算法是与步骤s7的机器人的路径规划与导航方法相耦合，以生成机器人的定位系统，耦合步骤如下：
[0046]
步骤c1.当机器人从当前视角行驶到下一个视角后，连接这两个视角的位姿变量的二元因子就被加入到因子图中，该二元因子用来约束两个位姿变量之间的相对变换关系，这两个视角的惯性里程计位姿由位姿估计器给出，分别记为则该二元因子的相对位姿为：
[0047][0048]
步骤c2.当机器人行驶到下一个视角处后，将进行当前点云子图的点云对建筑物地图的配准，并将配准结果为作为一元观测结果即上述一元边加入到因子图中；
[0049]
步骤c3.若两个视角之间的欧氏距离与偏航角小于设置的阈值，则会将这两个点云子图单独进行配准，点云子图在构建时已经将观测到的点云转换到世界坐标系下，由于位姿估计器误差的存在，导致构建第一个子图时的世界坐标系与构建第二个子图时的世界坐标系不完全一致，根据此不一致，可以得到其约束为：
[0050]
[0051]
其中，表示视角vp1的地图坐标系下的三维变换的逆；表示视角vp2所在的地图坐标系到视角vp1所在的地图坐标系的变换；表示视角vp2的地图坐标系下的三维变换；
[0052]
步骤c4.当机器人按照路径规划行走，且机器人准备执行到下一个视角之前，会对当前所有的视角的位姿进行更新，并相应地调整点云子图与全局地图的点云与栅格。
[0053]
有益效果：
[0054]
本发明针对户外场景的建筑物的主动重建问题，以地面移动机器人的运动学特性和应用场景为导向，提出并建立了一种新颖的基于移动机器人的面向建筑物的主动重建系统，从而实现了户外场景下建筑物的主动探索与重建技术的视角与路径规划，在此基础上，又进一步研究并提出了一种基于因子图优化的激光雷达-惯性里程计的定位性能提升方案，通过本发明的方案，机器人能够有效执行基于移动机器人的户外建筑物主动重建。
附图说明
[0055]
图1所示为本发明三维感知传感器的结构图；
[0056]
图2所示为本发明三维感知传感器中部分结构爆炸图；
[0057]
图3所示为本发明三维感知传感器的主视图；
[0058]
图4所示为图3沿a-a向的剖视图；
[0059]
图5所示为本发明面向建筑物的主动重建方法的视角与路径规划框架图；
[0060]
图6所示为本发明面向建筑物的主动重建方法中建筑物重建的八叉树地图的新旧两种更新方式；
[0061]
图7所示为本发明面向建筑物的主动重建方法中自由栅格的补全策略图；
[0062]
图8所示为本发明面向建筑物的主动重建方法中主动重建的并查集算法图；
[0063]
图9所示为本发明面向建筑物的主动重建方法中视角采样演示图；
[0064]
图10所示为本发明面向建筑物的主动重建方法中点云子图构建示意图；
[0065]
图11所示为本发明面向建筑物的主动重建方法中因子图构建示意图。
[0066]
附图标记
[0067]
1、云台底盘；11、交叉滚子轴承座；12、交叉滚子轴承；13、轴承内圈垫圈；2、云台支架；21、支架基板；22、支架侧板；3、第二电机；4、第一电机；41、连接板；42、转动板；5、激光雷达；6、陀螺仪；7、安装板；8、连杆升降机构；81、转动杆；82、升降杆；9、安装部；10、转动块。
具体实施方式
[0068]
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对照附图说明本发明的具体实施方式。显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图，并获得其他的实施方式。
[0069]
下面以具体实施例详细介绍本发明的技术方案。
[0070]
实施例1
[0071]
参考图1，一种三维感知传感器，包括：
[0072]
云台底盘1，所述云台底盘1上安装转动件，所述转动件可相对所述云台底盘1转动；
[0073]
云台支架2，所述云台支架2安装在所述转动件上端面，以使所述转动件带动所述传感器支架转动，所述云台支架2上设置连杆升降机构8；
[0074]
驱动电机，所述驱动电机包括第一电机4和第二电机3，所述第一电机4安装在所述云台底盘1上用于驱动所述转动件转动，所述第二电机3安装在所述云台支架2上，所述第二电机3用于驱动所述连杆升降机构8运动，以使所述连杆升降机构8远离所述云台底盘1的一端做升降运动；
[0075]
激光雷达5，所述激光雷达5通过安装板7与所述连杆升降机构8的上端处的转动块10连接，以使所述连杆升降机构8驱动所述安装板7上的激光雷达5做俯仰运动；
[0076]
陀螺仪6，所述陀螺仪6设置在所述激光雷达5的一侧，以使所述陀螺仪6与所述激光雷达5一起运动；
[0077]
所述第一电机4驱动所述云台支架2转动以使所述激光雷达5具有偏航角的自由度，所述第二电机3驱动所述连杆升降机构8上端处的转动块10转动以使所述激光雷达5具有俯仰角的自由度。
[0078]
参考图2和4所示，所述云台支架2包括支架基板21和设置在所述支架基板21上的支架侧板22，所述连杆升降机构8设置在所述支架侧板22上，所述安装板7设置在所述连杆升降机构8上端处的转动块10连接，所述所述转动件包括连接板41和转动板42，所述云台底盘1上设置用于容纳轴承的轴承座，支架基板21与轴承的内圈抵接，轴承的外圈与轴承座抵接，所述轴承可优选为交叉滚子轴承12，所述轴承座为交叉滚子轴承座11，所述第一电机4的输出轴的输出端安装连接板41，且在所述连接板41背离第一电机4的一侧设置转动板42，所述转动板42与所述连接板41通过螺栓固定，所述第一电机4驱动所述连接板41及与连接板41固定的转动板42转动，所述转动板42与所述支架基板21通过螺栓连接，以使所述支架基本与转动板42同时转动，同时在所述转动板42与所述支架基板21之间设置轴承内圈垫圈13，且所述轴承内圈的下端面与所述转动板42的上端面紧贴设置，设置可以对轴承起到保护作用，当所述第一电机4的输出轴转动时，会带动转动板42和支架基板21一起转动，从而使云台支架2上的激光雷达5的偏航角得到调节。
[0079]
参考图3所示，所述连杆升降机构8包括与所述第二电机3的输出轴连接的转动杆81和与所述转动杆81连接的升降杆82，所述升降杆82竖向设置，所述转动杆81的一端与所述第二电机3的输出轴连接，所述转动杆81的另一端与所述升降杆82的一端连接，所述升降杆82的另一端与所述转动块10连接，所述转动块10与所述云台支架2转动连接，以使所述升降杆82驱动所述转动块10转动，所述安装板7安装在所述转动块10上，以使所述转动块10转动时所述安装板7做俯仰运动。
[0080]
所述云台支架2的两侧设置用于安装转动块10的安装部9，所述安装部9固定设置在云台支架2上，所述安装部9的上端与转动块10的一端通过销轴连接，以使所述转动块10能够在安装部9上转动，所述转动块10的另一端与升降杆82的上端连接，当第二电机3驱动转动杆81做顺时针和逆时针转动时，所述升降杆82在转动杆81的带动下做升降往复运动，同时所述转动块10在所述升降杆82的带动下开始转动，从而带动与转动块10连接的安装板7做俯仰运动，由于在云台支架2的两侧均设置与安装部9连接的转动块10，从而使得安装板
7与云台支架2两侧的转动块10连接，进而确保安装板7在做俯仰运动时保证平稳，不会出现翻滚，从而保证了安装板7上的激光雷达5在第二电机3的作用下做稳定俯仰运动，同时陀螺仪6也安装在安装板7上，保证了陀螺仪6与激光雷达5同步运动。
[0081]
所述激光雷达5是livox horizon激光雷达5，所述驱动电机为无刷电机，通过第一电机4的码盘给出所述激光雷达5偏航角的角度，通过所述陀螺仪6给出所述激光雷达5俯仰角的角度。这样设置，可以使第一电机4和第二电机3通过正反转控制激光雷达5分别做偏航运动和俯仰运动，以分别调节激光雷达5的偏航角和俯仰角。同时livox horizon激光雷达5相对于传统的机械旋转式激光雷达5而言，其扫描图案为非重复式的扫描，livox horizon激光雷达5的每帧点云的扫描区域都不同，使得当激光雷达5静止放置时，其视场角内的观测的覆盖率会逐步提高，这一特性尤为符合主动重建的任务目标，如果采用传统的机械旋转式激光雷达5，则需要在激光雷达5扫描某个区域时，主动将激光雷达5进行往复摆动，因此带来了因为机构摆动而造成的运动不确定性以及点云的运动畸变，并且需要观测的建筑物部分的覆盖率也不能够保证；而采用livox horizon激光雷达5，则只需要在观测的位置固定好三维感知传感器，通过云台支架2上的第一电机4和第二电机3调节好激光雷达5的偏航角和俯仰角，然后使激光雷达5处于固定状态，随着积分时间增大，点云视场覆盖率会持续增大，直至视场覆盖率接近100％，然后将多帧点云配准就可以获得较好并且稠密的观测结果。
[0082]
实施例2
[0083]
如图5所示，一种面向建筑物的主动重建方法，从而实现户外场景下建筑物的主动探索与重建技术的视角与路径规划，并能使机器人有效执行基于移动机器人的户外建筑物主动重建，包括如下步骤：
[0084]
步骤s1.通过机器人的定位模块中的激光雷达对建筑物进行扫描，并将扫描区域中获取的每帧点云进行坐标转换，将每帧点云用于更新建筑物的地图，以完成重建建筑物的地图，建筑物重建的地图为栅格地图；
[0085]
步骤s2.对于步骤s1建筑物重建的地图，采用点云更新栅格地图中建筑物的占据栅格和占据栅格特定半径范围0.2-0.5m内的非占据栅格；
[0086]
步骤s3.每当步骤s2中的每一帧点云被用于更新栅格地图的占据栅格和非占据栅格后，计算该点云的轴对齐包围框，在轴对齐包围框内提取在空间上与建筑物的占据栅格毗邻，并且与未知栅格毗邻的栅格作为边境栅格，并且删除任何与轴对齐包围框有交集的边境栅格聚类，以完成对边境栅格的提取；
[0087]
步骤s4.在步骤s3提取得到边境栅格后，利用区域增长算法对边境栅格进行聚类，使得空间上相连的边境栅格组成边境栅格聚类，通过设置阈值对边境栅格聚类进行筛选，筛选出符合设定阈值范围内的边境栅格聚类；
[0088]
步骤s5.通过步骤s4提取边境栅格并将其根据空间位置关系进行聚类，设边境栅格聚类的集合为c
frt
，机器人对边境栅格聚类进行观测，直到环境中没有边境栅格聚类或者所有边境栅格聚类都不能在机器人的可行驶区域内被看见；对于每一个边境栅格聚类c
frt
，通过在机器人可行驶区域内对边境栅格聚类进行视角采样，从而获得其对应的视角集合vpi；
[0089]
步骤s6.在得到边境栅格聚类的视角集合vpi后，根据边境栅格聚类所能观测到的
信息以及边境栅格聚类到当前机器人的位置给视角打分，以获得当前环境下每个边境栅格聚类的最佳观测视角，进而得到最高得分视角集合；
[0090]
步骤s7.在得到最高得分视角集合后，进行旅行商问题求解，求出下一个最佳视角，根据下一个最佳视角完成机器人的路径规划与导航；
[0091]
步骤s8.在完成步骤s7的路径规划后，机器人的主动探索与路径规划模式为规划-执行-观测循环，机器人执行规划好的路径，且在行走过程中，通过定位模块中的激光雷达对建筑物进行扫描，并将建筑物的占据栅格按照一定半径进行膨胀，获取膨胀后的位于占据栅格周围的自由栅格，以补全自由栅格，完成地图更新，同时完成步骤s5的视角采样，以使机器人通过路径规划完成对建筑物的主动探索与地图重建；
[0092]
步骤s1中的所述定位模块包括三维感知传感器和惯性测量单元imu，所述三维感知传感器包括激光雷达和用于调整激光雷达偏航角和俯仰角的驱动电机。
[0093]
所述步骤s1中所述建筑物重建的地图为八叉树栅格地图，所述步骤s2中的占据栅格为三维占据栅格。
[0094]
所述步骤s2中用点云更新占据栅格时采用的方法如下：对于机器人采集的当前点云的每一个三维点，计算其到所述三维感知传感器中心的连线所经过的三维栅格，并且增加这些栅格的非占据概率，形成非占据栅格；随后对该三维点所在的栅格，增加其占据的概率，从而完成了一个三维点的地图更新，对于建筑物重建的地图，只更新占据栅格附近的非占据栅格，而保留其他栅格的未知状态。如图6所示，其中黑色栅格表示占据栅格，白色栅格表示自由栅格，即非占据栅格。本发明采用的方案是分别维护重建与导航的地图，上述方法为类似于bresenham算法以及其他离散空间的光线投射方法，相较于原始的不限定半径的更新方式，本发明提出的更新占据栅格方法只在占据栅格的特定半径内做了非占据栅格的更新，从而极大减小了光线投射的计算量，保证了地图的更新的实时性，同时可以解决在户外场景下场景过大达到上百米，当地图的分辨率设置得比较小时，因为光线投射的计算量太大而不能完成地图的实时更新的问题。
[0095]
进一步的，所述步骤s2还包括占据栅格设定半径内的自由栅格补全，所述自由栅格即为非占据栅格，自由栅格补全方法包括如下步骤：
[0096]
步骤a1.对占据栅格按照设定半径进行膨胀，将膨胀出来的新的栅格设为栅格集合v
dl
；占据栅格为图7中白色栅格上方的灰色栅格，膨胀出来的新的栅格集合v
dl
为图7中占据栅格上方、左侧和右侧的黑色栅格；
[0097]
步骤a2.遍历栅格集合v
dl
并找出三维感知传感器视场角之外的栅格集合，标记为v
iv
；v
iv
即为图7的第二张图中占据栅格右侧的浅灰色栅格；
[0098]
步骤a3.对经过步骤a2筛选剩下的栅格进行从三维感知传感器位置出发的光线投射，标记出被建筑物所遮挡的栅格集合，记为v
ocl
；v
ocl
即为图7的第二张图中的占据栅格上方的浅灰色栅格；
[0099]
步骤a4.通过删除经过步骤a2和a3标记出来的栅格结合，最终保留的被补全的自由栅格集合表示为：
[0100]vfree
＝{v|v∈v
dl
\(v
iv
∪v_ocl)}，v
free
即为图7的第二张图中的占据栅格右侧的黑色栅格。
[0101]
因为户外场景较为空旷，所以会有激光雷达部分点云测量值超过量程，导致测量
缺失的情况。由于测量的缺失，构建的地图中的部分自由栅格也会因此缺失。因此上述基于栅格膨胀与光线投射的自由栅格补全策略可以实现对占据栅格周围缺失的自由栅格进行补全。通过该策略对探索与重建空间内的自由栅格进行补全，可以完成对户外场景测量缺失情况下的地图更新，有助于在进行主动重建任务时标记已探索区域，保证重建任务的可持续执行。
[0102]
进一步的，在所述步骤s4中，当边境栅格聚类的数量小于设定的最小阈值时，该最小阈值设置为5，则忽略该边境栅格聚类，因为数量过小的聚类往往是因为传感器噪声导致，因此本发明将这些聚类忽略；当边境栅格的尺寸超过设定的最大阈值后，该最大阈值设置为2m/栅格的分辨率，对边境栅格进行主成分分析，算出边境栅格形状的长轴的方向，并且在长轴方向上将边境栅格递归地进行拆分，该过程是递归进行的，直到所有的边境栅格聚类都小于设定的最大阈值。通过边境栅格聚类的筛选以及对大尺寸边境栅格进行递归以实现递进地检测与更新边境栅格，从而减少计算量。
[0103]
进一步的，在所述步骤s3中，为了使边境栅格只在建筑物的占据栅格附近生成，以使机器人专注于探索建筑物的边界，需要判别某个栅格是否属于目标建筑物，判别栅格是否属于目标建筑物的方法如下：利用并查集算法，给每个栅格添加一个根节0点的信息，算法流程如图8所示，在机器人初始化启动时，对其初始点云构建出来的栅格进行归并，并将拥有最多子节点的根节点记为vbd，在之后每次向占据栅格所在的地图中插入点云时，都会将新生成的占据栅格与其邻近栅格进行归并，并更新其根节点的值，通过对占据栅格进行归并后，在空间上与vbd相连的栅格将拥有相同的根节点，而空间上不与vbd相连的栅格则不会拥有相同的根节点，进而通过判断当前占据栅格是否与vbd有相同的根节点，即可判断当前的占据栅格是否是建筑物的一部分，当确定目标建筑物的占据栅格后，即可继续完成对边境栅格的提取。
[0104]
进一步的，如图9所示，在所述步骤s5中，以某个边境栅格ci的聚类的中心点为圆心，采取固定的半径步长r
step
与角度步长θstep，在机器人的可行驶区域进行采样，所得到视角集合即为vpi。
[0105]
优选的，在所述步骤s6中，通过覆盖率来评价一个视角能真正地观测到其对应的栅格聚类的程度，一个视角的覆盖率是这个视角下所能看见的对应的边境栅格的数量占原边境栅格数量的比例，基于此，通过视角打分函数选择出的最佳视角为的计算公式为：
[0106][0107]
其中为视角集合中覆盖率最大的视角，f
dist
表示边境栅格聚类中心点到机器人当前位置点之间的距离，d
max
为设置的距离阈值，m为地图坐标系，r为机器人坐标系，p机器人的位置。通过该打分函数选出的最佳视角，既具有足够的覆盖率，同时离机器人的当前位置的距离也在单次探索的最长距离之内，从而保证了探索任务的有效执行，获得最高得分视角集合。
[0108]
进一步的，在所述步骤s8中，在机器人行走过程中，针对主动重建任务的特性，通过构建位姿因子图的方式，利用当前观测的建筑物点云与已重建的地图做回环检测与修正机器人位姿，利用机器人当前观测点云来修正机器人的位姿并构建点云子图的方法如下：
[0109]
步骤b1.获得当前机器人的观测点云，因为livox horizon激光雷达的非重复式扫描的特性，使得在对建筑物进行观测时，单帧点云只能够扫描到雷达视场角内的部分建筑物表面，因此需要在时间上构建一个当前观测的点云子图，使得扫描到的点云尽可能稠密，采用的点云子图形式为哈希栅格地图，哈希栅格地图具体为：对于要插入地图的每个点，根据地图的分辨率获得其三维栅格的索引，随后在哈希表里查找该栅格索引对应的栅格；
[0110]
步骤b2.如图10所示，当机器人运动到下一个视角的半径范围之内时，利用当前观测的点云构建点云子图，当机器人执行到该视角的位置处时，会将当前点云子图与已重建地图进行配准，并将配准的结果作为一元边加入到因子图中，步骤b1中哈希栅格地图对应的哈希子图构建时采用双向配准的策略，首先将激光雷达采集的点云根据当前惯性里程计imu的信息投影到世界坐标系中，得到点云，若当前的哈希子图的点云帧数或者三维点数量小于设定的阈值，则直接向哈希子图中插入世界坐标系中的点云，设以此法插入的点云帧数为k；否则将哈希子图中的所有点云配准到新的点云的坐标系下，并与新获取的点云一同重新加入到哈希子图中；最后，更新哈希子图的位姿信息；当一个子图构建完成后，将前k帧点云从哈希栅格地图中抽取出来，重新与剩下的点云进行配准，以保证点云的一致性。在该算法中，将哈希子图的点云重新配准到新来的点云的坐标系下，是为了构建因子图优化时，对后续里程计更新时能够直接利用最新的位姿。
[0111]
进一步的，如图11所示，通过gtsam算法构建因子图，从而构建优化问题并更新位姿，因子图的构建算法是与步骤s7的机器人的路径规划与导航方法相耦合，以生成机器人的定位系统，耦合步骤如下：
[0112]
步骤c1.当机器人从当前视角行驶到下一个视角后，连接这两个视角的位姿变量的二元因子就被加入到因子图中(图11中的里程计因子)，该二元因子用来约束两个位姿变量之间的相对变换关系，这两个视角的惯性里程计位姿由位姿估计器给出，分别记为则该二元因子的相对位姿为：
[0113][0114]
步骤c2.当机器人行驶到下一个视角处后，将进行当前点云子图的点云对建筑物地图的配准，并将配准结果为作为一元观测结果即上述一元边加入到因子图中(图8中的全局配准因子)；这里分析一下全局配准因子的有效性，点云配准算法能得到稳定结果的前提是两个点云之间需要有较高的重叠部分，而这个前提是由前端的视角与路径规划算法保证的，如图11所示，子图总是构建在边境栅格(最右侧灰色纵向排列栅格)的位置，由上述步骤s3中边境栅格的定义方式可知，边境栅格是已知的占据栅格与未知栅格的交界位置，因此能够保证新构建的点云子图与之前的地图既有足够的重叠面积(点云子图里的已知栅格部分)，又能够探索出足够多的区域。
[0115]
步骤c3.若两个视角之间的欧氏距离与偏航角小于设置的阈值，则会将这两个点云子图单独进行配准，点云子图在构建时已经将观测到的点云转换到世界坐标系下，由于
位姿估计器误差的存在，导致构建第一个子图时的世界坐标系与构建第二个子图时的世界坐标系不完全一致，根据此不一致，可以得到其约束为：
[0116][0117]
其中，表示视角vp1的地图坐标系下的三维变换的逆；表示视角vp2所在的地图坐标系到视角vp1所在的地图坐标系的变换；表示视角vp2的地图坐标系下的三维变换；
[0118]
步骤c4.当机器人按照路径规划行走，且机器人准备执行到下一个视角之前，会对当前所有的视角的位姿进行更新，并相应地调整点云子图与全局地图的点云与栅格。
[0119]
本实施例采用的位姿估计器是fast—lio2，在一些较为空旷的户外场景中，容易因为场景退化的问题导致位姿估计不够准确。而fast—lio2是没有回环检测的，因此当位姿估计失败时，该估计器没有任何策略来将其纠正，因此采用通过构建位姿因子图的方式，利用当前观测的建筑物点云与已构建的地图做回环检测与修正，使得机器人的定位系统更加鲁棒，构建出来的地图更加精确。
[0120]
以上对本发明所提供的实施例进行了详细阐述。本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的核心思想。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明的原理的前提下，还可以本发明进行若干改进和修饰，这些改进和修饰也落入本发明权利要求的保护范围内。
技术特征：1.一种三维感知传感器，其特征在于，包括：云台底盘，所述云台底盘上安装转动件，所述转动件可相对所述云台底盘转动；云台支架，所述云台支架安装在所述转动件上端面，以使所述转动件带动所述传感器支架转动，所述云台支架上设置连杆升降机构；驱动电机，所述驱动电机包括第一电机和第二电机，所述第一电机安装在所述云台底盘上用于驱动所述转动件转动，所述第二电机安装在所述云台支架上，所述第二电机用于驱动所述连杆升降机构运动，以使所述连杆升降机构远离所述云台底盘的一端做升降运动；激光雷达，所述激光雷达通过安装板与所述连杆升降机构的上端处的转动块连接，以使所述连杆升降机构驱动所述安装板上的激光雷达做俯仰运动；陀螺仪，所述陀螺仪设置在所述激光雷达的一侧，以使所述陀螺仪与所述激光雷达一起运动；所述第一电机驱动所述云台支架转动以使所述激光雷达具有偏航角的自由度，所述第二电机驱动所述连杆升降机构上端处的转动块转动以使所述激光雷达具有俯仰角的自由度。2.根据权利要求1所述的三维感知传感器，其特征在于，所述连杆升降机构包括与所述第二电机的输出轴连接的转动杆和与所述转动杆连接的升降杆，所述升降杆竖向设置，所述转动杆的一端与所述第二电机的输出轴连接，所述转动杆的另一端与所述升降杆的一端连接，所述升降杆的另一端与所述转动块连接，所述转动块与所述云台支架转动连接，以使所述升降杆驱动所述转动块转动，所述安装板安装在所述转动块上，以使所述转动块转动时所述安装板做俯仰运动。3.一种面向建筑物的主动重建方法，其特征在于，包括如下步骤：步骤s1.通过机器人的激光雷达对建筑物进行扫描，并将扫描区域中获取的每帧点云进行坐标转换，将每帧点云用于更新建筑物的地图，以完成重建建筑物的地图，建筑物重建的地图为栅格地图；步骤s2.对于步骤s1建筑物重建的地图，采用点云更新栅格地图中建筑物的占据栅格和占据栅格特定半径范围内的非占据栅格；步骤s3.每当步骤s2中的每一帧点云被用于更新栅格地图的占据栅格和非占据栅格后，计算该点云的轴对齐包围框，在轴对齐包围框内提取在空间上与建筑物的占据栅格毗邻，并且与未知栅格毗邻的栅格作为边境栅格，并且删除任何与轴对齐包围框有交集的边境栅格聚类，以完成对边境栅格的提取；步骤s4.在步骤s3提取得到边境栅格后，利用区域增长算法对边境栅格进行聚类，使得空间上相连的边境栅格组成边境栅格聚类，通过设置阈值对边境栅格聚类进行筛选，筛选出符合设定阈值范围内的边境栅格聚类；步骤s5.通过步骤s4提取边境栅格并将其根据空间位置关系进行聚类，设边境栅格聚类的集合为c
frt
，机器人对边境栅格聚类进行观测，直到环境中没有边境栅格聚类或者所有边境栅格聚类都不能在机器人的可行驶区域内被看见；对于每一个边境栅格聚类c
frt
，通过在机器人可行驶区域内对边境栅格聚类进行视角采样，从而获得其对应的视角集合vp
i
；步骤s6.在得到边境栅格聚类的视角集合vp
i
后，根据边境栅格聚类所能观测到的信息
以及边境栅格聚类到当前机器人的位置给视角打分，以获得当前环境下每个边境栅格聚类的最佳观测视角，进而得到最高得分视角集合；步骤s7.在得到最高得分视角集合后，进行旅行商问题求解，求出下一个最佳视角，根据下一个最佳视角完成机器人的路径规划与导航；步骤s8.在完成步骤s7的路径规划后，机器人的主动探索与路径规划模式为规划-执行-观测循环，机器人执行规划好的路径，且在行走过程中，通过定位模块中的激光雷达对建筑物进行扫描，并将建筑物的占据栅格按照一定半径进行膨胀，获取膨胀后的位于占据栅格周围的自由栅格，以补全自由栅格，完成地图更新，同时完成步骤s5的视角采样，以使机器人通过路径规划完成对建筑物的主动探索与地图重建；步骤s1中的所述定位模块包括三维感知传感器和惯性测量单元imu，所述三维感知传感器包括激光雷达和用于调整激光雷达偏航角和俯仰角的驱动电机。4.根据权利要求3所述的面向建筑物的主动重建方法，其特征在于，所述步骤s2中用点云更新占据栅格时采用的方法如下：对于机器人采集的当前点云的每一个三维点，计算其到所述三维感知传感器中心的连线所经过的三维栅格，并且增加这些栅格的非占据概率，形成非占据栅格；随后对该三维点所在的栅格，增加其占据的概率，从而完成了一个三维点的地图更新，对于建筑物重建的地图，只更新占据栅格附近的非占据栅格，而保留其他栅格的未知状态。5.根据权利要求3所述的面向建筑物的主动重建方法，其特征在于，所述步骤s2还包括占据栅格设定半径内的自由栅格补全，所述自由栅格即为非占据栅格，自由栅格补全方法包括如下步骤：步骤a1.对占据栅格按照设定半径进行膨胀，将膨胀出来的新的栅格设为栅格集合v
dl
；步骤a2.遍历栅格集合v
dl
并找出三维感知传感器视场角之外的栅格集合，标记为v
iv
；步骤a3.对经过步骤a2筛选剩下的栅格进行从三维感知传感器位置出发的光线投射，标记出被建筑物所遮挡的栅格集合，记为v
ocl
；步骤a4.通过删除经过步骤a2和a3标记出来的栅格结合，最终保留的被补全的自由栅格集合表示为：v
free
＝{v|v∈v
dl
\(v
iv
∪v_ocl)}。6.根据权利要求3所述的面向建筑物的主动重建方法，其特征在于，在所述步骤s4中，当边境栅格聚类的数量小于设定的最小阈值时，则忽略该边境栅格聚类，当边境栅格的尺寸超过设定的最大阈值后，对边境栅格进行主成分分析，算出边境栅格形状的长轴的方向，并且在长轴方向上将边境栅格递归地进行拆分，该过程是递归进行的，直到所有的边境栅格聚类都小于设定的最大阈值。7.根据权利要求3所述的面向建筑物的主动重建方法，其特征在于，在所述步骤s3中，为了使边境栅格只在建筑物的占据栅格附近生成，以使机器人专注于探索建筑物的边界，需要判别某个栅格是否属于目标建筑物，判别栅格是否属于目标建筑物的方法如下：利用并查集算法，给每个栅格添加一个根节点的信息，在机器人初始化启动时，对其初始点云构建出来的栅格进行归并，并将拥有最多子节点的根节点记为vbd，在之后每次向占据栅格所在的地图中插入点云时，都会将新生成的占据栅格与其邻近栅格进行归并，并更新其根节点的值，通过对占据栅格进行归并后，在空间上与vbd相连的栅格将拥有相同的根节点，而
空间上不与vbd相连的栅格则不会拥有相同的根节点，进而通过判断当前占据栅格是否与vbd有相同的根节点，即可判断当前的占据栅格是否是建筑物的一部分，当确定目标建筑物的占据栅格后，即可继续完成对边境栅格的提取。8.根据权利要求3所述的面向建筑物的主动重建方法，其特征在于，在所述步骤s6中，通过覆盖率评价一个视角能真正地观测到其对应的栅格聚类的程度，一个视角的覆盖率是这个视角下所能看见的对应的边境栅格的数量占原边境栅格数量的比例，基于此，通过视角打分函数选择出的最佳视角为角打分函数选择出的最佳视角为的计算公式为：其中为视角集合中覆盖率最大的视角，f
dist
表示边境栅格聚类中心点到机器人当前位置点之间的距离，d
max
为设置的距离阈值，m为地图坐标系，r为机器人坐标系，p机器人的位置。9.根据权利要求3所述的面向建筑物的主动重建方法，其特征在于，在所述步骤s8中，在机器人行走过程中，针对主动重建任务的特性，通过构建位姿因子图的方式，利用当前观测的建筑物点云与已重建的地图做回环检测与修正机器人位姿，利用机器人当前观测点云来修正机器人的位姿的方法如下：步骤b1.获得当前机器人的观测点云，在时间上构建一个当前观测的点云子图，使得扫描到的点云尽可能稠密，采用的点云子图形式为哈希栅格地图，哈希栅格地图具体为：对于要插入地图的每个点，根据地图的分辨率获得其三维栅格的索引，随后在哈希表里查找该栅格索引对应的栅格；步骤b2.当机器人运动到下一个视角的半径范围之内时，利用当前观测的点云构建点云子图，当机器人执行到该视角的位置处时，会将当前点云子图与已重建地图进行配准，并将配准的结果作为一元边加入到因子图中，步骤b1中哈希栅格地图对应的哈希子图构建时采用双向配准的策略，首先将激光雷达采集的点云根据当前惯性里程计imu的信息投影到世界坐标系中，得到点云，若当前的哈希子图的点云帧数或者三维点数量小于设定的阈值，则直接向哈希子图中插入世界坐标系中的点云，设以此法插入的点云帧数为k；否则将哈希子图中的所有点云配准到新的点云的坐标系下，并与新获取的点云一同重新加入到哈希子图中；最后，更新哈希子图的位姿信息；当一个子图构建完成后，将前k帧点云从哈希栅格地图中抽取出来，重新与剩下的点云进行配准，以保证点云的一致性。10.根据权利要求9所述的面向建筑物的主动重建方法，其特征在于，通过gtsam算法构建因子图，从而构建优化问题并更新位姿，因子图的构建算法是与步骤s7的机器人的路径规划与导航方法相耦合，以生成机器人的定位系统，耦合步骤如下：步骤c1.当机器人从当前视角行驶到下一个视角后，连接这两个视角的位姿变量的二元因子就被加入到因子图中,该二元因子用来约束两个位姿变量之间的相对变换关系，这两个视角的惯性里程计位姿由位姿估计器给出，分别记为则该二元因子的相对位姿为：
步骤c2.当机器人行驶到下一个视角处后，将进行当前点云子图的点云对建筑物地图的配准，并将配准结果为作为一元观测结果加入到因子图中；步骤c3.若两个视角之间的欧氏距离与偏航角小于设置的阈值，则会将这两个点云子图单独进行配准，点云子图在构建时已经将观测到的点云转换到世界坐标系下，由于位姿估计器误差的存在，导致构建第一个子图时的世界坐标系与构建第二个子图时的世界坐标系不完全一致，根据此不一致，可以得到其约束为：其中，表示视角vp1的地图坐标系下的三维变换的逆；表示视角vp2所在的地图坐标系到视角vp1所在的地图坐标系的变换；表示视角vp2的地图坐标系下的三维变换；步骤c4.当机器人按照路径规划行走，且机器人准备执行到下一个视角之前，会对当前所有的视角的位姿进行更新，并相应地调整点云子图与全局地图的点云与栅格。
技术总结本发明公开了一种三维感知传感器，通过设计由云台底盘、云台支架、驱动电机构成的二轴云台来提供激光雷达的俯仰角与偏航角，从而满足主动重建的任务。同时还公开了一种面向建筑物的主动重建方法，用于解决户外场景的建筑物的主动重建问题，以地面移动机器人的运动学特性和应用场景为导向，提出并建立了一种新颖的基于移动机器人的面向建筑物的主动重建系统，从而实现了户外场景下建筑物的主动探索与重建技术的视角与路径规划，在此基础上，又进一步研究并提出了一种基于因子图优化的激光雷达-惯性里程计的定位性能提升方案，通过本发明的方案，机器人能够有效执行基于移动机器人的户外建筑物主动重建。的户外建筑物主动重建。的户外建筑物主动重建。
技术研发人员：陈浩耀 曹明 李鸣航 楼云江
受保护的技术使用者：哈尔滨工业大学（深圳）
技术研发日：2022.03.23
技术公布日：2022/7/5
