CN112304318B - 一种虚实耦合约束环境下的机器人自主导航方法 - Google Patents

Abstract

本发明公开了一种虚实耦合约束环境下的机器人自主导航方法，包括虚拟空间的搭建，虚实耦合约束环境的构建，虚实耦合约束环境下应用改进的A^*算法进行机器人全局路径规划，得到最优规划路径，获取现实环境信息并结合虚实耦合约束环境，再考虑机器人的轮廓特征，在最优规划路径的相邻节点之间应用改进的DWA算法进行机器人局部避障。本方法采用BIM模型与三维重建获得的点云模型耦合的方法，建立建筑机器人工作的虚实耦合约束环境；结合虚实耦合约束环境数据，考虑机器人的安全性与通过性改进A^*算法，删除冗余转折点；根据虚实耦合约束环境数据对DWA算法进行改进，使机器人在复杂环境下绕过局部最小陷阱。

Description

一种虚实耦合约束环境下的机器人自主导航方法

技术领域

本发明涉及移动机器人自主导航领域，具体是一种虚实耦合约束环境下的机器人自主导航方法。

背景技术

近年来，智能建造是现代建筑业与新一代信息技术融合的最新研究领域。伴随着建筑工业化的发展，它将颠覆传统建造模式，催生一系列的新技术和新业态。发展智能化的建筑机器人已成为必然趋势，自主路径规划与实时避障是实现建筑机器人自主作业的关键技术。当前，SLAM方法是移动机器人领域解决环境感知与定位导航的有效手段之一。现有建筑机器人的质量和体积较大且作业空间受限，并不适合应用传统SLAM方法建立完整的环境地图，且建筑机器人施工环境已知，因此，迫切需要一种基于已知复杂建筑施工环境的自主导航方式，考虑建筑机器人的轮廓特征的安全性与通过性，并提升路径的平滑度。

发明内容

针对现有技术的不足，本发明拟解决的技术问题是，提供一种虚实耦合约束环境下的机器人自主导航方法。

本发明解决所述技术问题的技术方案是，提供一种虚实耦合约束环境下的机器人自主导航方法，其特征在于，该方法包括以下步骤：

第一步、虚拟空间的搭建：建立建筑物的BIM模型，再对BIM模型的信息进行提取，得到建筑物实体的位置信息；建筑物实体的位置信息和BIM模型构成虚拟空间；

第二步、虚实耦合约束环境的构建；

步骤1、由机器人得到周围环境的点云模型；

步骤2、根据第一步中得到的建筑物实体的位置信息，基于模型边界点的匹配方法将BIM模型中的边界点与点云模型中对应的边界点进行匹配，得到配准后的虚实耦合模型；

步骤3、将配准后的虚实耦合模型导入到物理引擎中，再在物理引擎中根据BIM模型与点云模型的建筑物实体属性对配准后的虚实耦合模型添加包络碰撞体，得到虚实耦合约束环境；

第三步、虚实耦合约束环境下的机器人全局路径规划；

步骤1、得到虚实耦合约束环境后，根据第一步得到的建筑物实体的位置信息进行栅格地图构建；

步骤2、得到栅格地图后，应用改进的A^*算法进行机器人全局路径规划，得到最优规划路径；

第四步、获取现实环境信息并结合虚实耦合约束环境，再考虑机器人的轮廓特征，在最优规划路径的相邻节点之间应用改进的DWA算法进行机器人局部避障。

与现有技术相比，本发明有益效果在于：

1、本方法采用BIM模型与三维重建获得的点云模型耦合的方法，建立建筑机器人工作的虚实耦合约束环境；结合虚实耦合约束环境数据，考虑机器人的安全性与通过性改进A^*算法，删除冗余转折点；根据虚实耦合约束环境数据对DWA算法进行改进，使机器人在复杂环境下绕过局部最小陷阱。

2、现有解决环境感知与定位导航的SLAM方法需要对未知环境下进行完整的巡视，对于自身轮廓较大且作业空间受限的建筑机器人很难应用现有方法进行完整信息获取。本发明搭建的虚实耦合约束环境可以使机器人的工作环境由未知变为已知，从而在已知环境下进行机器人全局路径规划与局部避障。

3、现有的全局路径规划算法并未考虑机器人的轮廓特征，搜索出的路径贴近障碍物，对机器人的安全性造成较大影响，且存在大量冗余转折点不利于机器人的路径跟踪。本发明对传统的A^*算法进行改进，根据机器人的轮廓特征与虚实耦合约束环境中的障碍物对搜索节点进行限制，得到的全局路径为机器人与障碍物之间预留了足够的安全距离，提升了机器人的安全性。结合虚实耦合约束环境信息进行空间通过性判断，并对冗余节点进行判断与剔除，得到最优规划路径，降低了路径长度、搜索时间与转折角度，提升了路径的平滑度。

4、现有的局部路径规划算法以与障碍物的安全距离为安全条件并且只能根据传感器获取的现实环境信息进行避障，在复杂环境下容易陷入局部最小陷阱，机器人必须掉头重新规划局部路径以到达目标点，这将花费更长的时间并使机器人移动更长的距离。本发明在改进的A^*算法得到的节点间应用改进的DWA方法进行局部避障，考虑了建筑机器人的轮廓特征，根据机器人所搭载的雷达传感器获得的局部现实环境信息与虚实耦合约束环境信息进行判断，帮助机器人绕过复杂空间下的局部最小陷阱，提升了机器人的安全性并缩短了路径长度。

附图说明

图1为本发明的搜索限制方法图；

图2为本发明的现实环境与虚实耦合约束环境信息获取图；

图3为本发明实施例1的建筑物实体信息提取与显示图。

具体实施方式

下面给出本发明的具体实施例。具体实施例仅用于进一步详细说明本发明，不限制本申请权利要求的保护范围。

本发明提供了一种虚实耦合约束环境下的机器人自主导航方法(简称方法)，其特征在于，该方法包括以下步骤：

第一步、虚拟空间的搭建：通过测绘或根据图纸，应用建筑物建模软件(本实施例为AutodeskRevit)建立建筑物的BIM模型(Building InformationModeling，建筑信息模型)，并导出为IFC格式文件；IFC是基于EXPRESS语言来定义建筑工程领域信息交换与共享的数据表达，EXPRESS不是编程语言并且不可以被计算机编译执行，但是可以被JAVA、C++、C#等面向对象的编程语言识别和处理；再通过C++正则表达式等面向对象的编程语言根据IFC格式文件对BIM模型中的建筑元素位置、形状等信息进行提取，得到建筑物实体(墙、门、窗等)的位置信息；将提取BIM模型信息时所用的代码封装为动态链接库(Dynamic LinkLibrary，DLL)，应用数据分析软件(本实施例为MATLAB)调用动态链接库并制作提取界面，显示建筑物实体的位置信息；

建筑物实体的位置信息和BIM模型构成虚拟空间；

第二步、虚实耦合约束环境的构建；

步骤1、将第一步搭建的虚拟空间上传至服务器，同时，由扫描机器人通过三维重建的方式得到周围环境的点云模型，将点云模型上传至服务器；

步骤2、考虑到BIM模型与点云模型之间存在的模型误差问题，结合迭代最近点(Iterative ClosestPoint，ICP)方法，本发明给出了一种基于模型边界点的匹配方法；根据第一步中得到的建筑物实体的位置信息，在服务器中基于模型边界点的匹配方法将BIM模型中的边界点与点云模型中对应的边界点进行匹配，得到配准后的虚实耦合模型；

优选地，第二步中，所述基于模型边界点的匹配方法具体是：

设BIM模型边界点集合为P＝{p₁,...,p_i,...,p_m}，点云模型边界点集合为P'＝{p'₁,...,p'_i,...,p'_m}，其中

N为自然数集，由p'_i到p_i的欧式变换R,t可表示为

R为旋转矩阵，t为平移矩阵；

但由于BIM模型是一种理想的虚拟模型，与实际的建筑物存在差异，则第i个BIM模型边界点p_i与第i个点云模型边界点p'_i的匹配误差表示为：

e_i＝p_i-(Rp'_i+t) (1)

将式(1)构建为最小二乘问题的表达式，求使匹配误差平方和达到极小值时的R,t如式(2)所示：

设BIM模型边界点集合的质心和点云模型边界点集合的质心的表达式分别为：

根据式(2)，得到匹配误差函数的表达式如式(3)所示：

由式(3)展开以及质心表达式得：

由于(p_i-p-R(p'_i-p'))项在求和后等于零，则式(4)可简化为：

式5)的两项中，第一项只和R相关，第二项既有R又有t，但第二项只和质心相关，因此只要使用第一项求得R，令第二项为零即可求得t；

根据式(5)可以得出：

t＝p-Rp'，其中，q_i为BIM模型边界点的去质心坐标，q'_i为点云模型边界点的去质心坐标，q_i＝p_i-p，q'_i＝p'_i-p'；

根据式(5)以及q_i和q'_i对式(5)的第一项进行展开：

由于式(6)的等号右边中只有第三项与R有关，实际上式(6)可表示为：

式7)中，tr表示矩阵的迹；

应用现有的奇异值分解方法求解出式(7)中的最优R：定义矩阵

对W进行奇异值分解，得W＝UΣV^T，其中，Σ为奇异值组成的对角矩阵，U和V为对角矩阵；当W满秩后，R＝UV^T；得到最优R后，根据t＝p-Rp'得到最优t；将最优R和最优t带入式(1)中，得到最优匹配误差集，进而得到配准后的虚实耦合模型；

步骤3、将配准后的虚实耦合模型导入到物理引擎(本实施例为Unity3D环境)中，再在物理引擎中根据BIM模型与点云模型的建筑物实体属性对配准后的虚实耦合模型添加包络碰撞体，得到虚实耦合约束环境；碰撞体可应用于虚拟环境下的碰撞检测，是建筑机器人在虚拟空间下进行安全路径规划的必要条件。

第三步、虚实耦合约束环境下的机器人全局路径规划；

步骤1、得到虚实耦合约束环境后，根据第一步得到的建筑物实体的位置信息在二维空间中映射为栅格地图，进行栅格地图构建；

优选地，栅格地图的构建方法具体是：

步骤(1)、移动虚拟空间所有建筑实体到正坐标系：

式8)中，X、Y为建筑物实体坐标，X_min、Y_min为未移动坐标系中的最小坐标；

步骤(2)、构建栅格地图的行数和列数：

式9)中，N_r为栅格地图行数，N_c为栅格地图列数，X_max、Y_max为坐标系移动后的最大坐标，s为步长；当出现未除尽的情况时，对障碍物进行补齐，将补齐的部分视为障碍物；

步骤(3)、根据建筑机器人的轮廓信息进行通过性判断：

设建筑机器人初始轮廓宽度w_r和高度h_r，虚拟环境中门的宽度w_d和高度h_d，σ为安全系数；如果w_r≤σw_d且h_r≤σh_d且真实环境中门的状态为打开，则将该区域映射为无障碍物栅格，其余情况映射为障碍物栅格；

最终得到二维栅格化地图，由栅格M_ij构成：

M_ij＝{M_ij|M_ij＝0,1,2,3} (10)

式10)中，M_ij＝0代表无障碍物栅格，M_ij＝1表示障碍物栅格，M_ij＝2表示起始点栅格，M_ij＝3表示终点栅格；

步骤2、得到栅格地图后，应用改进的A^*算法进行机器人全局路径规划，得到最优规划路径；

优选地，所述改进的A^*算法具体是：

步骤(1)、定义代价函数；

代价函数定义为：

F(n)＝G(n)+H(n) (11)

式11)中，n代表当前节点，F(n)是当前节点n的代价函数，G(n)是移动机器人从初始节点到达节点n的实际代价值，H(n)是从当前节点n到达目标点的代价值，即A^*算法的代价函数；选用Manhattan距离作为启发函数，即：

H(n)＝|n_x-g_x|+|n_y-g_y| (12)

式12)中，(n_x,n_y)为当前节点栅格中心坐标，(g_x,g_y)为终点栅格中心坐标；

步骤(2)、通过定义全局路径搜索代价函数获取规划路径；

步骤①、创建开放集合与闭合集合，并把起始点栅格加入开放集合；

步骤②、遍历开放集合，将F(n)的值最小的节点作为最优节点，或当F(n)的值相同时，将H(n)的值最小的节点作为最优节点；

步骤③、把最优节点移到闭合集合；

步骤④、对最优节点的若干个相邻节点进行判断：

如果该相邻节点是障碍物节点或者在闭合集合中或者l_s-o＜δw_r+s，则忽略该相邻节点；其中，l_s-o为该相邻节点的中心到虚实耦合约束环境中的真实环境障碍物的距离，δ为安全系数，s为步长，如图1所示；

否则，对于不忽略的相邻节点进行如下操作：

如果该相邻节点不在开放集合中，则把该相邻节点加入开放集合，并且把最优节点设置为该相邻节点的父节点，记录该相邻节点的F(n)、G(n)和H(n)的值；

如果该相邻节点已经在开放集合中，采用G(n)的值作为参考来检查由最优节点到该相邻节点的这条路径是否比由该相邻节点的父节点到该相邻节点的路径更好，更小的G(n)值表示路径更好；如果是，则把最优节点设置为该相邻节点的父节点；

步骤⑤、重复步骤②-④，直至终点栅格加入到开放集合中，完成整个搜索过程；

步骤⑥、从终点开始，依次沿着每个父节点移动至起点，得到规划路径；

步骤(3)、根据规划路径，删除冗余转折点后，由剩余节点组成最优规划路径。

优选地，规划路径中存在着大量的冗余转折点，增加了机器人在进行路径跟踪的过程中的转折角度，增加了机器人的运行时间，因此，需要对冗余转折点进行判断并删除。

判断是否为冗余转折点的方法是：设规划路径中节点为n_i，节点的方向向量为

如果n_i与n_i-1为相邻节点且

则n_i为冗余转折点；如果n_i与n_i-1为相邻节点且

若连接n_i+1与n_i-1的直线可通过，则n_i为冗余转折点。

第四步、在最优规划路径的相邻节点之间进行机器人局部避障：根据搭载在机器人上的雷达获取现实环境信息并结合虚实耦合约束环境，再充分考虑机器人的轮廓特征，在最优规划路径的相邻节点之间应用改进的DWA算法进行机器人局部避障。现实环境信息用来防止机器人在局部避障过程中突然出现的障碍物。

具体是：在机器人所能达到的速度窗口(v,w)中存在无穷多组速度，根据移动机器人自身和现实环境信息设定移动机器人采样速度的限制条件，再根据采样速度的限制条件筛选出符合条件的若干组速度，并得到机器人在这些速度下Δt时间间隔内的各自的轨迹；现有方法中，由于受建筑物遮挡的影响，导致机器人只能根据获取的局部信息进行速度评价；因此，本方法通过现实环境信息并结合虚实耦合约束环境，再考虑机器人的轮廓特征，在最优规划路径的相邻节点之间应用改进的DWA算法的安全评价函数G(v,w)筛选出最优轨迹对应的速度v,w来控制机器人的运动，进行局部避障，进而实现建筑机器人的自主导航。

优选地，采样速度的限制条件具体是：

限制条件一、移动机器人本体速度约束：

V_m＝{v∈[v_min,v_max],w∈[w_min,w_max]} (13)

式13)中，v_min,v_max分别为机器人的最小和最大线速度，w_min,w_max分别为机器人的最小和最大角速度；

限制条件二、移动机器人加速度约束：

式14)中，v_c,w_c分别为移动机器人当前的线速度和角速度，

分别为线速度的最大

加速度和最大减速度，

分别为角速度的最大加速度和最大减速度；

限制条件三、移动机器人现实环境中安全距离约束：

式15)中，dist(v,w)为速度窗口(v,w)对应轨迹上移动机器人与障碍物之间的最近距离。

优选地，速度对应的相应轨迹由式16)得出；机器人在时间间隔Δt内运动的运动学模型为：

式16)中，Δt为时间间隔；x’为机器人坐标系纵向坐标，y’为机器人坐标系横向坐标，x为Δt时间后机器人坐标系横向坐标，y为Δt时间后机器人坐标系横向坐标，θ_t为机器人偏航角，v,w为速度窗口(v,w)中的一组速度。

优选地，改进的DWA算法的安全评价函数G(v,w)具体是：如图2所示，设雷达(优选激光雷达)的最大测量范围为d_max，视场为[φ_min,φ_max]，角度分辨率为Δφ，机器人航向对应的扫描角度为φ_rob；每次扫描后，得到的测量距离为{d₁,d₂,d₃,...,d_k}，测量角度为φ_k＝φ_min+(k-1)Δφ，测量到障碍物边缘的距离和角度分别为d_j和φ_j，根据已知的建筑机器人轮廓参数r_rob对障碍物边缘进行扩展，扩展角度θ_j＝arctan(r_rob/d_j)；

设可通过的角度区间的区间中点为φ_mid，当前轨迹对应的角度为φ_rob，以区间[φ₁+θ₁,φ₂-θ₂]为例，其现实环境中安全角度评价函数为：

以雷达所在位置为起点，雷达搭载在机器人上，在φ_{i_o}方向到虚实耦合约束环境中障碍物的距离为l_{i_o}，该方向与相邻障碍物的夹角为θ_{i_o}；l_{i_o}反应了φ_{i_o}方向在虚实耦合约束环境中的安全距离评价，θ_{i_o}反应了φ_{i_o}方向在虚实耦合约束环境中的安全角度评价，d_σ为虚实耦合约束环境下安全距离阈值，则虚实耦合约束环境中安全距离评价函数为：

虚实耦合约束环境中角度的评价函数为：

a(φ_{i_o})＝θ_{i_o} (19)

虚实耦合约束环境中安全评价函数为：

couple(v,w,φ_{i_o})＝μ·a(v,w,φ_{i_o})+ν·l(v,w,φ_{i_o}) (20)

式20)中，μ,ν为安全评价系数；

因此，结合虚实耦合约束环境得到改进的DWA算法的安全评价函数为：

式21)中，heading(v,w)为方向角评价函数，表示移动机器人在当前的采样速度下，达到模拟轨迹末端时的朝向和目标之间的角度差距；velocity(v,w)为当前模拟速度的大小；angle(v,w,φ_{i_o})为现实环境中安全角度评价函数；couple(v,w,φ_{i_o})为虚实耦合约束环境中安全评价函数，ε为平滑函数；α,β,γ,ω,χ为加权系数。

实施例1

以河北工业大学红桥校区机械楼为实验场景，机械楼BIM模型中三楼的建筑物实体的位置信息如图3所示。选择构建机器人导航虚拟空间所必须的“墙”、“门”、“窗”三种构件进行解析。

根据第二步的步骤2中的基于模型边界点的匹配方法，对BIM模型中的8个边界点与点云模型中的8个边界点进行匹配，经过匹配后得出其最优匹配误差集中的最大误差为e_max＝0.01018m。

根据红桥校区机械楼三楼的建筑物实体的位置信息进行栅格地图构建，应用改进后的A^*算法进行全局路径规划，得到的全局路径为建筑机器人预留了足够的安全距离，并根据真实环境中门的通过性进行判断，删除冗余转折点后得到最优规划路径。经过优化后，最优规划路径搜索时间由115.83s减少为8.64s，节点数由137减少为6，转折角由540°减少为270°。

得到全局路径后，在改进的A^*算法的节点间应用改进的DWA算法进行局部路径避障，由于建筑机器人结合了现实环境信息与虚实耦合约束环境信息，改进的DWA算法可绕过局部最小陷阱，路径长度比原始DWA算法减少13.5％，对比无障碍物时的路径长度仅增加了3.7％。

本发明未述及之处适用于现有技术。

Claims (8)

1.一种虚实耦合约束环境下的机器人自主导航方法，其特征在于，该方法包括以下步骤：

第一步、虚拟空间的搭建：建立建筑物的BIM模型，再对BIM模型的信息进行提取，得到建筑物实体的位置信息；建筑物实体的位置信息和BIM模型构成虚拟空间；

第二步、虚实耦合约束环境的构建；

步骤1、由机器人得到周围环境的点云模型；

步骤2、根据第一步中得到的建筑物实体的位置信息，基于模型边界点的匹配方法将BIM模型中的边界点与点云模型中对应的边界点进行匹配，得到配准后的虚实耦合模型；

步骤3、将配准后的虚实耦合模型导入到物理引擎中，再在物理引擎中根据BIM模型与点云模型的建筑物实体属性对配准后的虚实耦合模型添加包络碰撞体，得到虚实耦合约束环境；

第三步、虚实耦合约束环境下的机器人全局路径规划；

步骤1、得到虚实耦合约束环境后，根据第一步得到的建筑物实体的位置信息进行栅格地图构建；

步骤2、得到栅格地图后，应用改进的A^*算法进行机器人全局路径规划，得到最优规划路径；

第四步、获取现实环境信息并结合虚实耦合约束环境，再考虑机器人的轮廓特征，在最优规划路径的相邻节点之间应用改进的DWA算法进行机器人局部避障；

第四步具体是：在机器人所能达到的速度窗口(v,w)中存在无穷多组速度，设定机器人采样速度的限制条件，再根据采样速度的限制条件筛选出符合条件的若干组速度，并得到机器人在这些速度下Δt时间间隔内的各自的轨迹；再通过现实环境信息并结合虚实耦合约束环境，再考虑机器人的轮廓特征，在最优规划路径的相邻节点之间应用改进的DWA算法的安全评价函数G(v,w)筛选出最优轨迹对应的速度v,w来控制机器人的运动，进行局部避障；

改进的DWA算法的安全评价函数G(v,w)具体是：设雷达的最大测量范围为d_max，视场为[φ_min,φ_max]，角度分辨率为Δφ，机器人航向对应的扫描角度为φ_rob；每次扫描后，得到的测量距离为{d₁,d₂,d₃,...,d_k}，测量角度为φ_k＝φ_min+(k-1)Δφ，测量到障碍物边缘的距离和角度分别为d_j和φ_j，根据已知的机器人轮廓参数r_rob对障碍物边缘进行扩展，扩展角度θ_j＝arctan(r_rob/d_j)；

设可通过的角度区间的区间中点为φ_mid，当前轨迹对应的角度为φ_rob，以区间[φ₁+θ₁,φ₂-θ₂]为例，其现实环境中安全角度评价函数为：

以雷达所在位置为起点，雷达搭载在机器人上，在φ_{i_o}方向到虚实耦合约束环境中障碍物的距离为l_{i_o}，该方向与相邻障碍物的夹角为θ_{i_o}；l_{i_o}反应了φ_{i_o}方向在虚实耦合约束环境中的安全距离评价，θ_{i_o}反应了φ_{i_o}方向在虚实耦合约束环境中的安全角度评价，d_σ为虚实耦合约束环境下安全距离阈值，则虚实耦合约束环境中安全距离评价函数为：

虚实耦合约束环境中角度的评价函数为：

a(φ_{i_o})＝θ_{i_o} (19)

虚实耦合约束环境中安全评价函数为：

couple(v,w,φ_{i_o})＝μ·a(v,w,φ_{i_o})+ν·l(v,w,φ_{i_o}) (20)

式20)中，μ,ν为安全评价系数；

因此，结合虚实耦合约束环境得到改进的DWA算法的安全评价函数为：

式21)中，heading(v,w)为方向角评价函数，表示机器人在当前的采样速度下，达到模拟轨迹末端时的朝向和目标之间的角度差距；velocity(v,w)为当前模拟速度的大小；angle(v,w,φ_{i_o})为现实环境中安全角度评价函数；couple(v,w,φ_{i_o})为虚实耦合约束环境中安全评价函数，ε为平滑函数；α,β,γ,ω,χ为加权系数。

2.根据权利要求1所述的虚实耦合约束环境下的机器人自主导航方法，其特征在于，第一步中，显示建筑物实体的位置信息的方法是：将提取BIM模型信息时所用的代码封装为动态链接库，应用软件调用动态链接库并制作提取界面，显示建筑物实体的位置信息。

3.根据权利要求1所述的虚实耦合约束环境下的机器人自主导航方法，其特征在于，第二步中，所述基于模型边界点的匹配方法具体是：

设BIM模型边界点集合为P＝{p₁,...,p_i,...,p_m}，点云模型边界点集合为P'＝{p'₁,...,p'_i,...,p'_m}，其中

N为自然数集，由p′_i到p_i的欧式变换R,t表示为

R为旋转矩阵，t为平移矩阵；

但由于BIM模型是一种理想的虚拟模型，与实际的建筑物存在差异，则第i个BIM模型边界点p_i与第i个点云模型边界点p′_i的匹配误差表示为：

e_i＝p_i-(Rp′_i+t) (1)

将式(1)构建为最小二乘问题的表达式，求使匹配误差平方和达到极小值时的R,t如式(2)所示：

设BIM模型边界点集合的质心和点云模型边界点集合的质心的表达式分别为：

根据式(2)，得到匹配误差函数的表达式如式(3)所示：

由式(3)展开以及质心表达式得：

由于(p_i-p-R(p′_i-p'))在求和后等于零，则式(4)可简化为：

式5)的两项中，第一项只和R相关，第二项既有R又有t，但第二项只和质心相关，因此只要使用第一项求得R，令第二项为零即可求得t；

根据式(5)可以得出：

t＝p-Rp'，其中，q_i为BIM模型边界点的去质心坐标，q′_i为点云模型边界点的去质心坐标，q_i＝p_i-p，q′_i＝p′_i-p'；

根据式(5)以及q_i和q′_i对式(5)的第一项进行展开：

由于式(6)的等号右边中只有第三项与R有关，实际上式(6)可表示为：

式7)中，tr表示矩阵的迹；

应用奇异值分解方法求解出式(7)中的最优R；得到最优R后，根据t＝p-Rp'得到最优t；将最优R和最优t带入式(1)中，得到匹配误差集，进而得到配准后的虚实耦合模型。

4.根据权利要求3所述的虚实耦合约束环境下的机器人自主导航方法，其特征在于，所述奇异值分解方法具体是：定义矩阵

对W进行奇异值分解，得W＝UΣV^T，其中，Σ为奇异值组成的对角矩阵，U和V为对角矩阵；当W满秩后，R＝UV^T。

5.根据权利要求1所述的虚实耦合约束环境下的机器人自主导航方法，其特征在于，第三步的步骤1中，栅格地图的构建方法具体是：

步骤(1)、移动虚拟空间所有建筑实体到正坐标系：

式8)中，X、Y为建筑物实体坐标，X_min、Y_min为未移动坐标系中的最小坐标；

步骤(2)、构建栅格地图的行数和列数：

式9)中，N_r为栅格地图行数，N_c为栅格地图列数，X_max、Y_max为坐标系移动后的最大坐标，s为步长；当出现未除尽的情况时，对障碍物进行补齐，将补齐的部分视为障碍物；

步骤(3)、根据机器人的轮廓信息进行通过性判断：

设机器人初始轮廓宽度w_r和高度h_r，虚拟环境中门的宽度w_d和高度h_d，σ为安全系数；如果w_r≤σw_d且h_r≤σh_d且真实环境中门的状态为打开，则将该区域映射为无障碍物栅格，其余情况映射为障碍物栅格；

最终得到二维栅格化地图，由栅格M_ij构成：

M_ij＝{M_ij|M_ij＝0,1,2,3} (10)

式10)中，M_ij＝0代表无障碍物栅格，M_ij＝1表示障碍物栅格，M_ij＝2表示起始点栅格，M_ij＝3表示终点栅格。

6.根据权利要求1所述的虚实耦合约束环境下的机器人自主导航方法，其特征在于，第三步的步骤2中，所述改进的A^*算法具体是：

步骤(1)、定义代价函数；

代价函数定义为：

F(n)＝G(n)+H(n) (11)

式11)中，n代表当前节点，F(n)是节点n的代价函数，G(n)是机器人从初始节点到达节点n的实际代价值，H(n)是从节点n到达目标点的代价值；选用Manhattan距离作为启发函数，即：

H(n)＝|n_x-g_x|+|n_y-g_y| (12)

式12)中，(n_x,n_y)为当前节点栅格中心坐标，(g_x,g_y)为终点栅格中心坐标；

步骤(2)、通过定义全局路径搜索代价函数获取规划路径；

步骤(3)、根据规划路径，删除冗余转折点后，由剩余节点组成最优规划路径。

7.根据权利要求6所述的虚实耦合约束环境下的机器人自主导航方法，其特征在于，第三步的步骤2的步骤(2)具体是：

步骤①、创建开放集合与闭合集合，并把起始点栅格加入开放集合；

步骤②、遍历开放集合，将F(n)的值最小的节点作为最优节点，或当F(n)的值相同时，将H(n)的值最小的节点作为最优节点；

步骤③、把最优节点移到闭合集合；

步骤④、对最优节点的若干个相邻节点进行判断：

如果该相邻节点是障碍物节点或者在闭合集合中或者l_s-o＜δw_r+s，则忽略该相邻节点；其中，l_s-o为该相邻节点的中心到虚实耦合约束环境中的真实场景障碍物的距离，δ为安全系数，s为步长，如图2所示；

否则，对于不忽略的相邻节点进行如下操作：

如果该相邻节点不在开放集合中，则把该相邻节点加入开放集合，并且把最优节点设置为该相邻节点的父节点，记录该相邻节点的F(n)、G(n)和H(n)的值；

如果该相邻节点已经在开放集合中，采用G(n)的值作为参考来检查由最优节点到该相邻节点的这条路径是否比由该相邻节点的父节点到该相邻节点的路径更好，更小的G(n)值表示路径更好；如果是，则把最优节点设置为该相邻节点的父节点；

步骤⑤、重复步骤②-④，直至终点栅格加入到开放集合中，完成整个搜索过程；

步骤⑥、从终点开始，依次沿着每个父节点移动至起点，得到规划路径。

8.根据权利要求6所述的虚实耦合约束环境下的机器人自主导航方法，其特征在于，第三步的步骤2的步骤(3)中，判断是否为冗余转折点的方法是：设规划路径中节点为n_i，节点的方向向量为

如果n_i与n_i-1为相邻节点且

则n_i为冗余转折点；如果n_i与n_i-1为相邻节点且

若连接n_i+1与n_i-1的直线可通过，则n_i为冗余转折点。

Images