地图的主动构建方法、主动构建装置与地图的构建系统与流程

本技术涉及三维空间地图的构建领域，具体而言，涉及一种地图的主动构建方法、主动构建装置、计算机可读存储介质与地图的构建系统。

背景技术：

1、目前移动机器人的建图导航系统主要是获得3d空间的一个2d占据栅格地图，用于避障规划。复合型机器人具有移动和执行能力，不仅要求能够在环境中安全的导航，还需要在3d空间执行一些操作，因此需要一个3d的地图。3d地图涉及更加丰富的环境信息，因此，在复合型机器人的建图过程中，对相机的观测姿态也具有更加精细的要求，以防止跟踪特征丢失导致建图失败。

2、现有技术中，为了对复合型机器人进行3d建图，需要首先确定出复合型机器人要移动的下一个目标点，在复合型机器人从当前的目标点移动到下一个目标点的过程中，对3d地图进行更新，直到目标区域的地图被完整构建出来。在实际的处理过程中，由于目标点是在笛卡尔空间，而复合型机器人的深度相机是在关节空间中，因此，需要从笛卡尔空间逆推以转换到关节空间中，并以深度相机的运动姿态作为约束，即需要求解一个高维空间带约束的规划问题，以确定复合型机器人是否可以从当前的目标点移动到下一个目标点。但在求解高维空间带约束的规划问题时，容易出现求解耗时且成功率低的问题。

3、因此，亟需一种正向求解方法以避免在求解高维空间带约束的规划模型的问题。

4、在背景技术部分中公开的以上信息只是用来加强对本文所描述技术的背景技术的理解，因此，背景技术中可能包含某些信息，这些信息对于本领域技术人员来说并未形成在本国已知的现有技术。

技术实现思路

1、本技术的主要目的在于提供一种地图的主动构建方法、主动构建装置、计算机可读存储介质与地图的构建系统，以避免现有技术中在构建机器人的地图过程中，需要对高维空间带约束的规划模型进行求解的问题。

2、根据本发明实施例的一个方面，提供了一种地图的主动构建方法，包括：获取机器人的当前状态信息及初始三维地图，所述当前状态信息包括初始关节角度，所述初始三维地图包括所述机器人在所述当前状态时的环境的信息；以所述初始关节角度为根节点，在目标采样空间中构建搜索树，并确定所述搜索树中的最佳节点，所述目标采样空间为所述机器人可达的空间；根据所述最佳节点，确定所述机器人从所述根节点移动至所述最佳节点的最优路径，并控制所述机器人按照所述最优路径从所述初始关节角度移动至所述最佳节点；获取所述机器人从所述初始关节角度移动至所述最佳节点过程中的环境的信息，得到目标点云信息，根据所述目标点云信息，对所述初始三维地图进行更新。

3、可选地，所述初始关节角度为所述机器人在当前状态的所有关节的角度；所述搜索树中的每个节点均对应一个信息增益值，所述最佳节点为所述搜索树中信息增益值最大的一个所述节点。

4、可选地，以所述初始关节角度为根节点，在目标采样空间中构建搜索树，包括：获取所述搜索树的最大节点数；以所述初始关节角度为根节点，在所述目标采样空间中，采集预定节点，并至少根据所述预定节点，确定扩展节点，并将所述扩展节点加入到当前的所述搜索树中；直到当前的所述搜索树中的总节点数达到所述最大节点数，停止构建所述搜索树。

5、可选地，在所述目标采样空间中，采样预定节点，包括：确定所述机器人的采样区间，并在所述采样区间中进行采样，得到采样角度，所述采样区间为所述机器人采样的关节角度的区间；获取所述机器人的当前关节角度；计算所述采样角度和所述当前关节角度之和，得到所述机器人的目标关节角度；确定所述目标关节角度是否超出所述机器人可移动的限位区间，在所述目标关节角度在所述限位区间内的情况下，将所述目标关节角度确定为所述预定节点，在所述目标关节角度不在所述限位区间内的情况下，重新确定所述采样角度。

6、可选地，确定所述机器人的采样区间，包括：获取所述机器人的最大运动速度，并获取所述机器人的采样周期；根据所述最大运动速度和所述采样周期，确定所述采样区间的上限值和下限值。

7、可选地，所述机器人还包括深度相机，至少根据所述预定节点，确定扩展节点，包括：对关节空间中的所述预定节点进行计算，得到所述预定节点在笛卡尔空间中的位姿信息；根据所述位姿信息，确定所述深度相机的姿态是否满足约束条件；在所述深度相机的姿态满足约束条件的情况下，根据所述预定节点，确定最近邻点，所述最近邻点为当前所述搜索树中与所述预定节点距离最小的所述节点；根据所述预定节点和所述最近邻点，并采用线性插值法，确定所述扩展节点。

8、可选地，根据所述预定节点，确定最近邻点，包括：计算所述预定节点与当前所述搜索树中每一个所述节点的欧式距离，得到多个目标距离；在多个所述目标距离中，确定最小目标距离；将所述最小目标距离对应的所述搜索树中的所述节点，确定为所述最近邻点。

9、可选地，对关节空间中的所述预定节点进行计算，得到所述预定节点在笛卡尔空间中的所述位姿信息，包括：采用正运动学，对所述关节空间中的所述预定节点进行坐标变换，得到所述预定节点在笛卡尔空间中的所述位姿信息。

10、可选地，根据所述预定节点和所述最近邻点，并采用线性插值法，确定所述扩展节点，包括：采用公式：ve＝vk+λ*(vs-vk)，计算所述扩展节点，其中，ve表示所述扩展节点，vk表示所述最近邻点，vs表示所述预定节点，0≤λ≤1。

11、可选地，在至少根据所述预定节点，确定扩展节点之后，在将所述扩展节点加入到当前的所述搜索树中之前，所述方法还包括：采用fcl集成库，确定所述扩展节点与所述最近邻点是否发生碰撞，在所述扩展节点与所述最近邻点未发生碰撞的情况下，将所述扩展节点加入到当前的所述搜索树中，在所述扩展节点与所述最近邻点发生碰撞的情况下，控制所述机器人重新采集所述预定节点。

12、可选地，计算所述搜索树中的每个所述节点的信息增益值包括：采用计算所述扩展节点的信息增益值，其中，ig(nk)表示所述扩展节点的所述信息增益值，ig(nk-1)表示所述最近邻点的所述信息增益值，表示当前所述深度相机在可视区域visible(m,ξk)内未观测体素mi的数量，i(mi)为指示函数，为当前所述搜索树中所述最近邻点到所述扩展节点之间的路径代价，β为所述最近邻点的所述信息增益值与所述路径代价之间的权重系数。

13、可选地，在根据所述目标点云信息，对所述初始三维地图进行更新之后，所述方法还包括：将所述机器人当前所在的最佳节点确定所述搜索树的根节点，依次执行：构建所述搜索树并确定所述最佳节点的步骤、确定所述最优路径并控制所述机器人移动的步骤以及获取所述目标点云信息以及更新所述地图的步骤；直到重新构建的所述搜索树中的所述最佳节点的信息增益值小于预定值为止。

14、根据本发明实施例的另一方面，还提供了一种地图的主动构建装置，包括：获取单元，用于获取机器人的当前状态信息及初始三维地图，所述当前状态信息包括初始关节角度，所述初始三维地图包括所述机器人在所述当前状态时的环境的信息；构建单元，用于以所述初始关节角度为根节点，在目标采样空间中构建搜索树，并确定所述搜索树中的最佳节点，所述目标采样空间为所述机器人可达的空间；第一确定单元，用于根据所述最佳节点，确定所述机器人从所述根节点移动至所述最佳节点的最优路径，并控制所述机器人按照所述最优路径从所述初始关节角度移动至所述最佳节点；更新单元，用于获取所述机器人从所述初始关节角度移动至所述最佳节点过程中的环境的信息，得到目标点云信息，根据所述目标点云信息，对所述初始三维地图进行更新。

15、根据本发明实施例的又一方面，还提供了一种计算机可读存储介质，所述计算机可读存储介质包括存储的程序，其中，所述程序执行任意一种所述的方法。

16、根据本发明实施例的一方面，还提供了一种地图的构建系统，包括：机器人、主动构建装置，所述主动构建装置与所述机器人通信连接，用于执行任意一种所述的方法。

17、在本发明实施例中，所述的地图的构建方法中，首先，获取机器人的当前状态信息以及初始三维地图，其中，所述机器人的所述当前状态信息包括所述机器人的初始关节角度，初始三维地图为所述机器人在当前状态时的环境的信息，然后，以所述初始关节角度作为根节点，在目标采样空间中进行采样以构建搜索树，并确定搜索树的最佳节点，之后，根据最佳节点，确定机器人从根节点移动至最佳节点的最优路径，并控制机器人按照最优路径从初始关节角度移动至最佳节点，即控制机器人按照最优路径从根节点移动至最佳节点，最后，获取机器人从初始关节角度移动至最佳节点的过程中的环境信息，得到目标点云信息，并根据所述目标点云信息，对初始三维地图进行更新。在本方案中，将机器人的初始关节角度作为根节点，在目标采样空间中进行采样以构建搜索树，并确定搜索树中的最佳节点，再根据最佳节点，确定机器人移动的最优路径，由于目标采样空间为机器人可达的空间，故机器人按照最优路径由根节点移动至最佳节点是必定可达的，与现有技术中，在确定出最佳节点后，通过逆向求解的方法来确定机器人由初始关节角度到最佳节点的路径是否可达的相比，即与建立的由最佳节点到初始关节角度的高维空间带约束的规划模型进行逆向求解相比，本方提供了一种通过正向搜索的方法来确定最佳节点，无需建立高维空间带约束的规划模型，进而避免了逆向求解过程中求解耗时且成功率较低的问题，从而避免了现有技术中在构建机器人的地图过程中，需要对高维空间带约束的规划模型进行求解的问题。另外，本方案中，获取机器人从初始关节角度移动最佳节点的过程中的目标点云信息，并根据目标点云信息来对初始三维地图进行更新，保证了能够较为准确地对初始三维地图进行更新，保证了更新后的地图较为完整。