CN114578824B - 一种适用于空地两用机器人的未知环境自主探索方法 - Google Patents

Abstract

一种适用于空地两用机器人的未知环境自主探索方法，涉及机器人环境探索技术领域，用以解决现有未知环境自主探索方法因仅考虑单一运动模态导致对环境探索的覆盖率、节能性、快速性不足的问题。本发明技术要点包括：获取未知环境的三维空间范围，以空地两用机器人当前位置为探索起点，对深度数据和姿态数据进行处理以更新栅格地图，进而更新待探索点集；基于度量函数在待探索点集中选择最优待探索点；对根据空地两用机器人当前位置和最优待探索点生成的全局路径进行优化，使得空地两用机器人沿优化后的全局路径到达最优待探索点。本发明使得机器人在对未知环境探索时探索速度显著提高，探索能耗显著降低，在实际工程中能实现更优的探索效果。

Description

一种适用于空地两用机器人的未知环境自主探索方法

技术领域

本发明涉及机器人环境探索技术领域，具体涉及一种适用于空地两用机器人的未知环境自主探索方法。

背景技术

未知环境自主探索是指机器人在没有任何先验信息的情况下，通过自主移动获得待探索环境的空间结构以及障碍物分布等信息，从而完整构建环境地图的过程。未知环境自主探索技术综合体现了机器人的感知能力、自主决策能力和环境适应能力，在地下空间探测、城市灾后搜救、林业防灾减灾等任务场景有着广泛的应用。

目前应用于自主探索任务的机器人主要分为地面机器人和空中机器人两类。其中地面机器人(如轮式、多足式、履带式机器人等)续航时间长，稳定性高，但其通行能力存在瓶颈，在障碍密集环境下运动能力有限，且无法通过高空作业执行任务。而空中机器人虽然具备高空作业能力，且具有较高的移动速度，但飞行状态下机器人耗能较高，导致平台续航时间较短。与上述两类机器人相比，空间可达性高、越障能力强的空地两用机器人能在一定程度上兼具地面机器人及空中机器人的优势，从而更好地完成探索任务。

现阶段未知环境自主探索采用的主流方法是将目标环境中的已探索区域和未探索区域的交界定义为边界，通过综合评价每个边界的探索代价和信息增益，以此生成环境探索的候选目标点，并引导机器人向目标点移动，机器人通过不断到达一系列目标点以逐步获取整个环境的信息。但传统的未知环境自主探索方法仅针对单一运动模态，机器人在目标点间运动时仅能选取地面运动或空中运动中的一种进行，导致传统方法在实际的工程应用中，尤其是对障碍密集环境的探索，难以获取完整的空间信息，且任务执行过程中的耗能与耗时较大。

发明内容

鉴于以上问题，本发明提出一种适用于空地两用机器人的未知环境自主探索方法，用以解决现有未知环境自主探索方法因仅考虑单一运动模态而产生的对环境探索的覆盖率、节能性、快速性不足的问题。

本发明提供一种适用于空地两用机器人的未知环境自主探索方法，该方法包括以下步骤：

步骤一、获取未知环境的三维空间范围，并以所述三维空间中任意一点为原点，建立三维坐标系，对三维空间范围内未知区域及由未知点组成的待探索点集进行初始化；其中，所述三维坐标系的坐标轴方向与世界坐标系相同；

步骤二、以空地两用机器人当前位置为探索起点，获取空地两用机器人携带的深度传感器和惯性测量单元所采集的深度数据和姿态数据，并对所述深度数据和姿态数据进行处理以更新所述三维空间对应的栅格地图，进而更新待探索点集；

步骤三、基于度量函数在待探索点集中选择最优待探索点；

步骤四、对根据空地两用机器人当前位置和最优待探索点生成的全局路径进行优化，使得空地两用机器人沿优化后的全局路径到达最优待探索点；

步骤五、重复执行步骤二至步骤四，直至待探索点集中的未知点达到预设阈值则停止重复执行，空地两用机器人完成对未知环境的自主探索。

进一步地，步骤一中未知区域初始化为E_u＝E，E表示未知环境的三维空间；待探索点集初始化为：

P_G＝{(x,y,z)|x＝k₁α,y＝k₂α,z＝k₃α,(x,y,z)∈E,k₁∈Z,k₂∈Z,k₃∈Z}

式中，Z表示整数；k₁、k₂、k₃为任意整数；α为固定阈值，代表待探索点彼此间的分布距离。

进一步地，步骤二的具体步骤包括：

步骤二一、根据所述深度数据和所述姿态数据通过坐标变换获取空地两用机器人当前位置P_B；对所述深度数据进行处理，获取点云集合P_W；

步骤二二、获取所述点云集合P_W中每个点膨胀后的点集P_W+，具体为：设置与空地两用机器人当前位置P_B的欧式距离为第一固定阈值的空间S_n，当该点在所述三维空间E∩S_n的区域内时，将该点沿x、y、z三轴方向进行膨胀，膨胀后的点集P_W+为：

P_W+＝{(x,y,z)|x₀-ε≤x<x₀+ε,y₀-ε≤y<y₀+ε,z₀-δ≤z<z₀+δ}

式中，(x,y,z)表示膨胀后的点集P_W+中点的三维坐标；(x₀,y₀,z₀)表示点云集合P_W中任意一点的三维坐标；ε、δ表示点膨胀的距离；

步骤二三、从每个膨胀后的点集P_W+中去除在所述三维空间E∩S_n的区域之外的点，然后将所有膨胀后的点集P_W+中剩余的点组合形成包含局部障碍信息的局部点云集合C_p；

步骤二四、在所述三维空间对应的栅格地图上，将所述局部点云集合C_p中的点赋值属性为占据，更新局部栅格地图M_p和全局栅格地图M_g，随后更新已知区域E_k和未知区域E_u，进而更新待探索点集P_G。

进一步地，步骤二四在更新已知区域E_k和未知区域E_u时，将局部栅格地图M_p中满足下述公式的部分点P₁加入已知区域E_k中，并从未知区域E_u中移除：

P₁＝{P_B+k₄(P_O-P_B)|0<k₄<1.2}

式中，P_O表示局部栅格地图M_p中被赋值为占据的点；

将局部栅格地图M_p中满足下述公式的部分点P₂加入已知区域E_k中，并从未知区域E_u中移除：

式中，表示不在区域S_n内且在点云集合P_W内的点，即/>

进一步地，步骤二四在对未知区域E_u移除部分点P₁和P₂后，对局部栅格地图M_p中的未知区域，即M_p∩E_u中的点进行聚类，将获得的多个聚类中心组成候选待探索点集P_D，并记录每个聚类簇中未知点的数量；然后依据下述顺序在候选待探索点集P_D中选择保留部分点加入待探索点集：1)预设第二固定阈值，对于所有候选待探索点集P_D中的点，若其与待探索点集P_G中任意一点的距离均大于所述预设第二固定阈值，则将候选待探索点集P_D中的该点加入待探索点集P_G；2)预设第三固定阈值，对于所有候选待探索点集P_D中的点若其与未知区域E_u中任意一点的距离均大于所述预设第三固定阈值，则将其从待探测点集P_G中移除。

进一步地，步骤三的具体步骤包括：

步骤三一、将待探索点集P_G中每个待探索点作为目标终点，计算其对应的度量函数，其中，度量函数的计算公式为：

F＝α_FG_T-β_FL_W+γ_FG_W

式中，α_F、β_F、γ_F为选定的系数，分别表示目标点信息增益值、空地混合路径惩罚项、途经信息增益值对度量函数的权重；G_T为目标点信息增益值，表示空地两用机器人在目标点附近能够感知到的未知环境信息量；L_W为空地混合路径惩罚项，表示空地两用机器人从当前位置去往目标终点过程中的路径消耗；G_W为途经信息增益值，表示空地两用机器人从当前位置去往目标终点过程中感知到的未知环境信息量；

步骤三二、选取度量函数值最大对应的待探索点为当前时刻的目标终点，即最优待探索点，并将其移出待探索点集。

进一步地，步骤三一中所述目标点信息增益值G_T的值等于该待探索点所属的聚类簇所包含未知点的数量；所述空地混合路径惩罚项L_W的值通过耗能与时间加权表示；所述途经信息增益值G_W的值等于空地两用机器人从当前位置去往目标终点过程中所经过的未知区域E_u与管状区域Φ的交空间E_u∩Φ所包含的点的数量；其中，管状区域Φ的定义为：

式中，Γ表示空地两用机器人从当前位置去往目标终点的全局路径；β_Γ表示预设第四固定阈值；P表示管状区域内的点的坐标。

进一步地，步骤三一中所述空地混合路径惩罚项L_W的具体计算公式为：

L_W＝β_EP_E+β_TP_T

式中，P_E为耗能惩罚项，P_T为时间惩罚项，β_E、β_T为权重；

式中，β_EA、β_EF、β_TA、β_TF为根据空地两用机器人结构及运动特性设计的权值；表示空地两用机器人从当前位置去往目标终点的全局路径中的空中路径的长度；/>表示空地两用机器人从当前位置去往目标终点的全局路径中的地面路径的长度。

进一步地，步骤四中对全局路径进行优化，其优化的目标函数表示为：

Q＝arg min J＝λ_sJ_s+λ_cJ_c+λ_dJ_d+λ_IJ_I+λ_hJ_h

式中，J_s为光滑项，J_c为碰撞项，J_d为动力学可行项，J_I为探索项，J_h为空地模态项，λ_s、λ_c、λ_d、λ_I、λ_h为每一个惩罚项对应的权值。

进一步地，步骤四中空地两用机器人沿优化后的全局路径到达最优待探索点的过程中，通过小幅调整偏航角以扩大空地两用机器人的探索范围，具体包括：

对于优化后的全局路径上的两个点，设置起点终点偏航角φ_k满足下述公式：

式中，α_k、β_k为可改变的系数，k△t表示起点到终点/>的时间间隔。

本发明的有益技术效果是：

本发明提出一种适用于空地两用机器人的未知环境自主探索方法，相较于地面机器人，本发明利用空地模态切换提高了平台可达性，有效缩短了机器人探索过程中的通行路径长度，从而提升了机器人的探索覆盖率和探索速度；相较于空中机器人，本发明利用平台地面运动的能力减少了机器人空中飞行时间，从而有效降低平台能耗，在同等尺寸、重量下，提升了机器人所能探索的最大空间范围。本发明结合了地面机器人及空中机器人的运动特点，针对空地两用机器人的自主探索优势，设计了一种具备空地切换能力的未知环境自主探索方法，提高探索速度的同时降低了探索能耗，在工程应用中能够实现相较传统机器人更优的自主探索效果。

附图说明

本发明可以通过参考下文中结合附图所给出的描述而得到更好的理解，所述附图连同下面的详细说明一起包含在本说明书中并且形成本说明书的一部分，而且用来进一步举例说明本发明的优选实施例和解释本发明的原理和优点。

图1为本发明实施例一种适用于空地两用机器人的未知环境自主探索方法的流程图。

图2为本发明实施例中一种空地两用机器人的结构示意图。

图3为本发明实施例中空地两用机器人路径规划效果示例图。

图4为本发明实施例中空地两用机器人与其他机器人的实验探索效果对比图；其中，a)为空中机器人的探索效果，b)为地面机器人的探索效果，c)为空地两用机器人的探索效果，d)为实验测试场景。

图5为本发明实施例中空地两用机器人与其他机器人的实验能耗对比图。

具体实施方式

为了使本技术领域的人员更好地理解本发明方案，在下文中将结合附图对本发明的示范性实施方式或实施例进行描述。显然，所描述的实施方式或实施例仅仅是本发明一部分的实施方式或实施例，而不是全部的。基于本发明中的实施方式或实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施方式或实施例，都应当属于本发明保护的范围。

基于空地两用机器人所具有的良好的空间可达性及长续航优势，本发明设计了一种结合空地两用机器人平台运动特点的未知环境自主探索方法，该方法能够有效提升未知环境中空地两用机器人的自主探索速度及续航能力。

本发明实施例提供一种适用于空地两用机器人的未知环境自主探索方法，该方法包括以下步骤：

步骤一、获取未知环境的三维空间范围，并以三维空间中任意一点为原点，建立三维坐标系，对三维空间范围内未知区域及由未知点组成的待探索点集进行初始化；其中，三维坐标系的坐标轴方向与世界坐标系相同；

步骤二、以空地两用机器人当前位置为探索起点，获取空地两用机器人携带的深度传感器和惯性测量单元所采集的深度数据和姿态数据，并对深度数据和姿态数据进行处理以更新三维空间对应的栅格地图，进而更新待探索点集；

步骤三、基于度量函数在待探索点集中选择最优待探索点；

步骤四、对根据空地两用机器人当前位置和最优待探索点生成的全局路径进行优化，使得空地两用机器人沿优化后的全局路径到达最优待探索点；

步骤五、重复执行步骤二至步骤四，直至待探索点集中的未知点达到预设阈值则停止重复执行，空地两用机器人完成对未知环境的自主探索。

本实施例中，可选地，步骤一中未知区域初始化为E_u＝E，E表示未知环境的三维空间；待探索点集初始化为：

P_G＝{(x,y,z)|x＝k₁α,y＝k₂α,z＝k₃α,(x,y,z)∈E,k₁∈Z,k₂∈Z,k₃∈Z}

式中，Z表示整数；k₁、k₂、k₃为任意整数；α为固定阈值，代表待探索点彼此间的分布距离。

本实施例中，可选地，步骤二的具体步骤包括：

步骤二一、根据深度数据和姿态数据通过坐标变换获取空地两用机器人当前位置P_B；对深度数据进行处理，获取点云集合P_W；

步骤二二、获取点云集合P_W中每个点膨胀后的点集P_W+，具体为：设置与空地两用机器人当前位置P_B的欧式距离为第一固定阈值(该阈值的取值范围依据深度传感器的传感能力决定，取值范围在3-10m，优选地，可取5m)的空间S_n，当该点在三维空间E∩S_n的区域内时，将该点沿x、y、z三轴方向进行膨胀，膨胀后的点集P_W+为：

P_W+＝{(x,y,z)|x₀-ε≤x<x₀+ε,y₀-ε≤y<y₀+ε,z₀-δ≤z<z₀+δ}

式中，(x,y,z)表示膨胀后的点集P_W+中点的三维坐标；(x₀,y₀,z₀)表示点云集合P_W中任意一点的三维坐标；ε、δ表示点膨胀的距离，取值范围在2-10m之间，优选地，ε取5，δ取3。

步骤二三、从每个膨胀后的点集P_W+中去除在三维空间E∩S_n的区域之外的点，然后将所有膨胀后的点集P_W+中剩余的点组合形成包含局部障碍信息的局部点云集合C_p；

步骤二四、在三维空间对应的栅格地图上，将局部点云集合C_p中的点赋值属性为占据，更新局部栅格地图M_p和全局栅格地图M_g，随后更新已知区域E_k和未知区域E_u，进而更新待探索点集P_G。

本实施例中，可选地，步骤二四在更新已知区域E_k和未知区域E_u时，将局部栅格地图M_p中满足下述公式的部分点P₁加入已知区域E_k中，并从未知区域E_u中移除：

P₁＝{P_B+k₄(P_O-P_B)|0<k₄<1.2}

式中，P_O表示局部栅格地图M_p中被赋值为占据的点；

将局部栅格地图M_p中满足下述公式的部分点P₂加入已知区域E_k中，并从未知区域E_u中移除：

式中，表示不在区域S_n内且在点云集合P_W内的点，即/>

本实施例中，可选地，步骤二四在对未知区域E_u移除部分点P₁和P₂后，对局部栅格地图M_p中的未知区域，即M_p∩E_u中的点进行聚类，将获得的多个聚类中心组成候选待探索点集P_D，并记录每个聚类簇中未知点的数量；然后依据下述顺序在候选待探索点集P_D中选择保留部分点加入待探索点集：1)预设第二固定阈值(取值范围为0.5m-3m,优选地，可取1m)，对于所有候选待探索点集P_D中的点，若其与待探索点集P_G中任意一点的距离均大于预设第二固定阈值，则将候选待探索点集P_D中的该点加入待探索点集P_G；2)预设第三固定阈值(取值范围为2m-10m，优选地，可取3m)，对于所有候选待探索点集P_D中的点，若其与未知区域E_u中任意一点的距离均大于预设第三固定阈值，则将其从待探测点集P_G中移除。

本实施例中，可选地，步骤三的具体步骤包括：

步骤三一、将待探索点集P_G中每个待探索点作为目标终点，计算其对应的度量函数，其中，度量函数的计算公式为：

F＝α_FG_T-β_FL_W+γ_FG_W

式中，G_T为目标点信息增益值，表示空地两用机器人在目标点附近能够感知到的未知环境信息量；L_W为空地混合路径惩罚项，表示空地两用机器人从当前位置去往目标终点过程中的路径消耗；G_W为途经信息增益值，表示空地两用机器人从当前位置去往目标终点过程中感知到的未知环境信息量；α_F、β_F、γ_F为选定的系数，分别表示目标点信息增益值、空地混合路径惩罚项、途经信息增益值对度量函数的权重，系数取值范围均为0-1之间，优选地，可分别取0.4、0.2、0.4；

步骤三二、选取度量函数值最大对应的待探索点为当前时刻的目标终点，即最优待探索点，并将其移出待探索点集。

本实施例中，可选地，步骤三一中目标点信息增益值G_T的值等于该待探索点所属的聚类簇所包含未知点的数量；空地混合路径惩罚项L_W的值通过耗能与时间加权表示；途经信息增益值G_W的值等于空地两用机器人从当前位置去往目标终点过程中所经过的未知区域E_u与管状区域Φ的交空间E_u∩Φ所包含的点的数量；其中，管状区域Φ的定义为：

式中，Γ表示空地两用机器人从当前位置去往目标终点的全局路径；β_Γ表示预设第四固定阈值；P表示管状区域内的点的坐标。

本实施例中，可选地，步骤三一中空地混合路径惩罚项L_W的具体计算公式为：

L_W＝β_EP_E+β_TP_T

式中，P_E为耗能惩罚项，P_T为时间惩罚项，β_E、β_T为权重，取值范围均为0-1，优选地，可分别取值0.5、0.5；

式中，β_EA、β_EF、β_TA、β_TF为根据空地两用机器人结构及运动特性设计的权值，取值范围均为0-1，优选地，可分别取值0.85、0.15、0.2、0.8；表示空地两用机器人从当前位置去往目标终点的全局路径中的空中路径的长度；/>表示空地两用机器人从当前位置去往目标终点的全局路径中的地面路径的长度。

本实施例中，可选地，步骤四中对全局路径进行优化，其优化的目标函数表示为：

Q＝arg min J＝λ_sJ_s+λ_cJ_c+λ_dJ_d+λ_IJ_I+λ_hJ_h

式中，J_s为光滑项，J_c为碰撞项，J_d为动力学可行项，J_I为探索项，J_h为空地模态项，λ_s、λ_c、λ_d、λ_I、λ_h为每一个惩罚项对应的权值，取值均大于0，优选地，可分别取值10，25，25，25，15。

本实施例中，可选地，步骤四中空地两用机器人沿优化后的全局路径到达最优待探索点的过程中，通过小幅调整偏航角以扩大空地两用机器人的探索范围，具体包括：

对于优化后的全局路径上的两个点，设置起点终点偏航角φ_k满足下述公式：

式中，α_k、β_k为可改变的系数，α_k取值范围为0-1，优选地，可取值0.2，β_k取值范围为0-π/4,优选地，可取值π/8；k△t表示起点到终点/>的时间间隔。

本发明另一实施例提供一种适用于空地两用机器人的未知环境自主探索方法，如图1所示，该方法包括以下步骤：

步骤一：选取空地两用机器人平台，所述的空地两用机器人平台具备空中运动与地面运动的能力；建立三维坐标系，确定机器人所需探索的三维空间范围，将空地两用机器人放置于所需探索的空间范围内，记录机器人的探索起点。

根据本发明实施例，空地两用机器人应搭载主控CPU，用于提供算力需求；搭载运动控制板，用于空地两用机器人的底层控制；搭载双目相机或激光雷达，用于作为深度传感器获取机器人附近的环境信息，以供空地两用机器人进行同时定位与建图，更优的，由于相较地面机器人及空中机器人，空地两用机器人能在更大的空间范围内运动，为更好的同时捕捉地面及空中的环境信息，深度传感器的数量可以是多个；搭载惯性测量单元，用于获取空地两用机器人当前姿态及加速度，并配合其他传感器实现机器人实时定位；此外，所述的空地两用机器人通过机械设计具备空中运动及地面运动的能力，并能通过运动控制实现两种运动模式之间的自主切换。其一种可行的构型如图2所示，通过旋翼推动实现空中运动，通过旋翼带动轮式结构实现地面运动，需要注意的是，该构型仅为空地两用机器人可行的构型之一，并不构成对本发明适用范围的限制。

其次，进行自主探测任务的初始化，设置空地两用机器人需要探索的三维空间，三维空间的范围表示为：

其中，E代表三维空间的范围，E_i代表三维空间包含的第i个子空间， 为第i个子空间在x、y、z三个方向上的上下界。未知区域初始化为E_u＝E，已知区域初始化为/>待探索点集初始化为：

P_G＝{(x,y,z)|x＝k₁α,y＝k₂α,z＝k₃α,(x,y,z)∈E,k₁∈Z,k₂∈Z,k₃∈Z} (2)

其中，α为固定阈值，代表待探索点彼此间的分布距离。

选取三维空间E中的一点作为原点，建立互相垂直的三轴坐标系，将空地两用机器人放置于三维空间E中的任意一点，记录探索起点坐标为其初始姿态与三维空间坐标系三轴正方向欧拉角差值为(ψ,θ,φ)，则姿态对应的旋转矩阵/>为：

据此可得从世界坐标系到起点坐标系的位姿变换矩阵

步骤二：根据传感信息更新栅格地图，并根据栅格地图更新待探索点集。

根据本发明实施例，首先，获取当前时刻空地两用机器人的世界坐标系位姿，对深度传感器所获取的深度数据进行预处理生成点云，并进一步生成局部栅格地图对全局地图进行更新；其次，根据局部栅格地图，确定与其相关的未知地图区域并聚类为若干个待探索点，若该待探索点距离任意已存储的待探索点的欧式距离小于一定阈值，则将其移出待探索点集；最后，对待探索点集进行更新，更新规则为：若距待探索点一定距离内均为已知地图区域，则将该待探索点移出待探索点集。具体如下。

空地两用机器人通过双目相机获取左右相机的图像，结合惯性测量单元输出的数据，通过双目视觉惯导SLAM方案VINS-FUSION获取当前时刻空地两用机器人相较于起点的相对位置及采用四元数形式表示的相对姿态/>则此时用旋转矩阵表示的相对姿态/>为：

则从起点坐标系到机身坐标系的位姿变换矩阵可表示为此外，根据结构光相机的安装位置及角度，可以获得从机身坐标系到相机坐标系的位姿变换矩阵通过标定好的相机内参，获取相机坐标系到图像坐标系的位姿变换矩阵/>由此可得用齐次坐标表示的图像坐标系中的一点P_I＝(u,v,z,1)与对应的世界坐标系中的点P_W＝(x,y,z,1)的转换关系为：

则对深度图像中的任意一点P_I来说，其所对应的世界坐标系中的点P_W的齐次坐标为：

设据此计算出的P_W在世界坐标系中的坐标为(x₀,y₀,z₀)，设距离空地两用机器人当前位置的欧式距离在一定阈值内的空间为S_n，当P_W在区域E∩S_n之内时，将其沿x，y，z三个方向进行膨胀，膨胀的点集P_W+为：

P_W+＝{(x,y,z)|x₀-ε≤x<x₀+ε,y₀-ε≤y<y₀+ε,z₀-δ≤z<z₀+δ} (7)

当P_W不在区域S_n内时，将其加入点集中。此外，由于空地两用机器人能够在平面上进行地面运动，因此在Z轴方向上空地两用机器人的建图需要更加精细，即δ的值应小于ε。

将膨胀后的点集P_W+中所有在区域E∩S_n之内的点加入局部点云集合C_p中，对深度图像中所有的点均进行上述膨胀操作后，去除C_p中重复的点，C_p即成为包含局部障碍信息的局部点云集合。随后将区域E∩S_n之内的点云信息清空，并依照C_p中的点云信息进行重新填充，C_p中出现的点云设置为占据，即可获得更新后的局部栅格地图M_p，全局栅格地图M_g也随之进行了更新。

依次取出局部栅格地图M_p中的所有被设置为占据的点云，设点云坐标为P_O＝(x_O,y_O,z_O)，将P_O加入E_k中，并从E_u中移除P_O。此外，根据空地两用机器人当前位置将局部栅格地图M_p中满足下述条件的点加入已知区域E_k中，并从未知区域E_u中移除：

P＝P_B+k_O(P_O-P_B),0<k<1.2 (8)

同样的，将局部栅格地图M_p中满足下述条件的点加入已知区域E_k中，并从未知区域E_u中移除：

P＝P_B+k_S(P_S-P_B),0<k<1,P∈S_n (9)

对局部栅格地图中的未知区域，即M_p∩E_u内的数据点，采用组内平方误差和来确定最佳聚类数目N，随后采用K-means聚类算法对未知点进行聚类，所得的N个聚类中心作为N个最新待探索点，并记录每个聚类点云中未知点的数量。对每个最新待探索点来说，将该待探索点记为P_D，待探索点集为P_G＝{P_Gi}，若P_D距离点集中任意一点距离均大于一固定阈值α_s，即：

则将P_D加入到待探索点集P_G中，直至N个待探索点均判断结束。随后根据未知地图区域E_u对待探索点集内的所有待探索点P_Gi依次进行筛查，若待探索点一定阈值内不存在未知地图区域，即：

则将待探索点移除出待探索点集，待探索点集即更新完成。

步骤三：设计由目标点信息增益值、空地混合路径惩罚项、途经信息增益值构成的度量函数，以确定最优待探索点。

根据本发明实施例，首先，根据当前时刻待探索点位置与其附近栅格地图未知区域，计算目标点信息增益值；其次，根据由能耗、时间等运动参数加权的代价函数，设计由当前位置到候选待探索点的空地混合路径，空地混合路径惩罚项的值即为该路径对应的代价函数值；再次，根据所选择的空地混合路径与其附近栅格地图未知区域，计算途经信息增益值；最后，所有待探索点计算完毕后，选取其中度量函数值最大的点设为最优待探索点。具体如下。

在步骤二所确定的当前待探索点集中取出一点P_Gi作为此时机器人路径规划的目标终点。点P_Gi所对应的度量函数F的值越大，取出的优先级越高。度量函数F的形式为：

F＝α_FG_T-β_FL_W+γ_FG_W (12)

其中，G_T为目标点信息增益值，L_W为空地混合路径惩罚项，G_W为途经信息增益值，α_F、β_F、γ_F为选定的系数。

对于目标点信息增益值G_T的计算：G_T代表了空地两用机器人在目标点附近能够感知到的未知环境信息量，其值等于当前目标终点P_Gi在步骤二中所属的聚类点云中包含的未知点的数量，其值越高，越倾向于优先探索该点。

对于空地混合路径惩罚项L_W的计算：首先进行由当前位置到目标位置的全局路径规划，与空中机器人不同，空地两用机器人可以在空中与地面运动，因此在进行路径规划前先通过算法去除掉全局栅格地图M_g中的地面点云，所采用的算法为LeGO-LOAM点云分割算法，去除地面点云后的全局栅格地图记为M_ga；进行地面点云分割后，给定目标点速度为零，目标点位置为P_Gi，起始点速度为空地两用机器人当前速度，起始点位置为空地两用机器人当前位置，将以上参数连同去除地面点云的全局栅格地图M_ga输入渐进最优快速随机搜索树路径规划算法中，即可获得可供空地两用机器人通行的全局路径Γ，全局路径包括空中路径Γ_A及地面路径Γ_F，将其长度分别记为与/>

然后，在获取全局路径Γ后，计算该路径所对应的空地混合路径惩罚项L_W，L_W代表了空地两用机器人在从当前位置去往目标终点时的路径消耗，其形式通过耗能与时间加权进行表示，即：

L_W＝β_EP_E+β_TP_T (13)

其中，P_E为耗能惩罚项，P_T为时间惩罚项，β_E、β_T为权重。通过调整β_E、β_T的数值，可以实现空地两用机器人快速性与节能性的平衡。特殊的，当β_E＝0、β_T≠0时，空地两用机器人不关注运动耗能，仅考虑时间最优；当β_E≠0，β_T＝0时，空地两用机器人不关注运动时间，仅考虑能量最优。耗能惩罚项P_E与耗时惩罚项P_T由空中路径长度与地面路径长度/>加权获得，即：

其中，β_EA、β_EF、β_TA、β_TF为根据空地两用机器人结构及运动特性设计的权值，一般来说，空地两用机器人空中运动较快，但耗能较高，地面运动反之，即β_EA>β_EF，β_TA<β_TF。由此可计算得到空地混合路径惩罚项P_W，其值越高，越倾向于延后探索该点。

对于途经信息增益值G_W的计算：途经信息增益值G_W代表了空地两用机器人从当前位置去往目标终点时能感知到的未知环境信息量。设与全局路径距离小于一定阈值的管状区域为Φ，该区域的定义为：

其中，β_Γ为一固定阈值。途经信息增益值G_W的值即为未知区域E_u与管状区域Φ的交空间E_u∩Φ所包含的点云数量，其值越高，越倾向于优先探索该点。

最后，根据式(12)计算度量函数F的值。当待探索点集中的所有待探索点均计算度量函数值完毕后，选取度量函数值最大的待探索点，将其移出待探索点集，并将其设置为当前时刻的目标点，记为

步骤四：根据全局地图生成探索轨迹，空地两用机器人跟踪轨迹到达最优待探索点。

根据本发明实施例，首先，将步骤三中所生成的空地混合路径作为全局轨迹，考虑轨迹平滑性、可行性、探索效率及机器人避障性能，利用机器人可空地切换的特性，设计代价函数对轨迹进行局部优化；其次，设计空地两用机器人的轨迹跟踪控制方法，同时在轨迹跟踪的过程中通过小幅调整偏航角扩大机器人的探索范围，直至空地两用机器人到达最优待探索点位置。具体如下。

首先，将步骤三确定最优探测点过程中生成的空地混合路径Γ作为全局参考轨迹，这是一条由机器人自身位置到最优探测点位置/>且不考虑未探测区域障碍物信息的B样条曲线。考虑轨迹平滑性、可行性、探索效率及机器人避障性能，利用机器人可空地切换的特性，对轨迹进行局部优化。将进行轨迹优化的局部轨迹记为Ψ，其阶数为p_b，由N_c个控制点/>和一个节点向量{t₁,t₂,...,t_M}所描述，其中/>M＝N_c+p_b。均匀B样条的每个节点具有相同时间间隔△t＝t_m+1-t_m，由此可求得轨迹Ψ的一阶、二阶、三阶导数的控制点：

其次，根据空地两用机器人的微分平坦特性减少要规划的变量。优化问题可以被定义为：

Q＝arg min J＝λ_sJ_s+λ_cJ_c+λ_dJ_d+λ_IJ_I+λ_hJ_h (18)

其中，J_s为光滑项，J_c为碰撞项，J_d为动力学可行项，J_I为探索项，J_h为空地模态项，λ_s、λ_c、λ_d、λ_I、λ_h为惩罚项的权值。各项惩罚项的计算方式分述如下：

a.光滑项J_s的作用是提升轨迹的平滑性。局部轨迹的平滑性越差，所构造的光滑项惩罚越高。光滑项惩罚采用加速度和加加速度表征：

b.碰撞项J_c的作用是避免机器人与障碍产生碰撞。碰撞项与曲线到障碍物的距离有关，由于全局规划时还未构建出全部障碍物，因此初始的局部轨迹可能穿过障碍物，需要构造惩罚函数把轨迹推离障碍物。每个与局部轨迹发生碰撞的控制点Q_i，都会生成一个与之对应的在障碍物表面的定位点p_ij，产生一个排斥方向向量其中i为控制点序号，j为{p,v}对序号，每个{p,v}对对应一个特定的控制点，Q_i到第j个障碍物距离即可表示为：

d_ij＝(Q_i-p_ij)v_ij (20)

机器人与障碍物的距离计算完成后，为将机器人轨迹推离该障碍物，设定机器人与障碍物的安全距离为s_f，对d_ij<s_f的控制点施加如下的惩罚项：

对于第i个控制点，惩罚项可表示为其中N_p为属于控制点Q_i的{p,v}_j对个数，据此将碰撞项计表征为：

c.动力学可行项J_d的作用是使生成的轨迹能够被机器人有效跟踪。动力学可行项考虑空地两用机器人运动的最大速度、最大加速度、最大加加速度，表征如下：

其中，ω_v、ω_a、ω_j为常值权重，c_r∈C∈{V_i,A_i,J_i}，a₁、b₁、c₁、a₂、b₂、c₂用来满足函数的二阶连续性，c_m为导数限制，c_j为二次和三次函数的交界，λ为常系数。

d.探索项J_I的作用是使机器人在运动中探索到更多信息。探索项体现了空地两用机器人从当前位置去往目标点的途中能感知的未知环境信息量，表征为：

其中，S(Φ∩E_u)为管状区域Φ和未知区域E_u的交集在平面内的投影面积。

e.空地模态项J_h的作用是利用平台地面运动能力减少机器人空中飞行时间，提升机器人续航。空地模态项表征为：

J_h＝a_hh+b_h (26)

其中，h为控制点离障碍物或地面的高度差，a_h、b_h为常值系数。

光滑项J_s、碰撞项J_c、动力学可行项J_d、探索项J_I、空地模态项J_h均计算完毕后，采用牛顿法求解上述无约束优化问题的最优解，即可生成优化轨迹。空地两用机器人通过路径规划生成优化轨迹的典型效果如图3所示。

最后，机器人跟踪优化轨迹上的轨迹点，到达最优待探索点。设开始规划时刻为t₀，位置为Q₀，则在下一时刻t₀+△t，期望位置为Q₁，期望速度为以此类推，在t₀+k△t时刻，期望位置为Q_k，期望速度为/>在此过程中，为了进一步扩大探索范围，空地两用机器人在跟踪路径的同时，会通过小幅调整偏航角扩展左右视野。对于某一局部规划路径，设Q₀＝(x₀,y₀,z₀)，/>则在t₀+k△t时刻，期望偏航角φ_k满足：

其中，α_k、β_k为常系数，通过改变系数可调整摆动的幅度和频率。底层控制器跟踪位置、速度、偏航角指令序列，当机器人与最优待探索点的距离到达一定范围以内时，视为机器人成功到达最优待探索点。

步骤五、重复步骤二至四，直至实现已知地图区域对所需探索空间范围的覆盖，空地两用机器人即完成了对未知环境进行自主探索的流程。

根据本发明实施例，直至未知区域E_u与目标探索区域E的点云数量比值小于1％则停止重复执行步骤二至四，此时视为空地两用机器人对未知环境的探索完毕。

进一步通过实验验证本发明的技术效果，在典型场景中对空地两用机器人、空中机器人及地面机器人的探索效果进行测试，为排除机器人重量、电机性能等因素对测试结果的影响，采用限制空地两用机器人运动模态的方式对空中机器人及地面机器人进行模拟，测试结果如图4、图5所示，从图中可以看出，空地两用机器人及空中机器人的探索范围均可覆盖整个测试场景，地面机器人无法对场景进行完整探索；相较于地面机器人，空地两用机器人在更短的探索时间内实现了更大的探索覆盖范围；相较于空中机器人，空地两用机器人可在耗能更低的条件下完成区域的探索。

尽管根据有限数量的实施例描述了本发明，但是受益于上面的描述，本技术领域内的技术人员明白，在由此描述的本发明的范围内，可以设想其它实施例。对于本发明的范围，对本发明所做的公开是说明性的，而非限制性的，本发明的范围由所附权利要求书限定。

Claims (5)

1.一种适用于空地两用机器人的未知环境自主探索方法，其特征在于，包括以下步骤：

步骤一、获取未知环境的三维空间范围，并以所述三维空间中任意一点为原点，建立三维坐标系，对三维空间范围内未知区域及由未知点组成的待探索点集进行初始化；其中，所述三维坐标系的坐标轴方向与世界坐标系相同；未知区域初始化为E_u＝E，E表示未知环境的三维空间；待探索点集初始化为：

P_G＝{(x,y,z)|x＝k₁α,y＝k₂α,z＝k₃α,(x,y,z)∈E,k₁∈Z,k₂∈Z,k₃∈Z}

式中，Z表示整数；k₁、k₂、k₃为任意整数；α为固定阈值，代表待探索点彼此间的分布距离；

步骤二、以空地两用机器人当前位置为探索起点，获取空地两用机器人携带的深度传感器和惯性测量单元所采集的深度数据和姿态数据，并对所述深度数据和姿态数据进行处理以更新所述三维空间对应的栅格地图，进而更新待探索点集；具体步骤包括：

步骤二一、根据所述深度数据和所述姿态数据通过坐标变换获取空地两用机器人当前位置P_B；对所述深度数据进行处理，获取点云集合P_W；

步骤二二、获取所述点云集合P_W中每个点膨胀后的点集P_W+，具体为：设置与空地两用机器人当前位置P_B的欧式距离为第一固定阈值的空间S_n，当该点在所述三维空间E∩S_n的区域内时，将该点沿x、y、z三轴方向进行膨胀，膨胀后的点集P_W+为：

P_W+＝{(x,y,z)|x₀-ε≤x＜x₀+ε,y₀-ε≤y＜y₀+ε,z₀-δ≤z＜z₀+δ}

式中，(x,y,z)表示膨胀后的点集P_W+中点的三维坐标；(x₀,y₀,z₀)表示点云集合P_W中任意一点的三维坐标；ε、δ表示点膨胀的距离；

步骤二三、从每个膨胀后的点集P_W+中去除在所述三维空间E∩S_n的区域之外的点，然后将所有膨胀后的点集P_W+中剩余的点组合形成包含局部障碍信息的局部点云集合C_p；

步骤二四、在所述三维空间对应的栅格地图上，将所述局部点云集合C_p中的点赋值属性为占据，更新局部栅格地图M_p和全局栅格地图M_g，随后更新已知区域E_k和未知区域E_u，进而更新待探索点集P_G；在更新已知区域E_k和未知区域E_u时，将局部栅格地图M_p中满足下述公式的部分点P₁加入已知区域E_k中，并从未知区域E_u中移除：

P₁＝{P_B+k₄(P_O-P_B)|0＜k₄＜1.2}

式中，P_O表示局部栅格地图M_p中被赋值为占据的点；

将局部栅格地图M_p中满足下述公式的部分点P₂加入已知区域E_k中，并从未知区域E_u中移除：

式中，表示不在区域S_n内且在点云集合P_W内的点，即/>

在对未知区域E_u移除部分点P₁和P₂后，对局部栅格地图M_p中的未知区域，即M_p∩E_u中的点进行聚类，将获得的多个聚类中心组成候选待探索点集P_D，并记录每个聚类簇中未知点的数量；然后依据下述顺序在候选待探索点集P_D中选择保留部分点加入待探索点集：1)预设第二固定阈值，对于所有候选待探索点集P_D中的点，若其与待探索点集P_G中任意一点的距离均大于所述预设第二固定阈值，则将候选待探索点集P_D中的该点加入待探索点集P_G；2)预设第三固定阈值，对于所有候选待探索点集P_D中的点若其与未知区域E中任意一点的距离u

均大于所述预设第三固定阈值，则将其从待探测点集P_G中移除；

步骤三、基于度量函数在待探索点集中选择最优待探索点；具体步骤包括：

步骤三一、将待探索点集P_G中每个待探索点作为目标终点，计算其对应的度量函数，其中，度量函数的计算公式为：

F＝α_FG_T-β_FL_W+γ_FG_W

式中，G_T为目标点信息增益值，表示空地两用机器人在目标点附近能够感知到的未知环境信息量；L_W为空地混合路径惩罚项，表示空地两用机器人从当前位置去往目标终点过程中的路径消耗；G_W为途经信息增益值，表示空地两用机器人从当前位置去往目标终点过程中感知到的未知环境信息量；α_F、β_F、γ_F为选定的系数，分别表示目标点信息增益值、空地混合路径惩罚项、途经信息增益值对度量函数的权重；

步骤三二、选取度量函数值最大对应的待探索点为当前时刻的目标终点，即最优待探索点，并将其移出待探索点集；

步骤四、对根据空地两用机器人当前位置和最优待探索点生成的全局路径进行优化，使得空地两用机器人沿优化后的全局路径到达最优待探索点；

步骤五、重复执行步骤二至步骤四，直至待探索点集中的未知点达到预设阈值则停止重复执行，空地两用机器人完成对未知环境的自主探索。

2.根据权利要求1所述的一种适用于空地两用机器人的未知环境自主探索方法，其特征在于，步骤三一中所述目标点信息增益值G_T的值等于该待探索点所属的聚类簇所包含未知点的数量；所述空地混合路径惩罚项L_W的值通过耗能与时间加权表示；所述途经信息增益值G_W的值等于空地两用机器人从当前位置去往目标终点过程中所经过的未知区域E_u与管状区域Φ的交空间E_u∩Φ所包含的点的数量；其中，管状区域Φ的定义为：

式中，Γ表示空地两用机器人从当前位置去往目标终点的全局路径；β_Γ表示预设第四固定阈值；P表示管状区域内的点的坐标。

3.根据权利要求2所述的一种适用于空地两用机器人的未知环境自主探索方法，其特征在于，步骤三一中所述空地混合路径惩罚项L_W的具体计算公式为：

L_W＝β_EP_E+β_TP_T

式中，P_E为耗能惩罚项，P_T为时间惩罚项，β_E、β_T为权重；

式中，β_EA、β_EF、β_TA、β_TF为根据空地两用机器人结构及运动特性设计的权值；表示空地两用机器人从当前位置去往目标终点的全局路径中的空中路径的长度；/>表示空地两用机器人从当前位置去往目标终点的全局路径中的地面路径的长度。

4.根据权利要求3所述的一种适用于空地两用机器人的未知环境自主探索方法，其特征在于，步骤四中对全局路径进行优化，其优化的目标函数表示为：

Q＝argminJ＝λ_sJ_s+λ_cJ_c+λ_dJ_d+λ_IJ_I+λ_hJ_h

式中，J_s为光滑项，J_c为碰撞项，J_d为动力学可行项，J_I为探索项，J_h为空地模态项，λ_s、λ_c、λ_d、λ_I、λ_h为每一个惩罚项对应的权值。

5.根据权利要求4所述的一种适用于空地两用机器人的未知环境自主探索方法，其特征在于，步骤四中空地两用机器人沿优化后的全局路径到达最优待探索点的过程中，通过小幅调整偏航角以扩大空地两用机器人的探索范围，具体包括：对于优化后的全局路径上的两个点，设置起点终点/>偏航角φ_k满足下述公式：

式中，α_k、β_k为可改变的系数，kΔt表示起点到终点/>的时间间隔。