CN117330081B - 一种基于机器人的感知导航装置及其方法 - Google Patents

Abstract

本申请涉及机器人智能化应用技术领域，提供一种基于机器人的感知导航装置及其方法，该装置包括机器人控制中心、环境感知模块、路径规划模块、数据获取单元、第一路径导航模块和第二路径导航模块；机器人控制中心分别与环境感知模块、路径规划模块、数据获取单元、第一路径导航模块和第二路径导航模块连接；机器人控制中心用于：控制环境感知模块、路径规划模块、数据获取单元、第一路径导航模块和第二路径导航模块的数据交互。本申请提供的基于机器人的感知导航装置实现了机器人在运行过程中对周围环境的感知，以及根据优化后的环境点云构建出运行的感知标注模型，同时根据实时的路径程度，通过不同的路径导航策略规划出最优导航路径结果。

Description

一种基于机器人的感知导航装置及其方法

技术领域

本申请涉及机器人智能化应用技术领域，具体涉及一种基于机器人的感知导航装置及其方法。

背景技术

现有的路径规划往往基于路网搜索的方法，这种方法无法应用于建筑物内的特殊场景下的路径规划。现有技术中的路径规划方法，没有考虑行动对象的体积，无法适用于特殊场景(如：狭长弯曲且相似度高的情况下)的路径规划。同时，现有的路径规划不能实施感知周围环境，并对周围环境做出实时、准确的相应标注。

发明内容

本申请提供一种基于机器人的感知导航装置及其方法，旨在实现机器人在运行过程中对周围环境的感知，以及根据优化后的环境点云构建出运行感知标注模型，同时根据实时的路径程度，通过不同的路径导航策略规划出最优导航路径结果。

第一方面，一种基于机器人的感知导航装置，包括机器人控制中心、环境感知模块、路径规划模块、数据获取单元、第一路径导航模块和第二路径导航模块；所述机器人控制中心分别与所述环境感知模块、所述路径规划模块、所述数据获取单元、所述第一路径导航模块和所述第二路径导航模块连接；所述机器人控制中心用于：控制所述环境感知模块、所述路径规划模块、所述数据获取单元、所述第一路径导航模块和所述第二路径导航模块的数据交互；

所述环境感知模块用于：获取机器人按照预定运行路径在运行过程中的待处理环境点云，并对所述待处理环境点云进行投影重建，得到优化环境点云；

所述路径规划模块用于：提取所述优化环境点云中的各个建筑物信息，基于所述建筑物信息进行模型训练，得到运行感知标注模型；

所述数据获取单元用于：获取机器人按照所述预定运行路径在运行过程中的路径程度；所述路径程度表征所述预定运行路径的实时路径程度；所述路径程度包括第一路径程度和第二路径程度，且所述第二路径程度的路径程度优于所述第一路径程度的路径程度；

所述数据获取单元用于：若所述路径程度为第一路径程度，则获取在运行过程中的起始位置点、中间位置点和终点位置点；

所述第一路径导航模块用于：基于所述起始位置点、所述中间位置点和所述终点位置点之间的关联性，构建第一最优规划路径结果；

所述数据获取单元用于：若所述路径程度为第二路径程度，则获取第一三维模型和第二三维模型；

所述第二路径导航模块用于：基于所述第一三维模型和所述第二三维模型，构建第二最优规划路径结果；所述第一三维模型为反映目标三维空间内各个内容物的尺寸和相对位置的三维模型，所述目标三维空间的内容物包括至少一个子空间、连接各个所述子空间的通道空间以及障碍体，所述第二三维模型为反映行动对象的尺寸的三维模型。

第二方面，本申请提供一种基于机器人的感知导航方法，包括：

获取机器人按照预定运行路径在运行过程中的待处理环境点云，并对所述待处理环境点云进行投影重建，得到优化环境点云；

提取所述优化环境点云中的各个建筑物信息，基于所述建筑物信息进行模型训练，得到运行感知标注模型；

获取机器人按照所述预定运行路径在运行过程中的路径程度；所述路径程度表征所述预定运行路径的实时路径程度；所述路径程度包括第一路径程度和第二路径程度，且所述第二路径程度的路径程度优于所述第一路径程度的路径程度；

若所述路径程度为第一路径程度，则获取在运行过程中的起始位置点、中间位置点和终点位置点；

基于所述起始位置点、所述中间位置点和所述终点位置点之间的关联性，构建第一最优规划路径结果；

若所述路径程度为第二路径程度，则获取第一三维模型和第二三维模型；

基于所述第一三维模型和所述第二三维模型，构建第二最优规划路径结果；所述第一三维模型为反映目标三维空间内各个内容物的尺寸和相对位置的三维模型，所述目标三维空间的内容物包括至少一个子空间、连接各个所述子空间的通道空间以及障碍体，所述第二三维模型为反映行动对象的尺寸的三维模型。

第三方面，本申请还提供一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现第二方面所述基于机器人的感知导航方法。

第四方面，本申请还提供一种非暂态计算机可读存储介质，所述非暂态计算机可读存储介质包括计算机程序，所述计算机程序被所述处理器执行时实现第二方面所述基于机器人的感知导航方法。

第五方面，本申请还提供一种计算机程序产品，所述计算机程序产品包括计算机程序，所述计算机程序被所述处理器执行时实现第二方面所述基于机器人的感知导航方法。

本申请实施例实现了机器人在运行过程中对周围环境的感知，以及根据优化后的环境点云构建出运行感知标注模型，同时，根据实时路径程度，通过不同的路径导航策略规划出最优导航路径结果。

附图说明

为了更清楚地说明本申请的技术方案，下面将对实施例或现有技术描述中所需使用的附图作一简单地介绍，显而易见地，下面描述中的附图是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1是本申请提供的基于机器人的感知导航装置的结构示意图；

图2是本申请提供的基于机器人的感知导航方法的流程示意图；

图3是本申请提供的电子设备的结构示意图。

具体实施方式

为使本申请的目的、技术方案和优点更加清楚，下面将结合本申请中的附图，对本申请中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。

本申请实施例提供了一种基于机器人的感知导航装置的实施例，需要说明的是，虽然在流程图中示出了逻辑顺序，但是在某些数据下，可以以不同于此处的顺序完成所示出或描述的步骤。

参照图1，图1是本申请提供的基于机器人的感知导航装置的结构示意图。本申请实施例提供的基于机器人的感知导航装置包括机器人控制中心、环境感知模块、路径规划模块、数据获取单元、第一路径导航模块和第二路径导航模块；机器人控制中心分别与环境感知模块、路径规划模块、数据获取单元、第一路径导航模块和第二路径导航模块连接；机器人控制中心控制环境感知模块、路径规划模块、数据获取单元、第一路径导航模块和第二路径导航模块的数据交互。

在一实施例中，环境感知模块可以获取机器人按照预定运行路径在运行过程中的待处理环境点云，由于待处理环境点云中投影的覆盖导致对应区域的地物信息损失严重，严重影响了后续的地物提取和分析，因此，需要对待处理环境点云进行投影重建，得到优化环境点云。

在一实施例中，路径规划模块可以提取优化环境点云中的各个建筑物信息，为了后续在运行过程中能够实时准确地标注出点云中的各个建筑物信息，因此，需要通过建筑物信息进行模型训练，得到运行感知标注模型。

在一实施例中，数据获取单元可以获取机器人按照预定运行路径在运行过程中的路径程度，其中，路径程度表征预定实时运行路径的路径程度，因此，路径程度包括第一路径程度和第二路径程度，且第二路径程度的路径程度优于第一路径程度的路径程度(比如，路径宽度小于或者等于50cm(厘米)为第一路径程度，路径宽度大于50cm为第二路径程度，可根据场景情况设定)。

在一实施例中，数据获取单元还可以获取在运行过程中的起始位置点、中间位置点和终点位置点，具体为：若路径程度为第一路径程度，则获取在运行过程中的起始位置点、中间位置点和终点位置点。

在一实施例中，第一路径导航模块可以根据起始位置点、中间位置点和终点位置点之间的关联性，构建第一最优规划路径结果。

在一实施例中，数据获取单元还可以获取第一三维模型和第二三维模型，具体为：若路径程度为第二路径程度，则获取第一三维模型和第二三维模型。

在一实施例中，第二路径导航模块可以能根据第一三维模型和第二三维模型，构建第二最优规划路径结果，其中，第一三维模型为反映目标三维空间内各个内容物的尺寸和相对位置的三维模型，目标三维空间的内容物包括至少一个子空间、连接各个子空间的通道空间以及障碍体，第二三维模型为反映行动对象的尺寸的三维模型。

在一实施例中，机器人状态观测模块可以建立状态方程的第一公式拓扑图，以及建立量测方程的第二公式拓扑图，并根据无迹卡尔曼滤波器以及第一公式拓扑图和第二公式拓扑图进行机器人状态观测。

本申请实施例实现了机器人在运行过程中对周围环境的感知，以及根据优化后的环境点云构建出运行感知标注模型，同时根据实时路径程度，通过不同的路径导航策略规划出最优导航路径结果。

参照图2，图2是本申请提供的基于机器人的感知导航方法的流程示意图。本申请实施例提供的基于机器人的感知导航方法包括：

步骤201，获取机器人按照预定运行路径在运行过程中的待处理环境点云，对所述待处理环境点云进行投影重建，得到优化环境点云；

需要说明的是，本申请实施例以机器人为执行主体进行举例说明，但不限制于机器人。

机器人获取机器人按照预定运行路径在运行过程中的待处理环境点云，由于待处理环境点云中投影的覆盖导致对应区域的地物信息损失严重，严重影响了后续的地物提取和分析，因此，需要对待处理环境点云进行投影重建，得到优化环境点云，具体为(有且不限于此)：

将待处理环境点云从RGB空间映射到CIELCh色彩空间，并获取在CIELCh色彩空间的光谱比值点云。其中，CIELCh色彩空间是CIELAB色彩空间的极坐标表示，相应地，CIELCh是基于CIELAB空间的一种变体，将颜色表示为亮度(L)、色度(C)和色相(h)。其中，亮度(L)表示颜色的明暗程度，取值范围从0到100。色度(C)表示颜色的鲜艳程度，取值范围通常是正数。色相(h)表示颜色的具体颜色类别，取值范围从0到360度。CIELAB是一种基于人眼感知的颜色空间，也被称为Lab色彩空间。包括亮度(L)、a轴(从青绿到红色的范围)和b轴(从蓝色到黄色的范围)。亮度(L)同样表示颜色的明暗程度，取值范围从0到100。a轴表示颜色在红色和绿色之间的位置，取值范围通常是负数到正数。b轴表示颜色在黄色和蓝色之间的位置，取值范围通常是负数到正数。

将待处理环境点云转换至CIELCh色彩空间，获取其亮度(Lightness)、色度(chroma)和色调(hue)，根据亮度L和色调h即可确定光谱比值点云，其具体过程为：

将待处理环境点云由RBG空间转换为CIEXYZ色彩空间；

基于CIEXYZ色彩空间的XYZ三基色得到CIELAB色彩空间；

将CIELAB色彩空间进行几何变换得到所述CIELCh色彩空间；

确定待处理环境点云在CIELCh色彩空间中的亮度和色调；

基于亮度和色调确定光谱比值点云。

具体地，首先将RGB转换为CIEXYZ色彩空间：

从XYZ三基色可以得到CIELAB色彩空间：

其中，

在上述方程中，X_n，Y_n，Z_n为标准D65光源的参考值，即X_n，Y_n，Z_n＝{95.047，100.00，108.883}。

CIELAB色彩空间可以通过简单的几何变换转化为CIELCh色彩空间：

h＝atan2(b,a)

由于缺少太阳光的直射，投影区域具有高色调、低强度的特征。计算像素的色调和亮度之间的比率以突出投影与非投影区域的值的差异：SR＝(h+1)/(L+1) (8)

其中SR是光谱比值点云，L和h分别指的是亮度L和色调h，在计算之前归一化至[0，1]。在光谱比值点云中，投影区域中的像素将具有比非投影区域中的像素更高的值

可选的，首先使用简单线性迭代聚类方法对待处理环境点云进行超像素分割，然后将分割的点云进行超像素合并，将合并后的超像素根据个像素均值进行重建。

其过程包括：基于简单线性迭代聚类方法对所述待处理环境点云进行超像素分割，得到分割超像素；

将分割超像素转换为区域邻接点云，将颜色相似的区域邻接点云进行合并，得到合并超像素点云；

对合并超像素点云进行重建，得到重建光谱比点云。

具体地，在本实施例中，如果直接对投影光谱比进行阈值分割，会出现很多细小噪声，而且难以保证投影边缘的完整性，所以对投影光谱比点云进行超像素重建。

首先使用简单线性迭代聚类(simple linear iterative clustering)方法对待处理环境点云进行超像素分割。简单线性迭代聚类是一种用于点云分割的聚类算法，能够将点云分割成具有相似颜色和纹理特征的区域。SLIC算法的基本思想是在点云上生成一组紧凑的初始聚类中心，然后通过迭代的方式将每个像素分配给最近的聚类中心，直到收敛为止。SLIC通过将搜索空间限制为与超像素大小成比例的区域，显着地减少了优化中的距离计算的数量，同时提供了对超像素的紧凑性的控制。然而，超像素较少会导致超像素边界粗糙，较多会导致后续计算复杂度变大。为解决这一问题，先生成较多的超像素，之后进行相似的相邻超像素合并。将SLIC计算得到的超像素转为区域邻接点云(Region AdjacencyGraph，RAG)，并逐步合并颜色相似的区域，得到最终的合并超像素点云。之后对合并后的超像素点云进行重建，即使用每个超像素内所有像素的均值作为超像素的值。假设待处理环境点云被分割n个超像素，每个超像素包含m_i个像素，那么重建后的像素比可表示为：

其中，SR_j为像素j的SR值。

可选的，通过自动确定阈值来实现待处理环境点云的全自动投影提取。由于深色物体、水体和投影往往位于光谱比直方点云的同一侧。若使用二分类确定阈值法，可能会将深色物体和水体误提取。因此，本实施例中可使用自动多级全局阈值确定方法得到多个阈值，取其中最大的阈值为投影阈值。将超像素重建的光谱比点云阈值化为二进制掩码，可以更好地从深色物体和水体中将投影分割出来。

具体如下步骤：

基于自动多级全局阈值确定方法得到多个待选阈值；

在多个待选阈值中选取投影阈值，基于投影阈值对超像素重建的光谱比点云进行检测分割；

将高于投影阈值的物体点云作为非投影点云，将低于投影阈值的物体点云作为投影点云。

最后，对分割检测后得到的投影点云进行分区域重建，得到投影重建后的优化环境点云，也就是说，待处理环境点云中提取到的投影点云由本影(Umbra)和半影(Penumbra)组成。其中，本影只受到环境光强的影响，半影由环境光强和动态衰减的太阳直射光强组成。将投影重建分为本影补偿和半影后处理两部分，分别对本影区域和半影区域进行重建之后，将两部分区域的重建点云进行组合，便得到最终的投影重建后的优化环境点云。

进一步地，需要补充说明的是，在理想状态下，投影点云重建的过程如下所示：

对于重建点云中任意一个像素点i的值I_i可表示为该像素点的光强度(intensity)L_i与该点反射率R_i的乘积：I_i＝L_i*R_i(10)

光强度(light intensity)包括直接强度(Direct intensity)和环境强度(Ambient intensity)：直接照度直接来自太阳辐射，环境照度主要是由天空散射引起的。影像中非投影区域和投影区域的像素值可以计算为：

I_unshaded,i＝(L_d+L_e)R_i

I_shaded_,i＝(k_iL_d+L_e)R_i,k_i∈[0,1)(11)

其中，I_unshaded，i是非投影像素的值，I_shaded，i是投影像素的值，L_d是直射光的强度，L_e是环境光的强度。R_i是像素的反射率，k_i表示直射光强度的衰减因子。如果像素仅被环境光照射，即处于本影区域，则k_i为0。如果像素仅被环境光和部分直射光照射，即处于半影区域，则k_i为(0，1)。

根据直射光和环境光之间的比率可以重新点亮投影区域的像素。对于任意像素i，环境照度和直接照度的比值r可以表示为：

最终的投影重建后像素值可以表示为：

重建的点云像素I_{ShadowReconstruction}，i在k＝0(即原始像素I_i在本影下)时可以简化为投影值乘以(r+1)。

以下给出在实际状态下的本影补偿和半影后处理过程。

可选的，对本影区域进行光照补偿，得到本影重建点云，包括：

S1、获取投影点云的投影超像素集合，对超像素集合进行标记，确定超像素数量；

S2、在超像素集合中选取第一投影超像素，并确定第一投影超像素的相邻非投影超像素集合，并确定相邻非投影超像素数量；

S3、获取第一投影超像素的第一直方点云，以及获取相邻非投影超像素集合的第二直方点云，基于巴氏系数计算第一直方点云和第二直方点云的相似性，基于相似性和相邻非投影超像素数量确定各投影超像素的权重；

S4、基于第一投影超像素和第一投影超像素的像素数目获取第一投影超像素的强度，以及基于超像素数量、相邻非投影超像素集合的超像素的像素数目以及各投影超像素的权重获取相邻非投影超像素强度；

S5、获取第一投影超像素的环境照度和直接照度的比值，基于比值和第一投影超像素的投影值，得到第一投影超像素的光照补偿；

重复S2-S5，获取投影超像素集合中的全部投影超像素的光照补偿，得到本影重建点云。

本实施例为投影点云的半影区域进行光照补偿的具体过程。

在本实施例中，由于相近的事物关联更紧密，每个超像素和与其相邻的超像素最有可能具有相同的属性。如果没有投影的影响，那么相邻的超像素接收太阳辐射最相近。所以利用与投影超像素相邻的非投影超像素对该投影超像素进行光照重建最为准确。地物被投影遮挡后，整体强度降低，但是直方点云的走势基本一致。所以，相邻超像素中直方点云的走势最为接近的贡献值越大。

在本实施例的详细步骤如下所示：

S1、将所有投影超像素集合标记为SP_shaded，超像素个数为S。

S2、获取第一个投影超像素SP_shaded，1的相邻非投影超像素集合：SP_near＝{SP_near，1，SP_near，2，……SP_near，S1}，S₁为SP_shaded，1的相邻非投影超像素的个数。

S3、计算SP_shaded，1和SP_near中超像素的直方点云H_shaded，1和H_near，利用巴氏系数ρ计算直方点云的相似性，计算每个超像素的权重W_near：

H_shaded,1＝hist(SP_shaded,1) (14)

S4、计算SP_shaded，1的强度和相邻非投影区域的强度，得出比率：

P为SP_shaded，1的像素数目，Q为SP_near中超像素的像素数目。

S5、计算SP_shaded，1对应的环境照度和直接照度的比值r₁，进而得到SP_shaded，1进行本影补偿后的结果为SP_{ShadowReconstruction，1}：

SP_{ShadowReconstruction,1}＝(r₁+1)SP_shaded,1 (21)

S6、对剩余的投影超像素重复步骤S2-S5，直至全部投影超像素进行光照补偿。

本实施例通过利用利用与投影超像素相邻的非投影超像素对该投影超像素进行光照重建，实现对本影区域的投影点云进行光照补偿，因而能够提升光照重建的准确性。

对半影区域进行均值滤波，得到半影重建点云，包括：

提取投影点云的投影边界，对投影边界进行膨胀操作，得到半影区域；判断目标像素点是否存在于半影区域中，若目标像素点存在于半影区域中，基于结构内核和无投影像素点确定目标像素点的重建结果，若目标像素点不存在于半影区域中，确定无投影像素点为目标像素点的重建结果；根据半影区域内的全部目标像素点的重建结果得到半影重建点云。

具体地，本实施例为半影区域的投影点云重建过程。

需要说明的是，半影处在本影和非投影的过渡区域，使用自动多级阈值提取的投影只包含半影的部分区域。较暗的半影会被提取为投影，经过本影补偿后会出现过补偿现象。较亮的半影不会被提取出来，进而不会进行光照补偿，出现欠补偿现象。所以需要对半影区域进行后处理。首先提取投影点云的投影边界，对投影边界进行膨胀操作，得到半影区域；

需要说明的是，膨胀因子根据结构内核使投影边界中的值增长。然后对目标像素点进行均值滤波，以缓解半影区域光照补偿的异常。需要判断目标像素点是否存在于半影区域中，若目标像素点存在于半影区域中，基于结构内核和无投影像素点确定目标像素点的重建结果，若目标像素点不存在于半影区域中，确定无投影像素点为目标像素点的重建结果；

重复上述步骤，获取半影区域内的全部目标像素点的重建结果，根据半影区域内的全部目标像素点的重建结果得到半影重建点云。

步骤202，提取所述优化环境点云中的各个建筑物信息，基于所述建筑物信息进行模型训练，得到运行感知标注模型；

机器人获取建筑物信息的标注信息，以及根据建筑物信息获取建筑物匹配组，其中，建筑物匹配组中包括多个建筑物数据。进一步肚饿，机器人计算建筑物信息与建筑物匹配组中每个建筑物数据之间的匹配置信度，并将建筑物匹配组中匹配置信度大于预设置信度阈值的建筑物数据确定为目标建筑物数据。进一步地，以建筑物信息和目标建筑物数据为训练数据对，以标注信息为标注训练数据，对预设模型中进行模型训练，得到运行感知标注模型；

其中，匹配置信度的计算公式为：

其中，S_ij表示第i个建筑物信息和第j个建筑物数据的匹配置信度，表示第i个建筑物信息和第j个建筑物数据之间匹配的轨迹点的数量，/>表示第i个建筑物信息和第j个建筑物数据之间轨迹覆盖的轨迹点的数量，/>表示第i个建筑物信息和第j个建筑物数据之间轨迹重合的轨迹点的数量。

步骤203，获取机器人按照所述预定运行路径在运行过程中的路径程度；所述路径程度表征所述预定运行路径的路径程度；所述路径程度包括第一路径程度和第二路径程度，且所述第二路径程度的路径程度优于所述第一路径程度的路径程度；

可选的，机器人获取机器人按照预定运行路径在运行过程中的路径程度，其中，路径程度表征预定运行路径的路径程度，路径程度包括第一路径程度和第二路径程度，且第二路径程度的路径程度优于第一路径程度的路径程度。

步骤204，若所述路径程度为第一路径程度，则获取在运行过程中的起始位置点、中间位置点和终点位置点；

步骤205，基于所述起始位置点、所述中间位置点和所述终点位置点之间的关联性，构建第一最优规划路径结果；

进一步地，若路径程度为第一路径程度，机器人则获取在运行过程中的起始位置点、中间位置点和终点位置点，并根据起始位置点、中间位置点和终点位置点之间的关联性，构建第一最优规划路径结果，具体过程为：

基于起始位置点n_s、终点位置点n_t和中间位置点n_i构建第一关联表示集合；第一关联表示集合中包括起始位置点到终点位置点之间的第一关联表示，和，起始位置点、多个中间位置点及终点位置点中，相邻两个位置点之间的多个第二关联表示；确定起始位置点n_s到终点位置点n_t之间的第一关联表示为起始位置点n_s到第一个中间位置点之间的第二关联表示为/>…，中间位置点n_i到中间位置点n_j之间的第二关联表示为/>最后一个中间位置点到终点位置点n_t之间的第二关联表示为/>第一关联表示集合V为/>

确定与第一关联表示集合关于第一关联表示对称的第二关联表

从预设关联映射网络中，确定的第一关联表示集合和第二关联表示集合共同的第一关联区域T₁，从预设关联映射网络中，确定的第一关联表示和第二关联表示集合构成的第二关联区域T₂，第一关联区域T₁与第二关联区域T₂的目标关联区域为T，目标关联区域T表示如下：

其中，Λ_k表示中间位置点n_i到中间位置点n_j之间的第二关联表示和/>构成的区域；

基于目标关联区域构建第一最优规划路径结果。

示例地，在本申请实施例中，假设预设关联映射网络中包括m个位置点，其中，预设关联映射网络中，第一个位置点可记为n₁，第二个位置点可记为n₂，第三个位置点可记为n₃，…，第i个位置点可记为n_i，第j个位置点可记为n_j，第m个位置点可记为n_m，第一个位置点n₁与第二个位置点n₂之间存在直接关联关系l₁，第一个位置点n₁与第三个位置点n₃之间存在直接关联关系l₂，第i个位置点n_i与第j个位置点n_j存在直接关联关系l_k，第m-1个位置点n_m-1与第m个位置点n_m存在直接关联关系l_t。其中，m表示预设关联映射网络中位置点的数量，t表示预设关联映射网络中直接关联关系的数量。则在本申请实施例中，预设关联映射网络对应的映射网络评价矩阵G’可通过下述公式1表示：

其中，第一个位置点n₁与第二个位置点n₂可以看作一个存在直接关联关系关系的位置点对，该位置点对的相关信息包括第一个位置点的标识n₁，第二个位置点的标识n₂，第一个位置点n₁的得分en₁，第二个位置点n₂的得分en₂，第一个位置点n₁与第二个位置点n₂之间的直接关联关系的标识l₁，以及直接关联关系l₁的得分el₁。此外，第一个位置点n₁与第三个位置点n₃也可以看作一个位置点对，第i个位置点n_i与第j个位置点n_j也可看作一个位置点对，第m-1个位置点可记为n_m-1与第m个位置点n_m也可看作一个位置点对等。

可以理解的是，在本申请实施例中，映射网络评价矩阵G’为预先基于预设关联映射网络构建的，其具体构建方法将在后续进行详细描述，此处先不做说明。

示例地，从映射网络评价矩阵中，剔除包括目标位置点的位置点对的相关信息时，假设第一个位置点n₁为处于目标关联区域之外的节点，即目标位置点，则从映射网络评价矩阵中剔除掉包括第一个位置点n₁的位置点对以及该位置点对的相关信息。假设公式1所示的映射网络评价矩阵G’中，包括第一个位置点n₁的位置点对只有第一个位置点n₁与第二个位置点n₂构成的位置点对，和第一个位置点n₁与第三个位置点n₃构成的位置点对，则删除G’中的第一行元素和第二行元素，得到更新后的映射网络评价矩阵，可记为G_c。

可选的，基于更新后的映射网络评价矩阵，从目标关联区域中确定第一最优规划路径结果。

示例的，在本申请实施例中，基于更新后的映射网络评价矩阵，从目标关联区域中确定第一最优规划路径结果时，可以先从关联区域中，确定包括起始位置点、多个中间位置点及终点位置点的多个候选路径；并基于更新后的映射网络评价矩阵，确定各候选路径对应的路径得分；再将最大路径得分对应的候选路径，确定为最终的第一最优规划路径结果，从而得到第一最优规划路径结果。

通常情况下，路径得分评分值越高，说明对应的路径越优；路径得分评分值越低，说明对应的路径越差。

示例的，在本申请实施例中，基于更新后的映射网络评价矩阵，确定各候选路径对应的路径得分时，针对各候选路径，可以从更新后的映射网络评价矩阵，查找候选路径中包括的各位置点的得分、及候选路径中各段路径的得分；再基于各位置点的得分和各段路径的得分，确定候选路径对应的路径得分。

其中，一段路径即为更新后的映射网络评价矩阵中，两个位置点之间的直接关联关系。

可以理解的是，上述每一条候选路径中会包括多段路径，每一段路径的两个端点为不同的两个位置点，这样对于每一条候选路径而言，会对应一个节点序列和一个链路序列，示例地，一条候选路径对应的节点序列和链路序列可通过下述公式2表示：

Rs＝{(ns_i)_1×p,(ls_j)_1×q,Eva_Rs}_1×(p+q+1) 公式2

其中，Rs表示一条候选路径对应的节点序列和链路序列，(ns_i)_1×p表示候选路径对应的节点序列，(ls_j)_1×q表示候选路径对应的链路序列，p表示候选路径中位置点的数量，q表示候选路径中位置点的链路的数量。

示例地，上述基于各位置点的得分和各段路径的得分，确定候选路径对应的路径得分时，可以基于各位置点的得分和候选路径中包括的位置点的数量，确定候选路径对应的第一平均得分；并基于各段路径的得分和候选路径中包括的段路径的数量，确定候选路径对应的第二平均得分；再将第一平均得分和第二平均得分的和，确定为候选路径对应的路径得分Eva_Rs，具体过程可通过下述公式3表示：

结合公式3，在确定出各候选路径对应的路径得分后，就可以将最大路径得分对应的候选路径，确定为最终的第一最优规划路径结果。

可选的，基于预设关联映射网络，构建对应的映射网络评价矩阵。

其中，映射网络评价矩阵中包括预设关联映射网络中，所有存在直接关联关系关系的位置点对的初始相关信息，初始相关信息包括位置点对中各位置点的标识、直接关联关系的标识及直接关联关系的距离，位置点对应有多个节点参数和多个链路参数。

示例地，在本申请实施例中，仍假设预设关联映射网络中包括m个位置点，其中，预设关联映射网络中，第一个位置点可记为n₁，第二个位置点可记为n₂，第三个位置点可记为n₃，…，第i个位置点可记为n_i，第j个位置点可记为n_j，第m个位置点可记为n_m，第一个位置点n₁与第二个位置点n₂之间存在直接关联关系l₁，第一个位置点n₁与第三个位置点n₃之间存在直接关联关系l₂，第i个位置点n_i与第j个位置点n_j存在直接关联关系l_k，第m-1个位置点n_m-1与第m个位置点n_m存在直接关联关系l_t，则映射网络评价矩阵G可通过下述公式4表示：

在上述公式4所示的映射网络评价矩阵G中，ld₁表示第一个位置点n₁与第二个位置点n₂之间的直接关联关系l₁的链路距离，ld₂表示第一个位置点n₁与第三个位置点n₃之间的直接关联关系l₂的链路距离，ld_k表示第i个位置点n_i与第j个位置点n_j的直接关联关系l_k的链路距离，ld_t表示第m-1个位置点n_m-1与第m个位置点n_m存在直接关联关系l_t的链路距离。

示例的，在本申请实施例中，针对各位置点而言，其对应的多个节点参数可以包括位置点的负载能力，单波速率，连通度和计算能力；对应的多个链路参数可以包括链路距离，带宽利用率和链路故障率，具体可以根据实际需要进行设置，在此，本申请实施例只是以位置点包括四个节点参数和三个链路参数为例进行说明，但并不代表本申请实施例仅局限于此。

示例地，在本申请实施例中，m个位置点对应的节点参数矩阵P_n可通过下述公式5表示：

其中，lc₁表示第一个位置点n₁的负载能力，swr₁表示第一个位置点n₁的单波速率，con₁表示第一个位置点n₁的连通度，cp₁表示第一个位置点n₁的计算能力；lc₂表示第二个位置点n₂的负载能力，swr₂表示第二个位置点n₂的单波速率，con₂表示第二个位置点n₂的连通度，cp₂表示第二个位置点n₂的计算能力；lc_m表示第m个位置点n_m的负载能力，swr_m表示第m个位置点n_m的单波速率，con_m表示第m个位置点n_m的连通度，cp_m表示第m个位置点n_m的计算能力。

示例地，在本申请实施例中，t个链路参数对应的链路参数矩阵Pl可通过下述公式6表示：

其中，ld₁表示第一个直接关联关系l₁的链路距离，bur₁表示第一个直接关联关系l₁的带宽利用率，lfr₁表示第一个直接关联关系l₁的链路故障率；ld₂表示第二个直接关联关系l₂的链路距离，bur₂表示第二个直接关联关系l₂的带宽利用率，lfr₂表示第二个直接关联关系l₂的链路故障率；ld_t表示第t个直接关联关系l_t的链路距离，bur_t表示第t个直接关联关系l_t的带宽利用率，lfr_t表示第t个直接关联关系l_t的链路故障率。

基于主观层次分析法AHP和客观熵权法EWM，确定多个节点参数中各节点参数的权重和多个链路参数中各链路参数的权重。

示例地，在本申请实施例中，位置点对应的多个节点参数的权重矩阵W_n可以通过下述公式7表示：

Wn＝{a₁,a₂,a₃,a₄}公式7

其中，a₁表示多个节点参数中负载能力的权重，a₂表示多个节点参数中单波速率的权重，a₃表示多个节点参数中连通度的权重，a₄表示多个节点参数中计算能力的权重。

示例地，在本申请实施例中，位置点对应的多个链路参数的权重矩阵W_l可以通过下述公式8表示：

Wl＝{b₁,b₂,b₃} 公式8

其中，b₁表示多个链路参数中链路距离的权重，b₂表示多个链路参数中带宽利用率的权重，b₃表示多个链路参数中链路故障率的权重。

示例的，在本申请实施例中，基于主观层次分析法AHP和客观熵权法EWM，确定多个节点参数中各节点参数的权重时，针对各节点参数，基于AHP确定节点参数对应的第一权重，并基于EWM确定节点参数对应的第二权重；将第一权重和第二权重的平均值，确定为节点参数的权重，这样结合主观层次分析法AHP与客观熵权法EWM，共同确定节点参数的权重，可以进一步提高对节点参数评价的准确度。

基于AHP确定节点参数对应的第一权重时，可以先构建该节点参数的判断矩阵，并计算对应的权重向量，再对判断矩阵和权重向量进行一致性检测；若判断矩阵和权重向量不一致，则修订前一次构建的判断矩阵，并重新计算对应的权重向量，并对修订后的判断矩阵和重新计算得到的权重向量进行一致性检测；若判断矩阵和权重向量一致，则进行等级排序，并对排序后的进行一致性检测，若排序后的一致，则得到节点参数对应的第一权重；若排序后的不一致，则修订前一次构建的判断矩阵，并重新执行上述步骤，直至排序后的一致，从而得到节点参数对应的第一权重。

基于EWM确定节点参数对应的第二权重时，可以先对节点参数进行数据规范化处理，并计算规范化处理后的数据的信息熵，再根据信息熵计算信息效用值，并根据信息效用值计算冗余信息熵，从而得到节点参数对应的第二权重。

以计算某一个位置点的节点参数的权重，即负载能力的权重a₁、单波速率的权重a₂、连通度的权重、以及计算能力的权重a₄时，针对该4个权重中的第i个权重，则其权重的计算可通过下述公式9表示：

其中，a_i表示节点参数的4个权重中的第i个权重，wen_i表示基于客观熵权法EWM确定的节点参数的第二权重，wn’_i表示基于主观层次分析法AHP确定的节点参数的第一权重。

结合上述公式9，可以计算得到位置点的多个节点参数中，每一个节点参数的权重。下面，将详细描述如何计算得到位置点的多个链路参数中，每一个链路参数的权重。

示例的，在本申请实施例中，基于主观层次分析法AHP和客观熵权法EWM，确定多个链路参数中各链路参数的权重时，针对各链路参数，基于AHP确定链路参数对应的第三权重，并基于EWM确定链路参数对应的第四权重；将第三权重和第四权重的平均值，确定为链路参数的权重，这样结合主观层次分析法AHP与客观熵权法EWM，共同确定链路参数的权重，可以进一步提高对链路参数评价的准确度。

基于AHP确定链路参数对应的第三权重时，其具体实现与上述基于AHP确定节点参数对应的第一权重的方法类似，可参见上述基于AHP确定节点参数对应的第一权重的相关描述，在此，本申请实施例不再进行赘述。类似的，基于EWM确定链路参数对应的第四权重时，其具体实现与上述基于EWM确定节点参数对应的第二权重的方法类似，可参见上述基于EWM确定节点参数对应的第二权重的相关描述，在此，本申请实施例不再进行赘述。

以计算某一个位置点的链路参数的权重，即链路距离的权重b₁、带宽利用率的权重b₂、以及链路故障率的权重b₃时，针对该3个权重中的第j个权重，则其权重的计算可通过下述公式10表示：

其中，b_j表示链路参数的4个权重中的第j个权重，wel_j表示基于客观熵权法EWM确定的链路参数的第四权重，wl’_j表示基于主观层次分析法AHP确定的链路参数的第三权重。

结合上述公式10，可以计算得到位置点的多个链路参数中，每一个链路参数的权重。

通过基于矢量方法的拓扑压缩方式，保证路由计算结果有解的前提下，与主客观多因子评价结合，执行最优路由计算方法，兼顾路由计算时间上的效率与路由计算结果的最优性。

针对映射网络评价矩阵中的各位置点对，根据位置点对中位置点的多个节点参数和各节点参数的权重，确定位置点的得分，并根据位置点的多个链路参数和各链路参数的权重，确定直接关联关系的得分。

示例地，以确定第i个位置点n_i的得分为例，可以通过下述公式11计算：

en_i＝a₁lc’_i+a₂swr’_i+a₃con’_i+a₄cp’_i 公式11

其中，en_i表示第i个位置点n_i的得分，lc'_i为第i个位置点n_i的负载能力lc_i数据规范后的数据，swr'_i为第i个位置点n_i的单波速率swr_i数据规范后的数据，con'_i为第i个位置点n_i的连通度con_i数据规范后的数据，cp'_i为第i个位置点n_i的计算能力cp_i数据规范后的数据。

示例地，以确定第j个直接关联关系得分为例，可以通过下述公式12计算：

el_j＝b₁ld^' _j+b₂bur^' _j+b₃lfr^' _j公式12

其中，el_j表示第j个直接关联关系得分，ld_j'为第j个直接关联关系的链路距离ld_j数据规范后的数据，bur_j'为第j个直接关联关系的带宽利用率bur_j数据规范后的数据，lfr_j'为第j个直接关联关系的链路故障率lfr_j数据规范后的数据。

将各位置点对中位置点的得分和直接关联关系的得分，补充至各位置点的初始相关信息中，并从补充后的相关信息中剔除直接关联关系的距离，得到映射网络评价矩阵。

以上述公式4所示的映射网络评价矩阵G为例，将各位置点对中位置点的得分和直接关联关系的得分，补充至各位置点的初始相关信息中，并从补充后的相关信息中剔除直接关联关系的距离，得到映射网络评价矩阵可通过上述公式1表示：

步骤206，若所述路径程度为第二路径程度，则获取第一三维模型和第二三维模型；

步骤207，基于所述第一三维模型和所述第二三维模型，构建第二最优规划路径结果；所述第一三维模型为反映目标三维空间内各个内容物的尺寸和相对位置的三维模型，所述目标三维空间的内容物包括至少一个子空间、连接各个所述子空间的通道空间以及障碍体，所述第二三维模型为反映行动对象的尺寸的三维模型。

可选的，若路径程度为第二路径程度，则获取第一三维模型和第二三维模型，并第一三维模型和第二三维模型，构建第二最优规划路径结果；第一三维模型为反映目标三维空间内各个内容物的尺寸和相对位置的三维模型，目标三维空间的内容物包括至少一个子空间、连接各个子空间的通道空间以及障碍体，第二三维模型为反映行动对象的尺寸的三维模型。

获取路径集，路径集中包括多条已规划路径，基于第一三维模型和目标终点，以目标已规划路径的终点为起点生成至少一条样条曲线，将样条曲线分别与目标已规划路径连接，生成多条已规划路径加入至路径集中，并从路径集中删除目标已规划路径。

基于第二三维模型以及第一三维模型对路径集中的各个已规划路径进行碰撞测试，得到碰撞测试结果，基于碰撞测试结果对路径集中的各个已规划路径进行保留或删除操作，碰撞测试结果反映第二三维模型沿样条曲线行进时是否与第一三维模型中的障碍体产生碰撞。

重新执行获取路径集的操作，直至路径集中的已规划路径的终点为目标终点。

基于路径集中的已规划路径确定第二最优规划路径结果，第二最优规划路径结果包括行动对象从目标三维空间内的目标起点到目标终点的行进路径。

每次延长路径集中的已规划路径时，将路径集中的每条已规划路径均分别作为目标已规划路径，进行处理，具体地，基于第一三维模型和目标终点，以目标已规划路径的终点为起点生成至少一条样条曲线，包括：

基于目标已规划路径的终点、目标终点和第一三维模型，确定至少一个延长方向；基于目标已规划路径的终点在第一三维模型中的位置，确定控制点密度；将目标已规划路径的终点作为一个控制点，基于控制点密度在各个延长方向上确定控制点集，控制点集中包括除目标已规划路径的终点之外的至少一个控制点；基于各个延长方向上的控制点集生成各个样条曲线，每个样条曲线的终点为样条曲线对应的延长方向上的控制点集中的最后一个控制点。

在目标已规划路径的终点不是目标终点之前，均需要以目标已规划路径的终点为起点生成样条曲线以对目标已规划路径进行延长。在一种可能的实现方式中，延长方向可以基于深度优先算法确定。在另一种可能的实现方式中，可以预先设置多个候选方向范围，并为每个候选方向范围设置对应的概率，在候选方向范围内按照对应的概率随机采样得到多个延长方向。候选方向范围可以包括第一范围和第二范围，第一范围满足条件：以目标已规划路径的终点为起点，向第一范围内的延长方向延长得到的射线与直接连线的夹角不超过90°，直接连线为目标已规划路径的终点与目标终点的连线，第二范围满足条件：以目标已规划路径的终点为起点，向第一范围内的延长方向延长得到的射线与直接连线的夹角超过90°。第一范围对应的概率大于第二范围对应的概率。

为了及时将路径集中不满足条件的路径删除，本申请提供的方法中，生成的样条曲线并不直接到达目标终点，而是每次延长部分，为了进一步提升路径规划效率，本申请提供的方法中，设置有控制点，在每个控制点处进行碰撞测试。而根据空间内障碍体的密集程度和通道空间的形状，控制点的密度不同，具体地，基于目标已规划路径的终点在第一三维模型中的位置，确定控制点密度，包括：

获取目标已规划路径的终端的预设范围内的环境复杂度，其中，环境复杂度反映第一三维模型在预设范围内的通道空间的形状以及障碍体的密集程度；基于环境复杂度确定控制点密度。

具体地，预设范围可以基于目标三维空间的大小确定，预设范围的大小与目标三维空间的大小正相关，即，目标三维空间越大，预设范围越大，目标三维空间越小，预设范围越小。通道空间的形状复杂度越高，障碍体密集程度越高，那么对应的环境复杂度越高，通道空间的形状复杂度可以通过通道空间的长宽比、弯曲部分个数以及弯曲程度衡量。障碍体密度程度可以通过障碍体占预设范围的比例确定。

将目标已规划路径的终点作为一个控制点，基于确定的控制点密度在延长方向上在目标已规划路径的终点往后设置至少一个控制点。基于延长方向上的各个控制点生成样条曲线，样条曲线通过延长方向上的每个控制点。

每次生成一个样条曲线后，在每个控制点处，执行碰撞测试，碰撞测试通过后，则保留延长的已规划路径，否则将延长的已规划路径从路径集中删除，然后生成新的样条曲线。

基于第二三维模型以及第一三维模型对路径集中的各个已规划路径进行碰撞测试，包括：

控制第二三维模型按照预设的姿态沿已规划路径行进；

若第二三维模型到达已规划路径的终点时，第二三维模型未与第一三维模型中的障碍体产生过碰撞，则停止碰撞测试；

若第二三维模型到达已规划路径的终点前，第二三维模型与第一三维模型中的障碍体产生碰撞，则选择新的姿态，重新执行控制第二三维模型按照预设的姿态沿已规划路径行进的步骤，直至停止碰撞测试，或者已控制第二三维模型按照所有的姿态沿已规划路径行进。

碰撞测试可以通过碰撞测试器，碰撞测试器是根据一定算法，测试物体在某一点是否能够避免与障碍体发生碰撞的执行器，姿态是指行动对象的姿态，比如机器人的水平运行姿态和侧运行姿态，基于行动对象的姿态可行性，可以预设设置多个姿态。控制第二三维模型按照预设的姿态沿已规划路径行进，在到达每个控制点处，调用碰撞测试器进行碰撞测试，如果未发生碰撞则通过该控制点，如果发生碰撞，那么在该控制点处，重新从预设的姿态中选择姿态，再次在该控制点处进行碰撞测试，如果最终可以找到不发生碰撞的姿态，那么，记录不发生碰撞的姿态作为目标姿态，并且结束在该控制点处的碰撞测试，将碰撞测试结果记为通过。如果遍历所有预设的姿态，都不能找到可以不发生碰撞的姿态，那么也结束在该控制点处的碰撞测试，将碰撞测试结果记为不通过。

对于碰撞测试不通过的已规划路径，将其从路径集中删除，碰撞测试通过的已规划路径，在路径集中保留。在每个控制点处进行的第一次碰撞测试时第二三维模型的姿态选择在前一个控制点处通过碰撞测试的姿态，即前一个控制点的目标姿态。

经过生成样条曲线，执行碰撞测试，从路径集中删除或保留已规划路径的过程，实际上是对路径集进行了更新，生成了新的路径集，对于新的路径集，重复这些操作，直到路径集中的所有已规划路径的终点都是目标终点。

当路径集中的所有已规划路径的终点都是目标终点后，基于路径集中的已规划路径确定路径规划结果，具体包括：

对路径集中的各个已规划路径进行评价，得到评价结果；

基于评价结果在路径集中确定至少一条已规划路径作为路径规划结果。

在一种可能的实现方式中，可以基于路径长度对已规划路径进行评价，路径长度越短，已规划路径评价结果越好，这样可以缩短行动对象从目标起点出发到目标终点的时间。由于前面的步骤中碰撞测试是在每个控制点处执行的，为了提升行动对象从目标起点出发达到目标终点的可靠性，在本实施例中，对路径集中的各个已规划路径进行评价，得到评价结果，包括：

基于各个已规划路径的路径长度、已规划路径中的控制点数量以及各个已规划路径中的各个控制点的碰撞测试过程数据对路径集中的各个已规划路径进行评价，得到评价结果。

具体地，控制点的碰撞测试过程数据指的是碰撞测试中的目标姿态，即通过碰撞测试时的姿态，对于一条已规划路径来说，其评价结果的获取过程可以包括：

获取该已规划路径中各个控制点处的目标姿态，得到已规划路径中各个控制点处的目标姿态的变化次数；

对该已规划路径的路径长度、控制点数量、目标姿态的变化次数进行加权求和，得到评价结果；

其中，已规划路径的路径长度的权值为负值，控制点数量的权值为正值，目标姿态的变化次数的权值为负值。

也就是说，已规划路径的路径长度越短、控制点数量越多，目标姿态的变化次数越少，已规划路径的评价结果越好。已规划路径的路径长度越短，可以缩短行动对象的行动时间，已规划路径的控制点数量越多，说明对该路径的碰撞可能性预测结果更加全面，该路径的可靠性更高，已规划路径的目标姿态的变化次数越少，可以降低行动对象修改姿态的次数，提升行动对象的行动效率。

得到路径集中的各个已规划路径的评价结果后，按照评价结果由高到低进行排序，展示排序结果。

在一可选实施例中，基于机器人的感知导航方法还包括：

建立状态方程的第一公式拓扑图，具体包括：

对于若干个要进行估计的量x₁,x₂,…,x_m，每次以其中一个量x_k为待求量，选择若干个已知量构成初始输入向量U_initial，选择机器人模型和机器人外皮模型，通过全局路径搜索得到状态方程的第一公式拓扑图T_xk，并根据状态方程的第一公式拓扑图T_xk确定包含所选待求量的状态向量X_k、由已知量构成的向量U_xk以及所需公式序列F_xk，并由X＝X₁∪X₂…∪X_k…∪X_m得到最终的状态向量X；

建立量测方程的第二公式拓扑图，具体包括：

选择若干个可由机器人传感器直接测量得到，或由前序观测器间接估计得到的物理量z₁,z₂,…,z_n构成量测向量Z；其中，量测向量Z的维数n不应少于状态向量X维数的1/2；依次以z_k作为待求量，以集合X∪U_initial作为已知量；选择机器人模型和机器人外皮模型，通过全局路径搜索得到z_k与已知量之间的公式拓扑图T_yk；由初始输入向量U_initial中的元素构成的局部输入向量U_yk，以及所需的公式序列F_yk，并由U＝U_x1∪U_x2…∪U_xm∪U_y1…∪U_yn得到最终的输入向量U；

基于无迹卡尔曼滤波器以及第一公式拓扑图和第二公式拓扑图进行机器人状态观测，具体包括：

公式序列F_x1～F_xm描述了状态向量X与输入向量U之间的数学关系；公式序列F_y1～F_yn描述了将量测向量Z与状态向量X、输入向量U之间的数学关系；将公式序列F_x1～F_xm和F_y1～F_yn应用于无迹卡尔曼滤波器进行机器人状态观测。

其中，待求量和已知量是通过对与机器人状态观测相关的模型中的变量进行综合和抽象而得到的。本申请将与机器人状态观测相关的模型划分为机器人模型模块、机器人外皮模型模块、运动学模型模块、滑移计算模块和垂向载荷计算模块。定义模块公式序列的难点在于确定公式的形式。一方面，一个公式可能会有多种近似形式；另一方面，不同公式联立后会得到一个复合形式的公式。如果公式序列公式的形式定义的不合理，会导致无法建立不同模块之间物理量之间的联系。本申请对各个模块中每个公式的形式和包含的物理量进行了合理的定义。

进一步地，公式拓扑图利用本申请所提出的路径全局路径搜索算法得到的。公式拓扑图呈向右生长的复杂树状结构，并以所选待求量作为根结点。本申请将任一公式λ₀＝f(λ₁,λ₂,…,λ_m)所描述的数学关系抽象为一个以物理量λ₀为父结点，以其它物理量λ₁,λ₂,…,λ_m为子结点的简单树状结构，并称父节点到子节点存在单向的路径。全局路径搜索的结果中包含多个公式，这些公式构成一个公式序列，并表明了各个公式的采用顺序，因此最终的公式拓扑图是由各个公式所表示的简单树状结构组合构成的复杂树状结构。

进一步地，全局搜索算法是指本申请将机器人状态观测模型的构建问题转化为以待求量为起点，以输入量和状态变量为终点的路径搜索问题，并可实现对存在的所有可能路径的搜索。

进一步地，全局路径搜索包含路径搜索的结束准则。对于每一条分支路径，本申请提出的搜索结束准则是该路径到达了已知的输入量，或者所到达的未知物理量在此条路径上第二次出现，此时，在该条分支路径上停止搜索，并将出现两次的物理量包含在状态向量X中。当所有分支路径上都停止搜索后，全局路径搜索结束。

进一步地，在全局路径搜索结束后，对公式拓扑图进行剪枝。本申请提出的剪枝规则是作为中间节点的物理量，当在公式拓扑图的下部第二次出现后，剪去该物理量后续的分支路径，以使得公式拓扑图更为清晰地表征全局路径搜索的结果和各物理量之间的拓扑关系

本申请将与机器人状态观测相关的模型划分为5个模块，分别为：

①机器人模型模块，包括双轨机器人模型和单轨机器人模型；

②机器人外皮模型模块，包括非线性机器人外皮模型和线性机器人外皮模型；

③运动学模型模块，描述各物理量之间的运动学关系；

④滑移计算模块，描述机器人外皮侧偏角和滑移率与其他物理量的关系；

⑤垂向载荷计算模块，描述机器人外皮垂向载荷与机器人横纵向加速度的关系。在双轨机器人模型中体现了机器人外皮力，包含的公式序列为：

在单轨机器人模型中采用了左右机器人外皮上的合力，其可表示为：

由于本申请实施例定义的机器人外皮模型模块以及垂向载荷计算模块的公式序列中针对的皆是机器人外皮力，因此本申请实施例在单轨机器人模型的公式序列中增加了4个公式，用于建立机器人外皮合力与左右机器人外皮分力的关系：

机器人外皮模型模块包括非线性机器人外皮和线性机器人外皮模型。非线性机器人外皮模型考虑了机器人外皮力与路面附着系数以及滑移率及机器人外皮侧偏角之间的关系，虽然包含了更多的物理量，但对机器人外皮力特性的描述更为精确，其包含的公式序列为：

线性机器人外皮模型包含的公式序列为：

F_xij＝c_xijs_ij (11)

F_yij＝c_yijα_ij (12)

运动学模型模块包含的公式序列为：

滑移计算模块包含的公式序列为：

垂向载荷计算模块包含的公式序列为：

本申请实施例假设机器人的质量、惯量、尺寸参数以及机器人外皮刚度已知，并定义了与机器人状态观测相关的物理量，如表1所列。

表1

其中，本申请实施例对于机器人外皮的转速、滑移率、侧偏角以及路面附着系数分别仅采用1个物理量ω_ij，s_ij，α_ij和μ_ij来表示，以减少物理量的个数。

本申请实施例将机器人状态观测的模型构建问题抽象为以待求量为起点，以状态向量和输入量为终点的路径搜索问题。所定义的公式序列规定了物理量之间的拓扑关系，即作为路径结点的一个物理量是否可以到达另一个物理量。基于所定义的公式序列，所提出的全局路径搜索方法，本申请实例提供的机器人状态观测方法可以在待求量可求的情况下快速建立从起点到终点之间的公式拓扑图。公式拓扑图是一个树状结构，并以状态变量和输入量作为终点。本申请实例提出的确定状态变量的规则是当某个未知物理量在某个分支路径上第二次出现时，停止此条分支上的路径搜索，并将该物理量作为其中一个状态变量。当所有分支上的路径搜索都结束时，全局路径搜索结束，由所有的公式拓扑图可以确定状态向量X、输入向量U，以及建立复合函数X＝F(U)和Z＝G(X，U)所需的公式序列。通过将X＝F(U)和Z＝G(X，U)应用于无迹卡尔曼滤波器，可以实现对状态向量X的求解和机器人状态的观测。

其中，无迹卡尔曼滤波器是一种非线性滤波方法，该方法无需通过求解雅克比矩阵以对非线性方程F和G在估计点处进行线性化逼近，适用于解决具有较强非线性的***的状态观测问题。

以下为利用全局路径搜索方法进行机器人状态观测的一个具体的实施例。

首先，构建状态方程的公式拓扑图。以纵向速度v_x为待求量，令初始输入向量U_initial＝[δ，ω_ij，μ_ij]^T，选择双轨机器人模型和非线性机器人外皮模型，经过全局路径搜索，可以得到如图2所示的公式拓扑图T_x。由公式拓扑图T_x以及本申请实例提出的确定状态变量的规则，可以得到状态变量X＝[v_x，v_y，ω_r，a_x，a_v]^T，局部输入向量U_x＝[δ，ω_ij，μ_ij]^T，以及所需公式序列E_x。其中，公式序列F_x中包含了以下几组公式：

F_zij＝f₁(a_x，a_y) (19)

s_ij＝f₂(v_x，ω_ij) (20)

α_ij＝f₃(v_x，v_y，ω_r，δ) (21)

F_xij＝f₄(μ_ij，F_zij，s_ij，α_ij) (22)

F_yij＝f₅(μ_ij，F_zij，s_ij，α_ij) (23)

a_x＝f₇(F_x11，F_x12，F_x21，F_x22，F_y11，F_y12，δ) (25)

a_y＝f₈(F_x11，F_x12，F_y11，F_y12，F_y21，F_y22，δ) (26)

其次，建立量测方程的公式拓扑图。以可通过机器人IMU直接测量的ω_r，a_x和a_y构成量测向量，因此有Z＝[ω_r，a_x，a_y]^T。由于 公式序列E_x可以得到X＝F(U)，由公式序列E_y1～E_y3得到Z＝G(X，U)，进而采用无迹卡尔曼滤波器对状态向量X进行求解。

为了验证机器人状态观测效果，本申请实例利用CarSim机器人模型进行了双移线工况仿真，并与采用上述步骤得到的机器人状态观测结果进行了对比，分别对比了纵向速度v_x、侧向速度v_v、横摆角速度ω_r、纵向加速度a_x以及侧向加速度a_y的仿真与观测结果。可见，利用本申请实例提供的仿真准确地实现了对机器人状态的观测。

本申请实施例实现了机器人在运行过程中对周围环境的感知，以及根据优化后的环境点云构建出运行感知标注模型，同时，根据实时路径程度，通过不同的路径导航策略规划出最优规划路径结果。

参照图3，图3示例了一种电子设备的实体结构示意图，电子设备可以包括：处理器(processor)310、通信接口(Communications Interface)320、存储器(memory)330和通信总线340，其中，处理器310，通信接口320，存储器330通过通信总线340完成相互间的通信。处理器310可以调用存储器330中的逻辑指令，以执行基于机器人的感知导航方法。

此外，上述的存储器330中的逻辑指令可以通过软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本申请各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。

另一方面，本申请还提供一种计算机程序产品，计算机程序产品包括存储在非暂态计算机可读存储介质上的计算机程序，计算机程序包括程序指令，当所述程序指令被计算机执行时，计算机能够执行上述基于机器人的感知导航方法。

又一方面，本申请还提供一种非暂态计算机可读存储介质，其上存储有计算机程序，该计算机程序被处理器执行时实现以执行上述各提供的基于机器人的感知导航方法。

以上所描述的装置实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。本领域普通技术人员在不付出创造性的劳动的情况下，即可以理解并实施。

通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到各实施方式可借助软件加必需的通用硬件平台的方式来实现，当然也可以通过硬件。基于这样的理解，上述技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品可以存储在计算机可读存储介质中，如ROM/RAM、磁碟、光盘等，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行各个实施例或者实施例的某些部分所述的方法。

最后应说明的是：以上实施例仅用以说明本申请的技术方案，而非对其限制；尽管参照前述实施例对本申请进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本申请各实施例技术方案的精神和范围。

Claims (10)

1.一种基于机器人的感知导航装置，其特征在于，包括机器人控制中心、环境感知模块、路径规划模块、数据获取单元、第一路径导航模块和第二路径导航模块；所述机器人控制中心分别与所述环境感知模块、所述路径规划模块、所述数据获取单元、所述第一路径导航模块和所述第二路径导航模块连接；所述机器人控制中心用于：控制所述环境感知模块、所述路径规划模块、所述数据获取单元、所述第一路径导航模块和所述第二路径导航模块的数据交互；

所述环境感知模块用于：获取机器人按照预定运行路径在运行过程中的待处理环境点云，并对所述待处理环境点云进行投影重建，得到优化环境点云；

所述路径规划模块用于：提取所述优化环境点云中的各个建筑物信息，基于所述建筑物信息进行模型训练，得到运行感知标注模型；

所述数据获取单元用于：获取机器人按照所述预定运行路径在运行过程中的路径程度；所述路径程度表征所述预定运行路径的实时路径程度；所述路径程度包括第一路径程度和第二路径程度，且所述第二路径程度的实时路径程度优于所述第一路径程度的实时路径程度；其中，所述第一路径程度为路径宽度小于或等于预设宽度的路径程度，所述第二路径程度为路径宽度大于预设宽度的路径程度；

所述数据获取单元用于：若所述路径程度为第一路径程度，则获取在运行过程中的起始位置点、中间位置点和终点位置点；

所述第一路径导航模块用于：基于所述起始位置点、所述中间位置点和所述终点位置点之间的关联性，构建第一最优规划路径结果；

所述数据获取单元用于：若所述路径程度为第二路径程度，则获取第一三维模型和第二三维模型；

所述第二路径导航模块用于：基于所述第一三维模型和所述第二三维模型，构建第二最优规划路径结果；所述第一三维模型为反映目标三维空间内各个内容物的尺寸和相对位置的三维模型，所述目标三维空间的内容物包括至少一个子空间、连接各个所述子空间的通道空间以及障碍体，所述第二三维模型为反映行动对象的尺寸的三维模型。

2.一种基于机器人的感知导航方法，其特征在于，包括：

获取机器人按照预定运行路径在运行过程中的待处理环境点云，并对所述待处理环境点云进行投影重建，得到优化环境点云；

提取所述优化环境点云中的各个建筑物信息，基于所述建筑物信息进行模型训练，得到运行感知标注模型；

获取机器人按照所述预定运行路径在运行过程中的路径程度；所述路径程度表征所述预定运行路径的实时路径程度；所述路径程度包括第一路径程度和第二路径程度，且所述第二路径程度的实时路径程度优于所述第一路径程度的实时路径程度；其中，所述第一路径程度为路径宽度小于或等于预设宽度的路径程度，所述第二路径程度为路径宽度大于预设宽度的路径程度；

若所述路径程度为第一路径程度，则获取在运行过程中的起始位置点、中间位置点和终点位置点；

基于所述起始位置点、所述中间位置点和所述终点位置点之间的关联性，构建第一最优规划路径结果；

若所述路径程度为第二路径程度，则获取第一三维模型和第二三维模型；

基于所述第一三维模型和所述第二三维模型，构建第二最优规划路径结果；所述第一三维模型为反映目标三维空间内各个内容物的尺寸和相对位置的三维模型，所述目标三维空间的内容物包括至少一个子空间、连接各个所述子空间的通道空间以及障碍体，所述第二三维模型为反映行动对象的尺寸的三维模型。

3.根据权利要求2所述的基于机器人的感知导航方法，其特征在于，所述对待处理环境点云进行投影重建，得到优化环境点云，包括：

将所述待处理环境点云从RGB空间映射到CIELCh色彩空间，获取所述待处理环境点云在CIELCh色彩空间中的光谱比值点云；

对所述光谱比值点云进行超像素重建，得到重建光谱比点云；

基于自动确定的投影阈值对所述重建光谱比点云进行检测分割，确定投影和非投影点云；

确定所述投影点云的本影区域和半影区域，对所述本影区域进行光照补偿，得到本影重建点云，以及对所述半影区域进行均值滤波，得到半影重建点云，基于所述本影重建点云和半影重建点云确定投影重建后的优化环境点云。

4.根据权利要求2所述的基于机器人的感知导航方法，其特征在于，基于所述建筑物信息进行模型训练，得到运行感知标注模型，包括：

获取所述建筑物信息的标注信息，以及根据所述建筑物信息获取建筑物匹配组；所述建筑物匹配组中包括多个建筑物数据；

计算所述建筑物信息与所述建筑物匹配组中每个建筑物数据之间的匹配置信度；

将所述建筑物匹配组中匹配置信度大于预设置信度阈值的建筑物数据确定为目标建筑物数据；

以所述建筑物信息和所述目标建筑物数据为训练数据对，以所述标注信息为标注训练数据，对预设模型进行模型训练，得到所述运行感知标注模型；

其中，匹配置信度的计算公式为：

其中，S_ij表示第i个建筑物信息和第j个建筑物数据的匹配置信度，表示第i个建筑物信息和第j个建筑物数据之间匹配的轨迹点的数量，/>表示第i个建筑物信息和第j个建筑物数据之间轨迹覆盖的轨迹点的数量，/>表示第i个建筑物信息和第j个建筑物数据之间轨迹重合的轨迹点的数量。

5.根据权利要求2所述的基于机器人的感知导航方法，其特征在于，所述基于所述起始位置点、所述中间位置点和所述终点位置点之间的关联性，构建第一最优规划路径结果，包括：

基于所述起始位置点n_s、所述终点位置点n_t和所述中间位置点n_i构建第一关联表示集合；所述第一关联表示集合中包括所述起始位置点到所述终点位置点之间的第一关联表示，和，所述起始位置点、所述多个中间位置点及所述终点位置点中，相邻两个位置点之间的多个第二关联表示；确定起始位置点n_s到终点位置点n_t之间的第一关联表示为起始位置点n_s到第一个中间位置点之间的第二关联表示为/>…，中间位置点n_i到中间位置点n_j之间的第二关联表示为/>最后一个中间位置点到终点位置点n_t之间的第二关联表示为/>第一关联表示集合V为/>

确定与所述第一关联表示集合关于所述第一关联表示对称的第二关联表示集合 表示起始位置点n_s到终点位置点n_t之间的第一关联表示关于/>对称的关联表示，/>表示起始位置点n_s到第一个中间位置点之间的第二关联表示/>关于/>对称的关联表示，/>表示中间位置点n_i到中间位置点n_j之间的第二关联表示/>关于/>对称的关联表示，/>表示最后一个中间位置点到终点位置点n_t之间的第二关联表示/>关于/>对称的关联表示；

从预设关联映射网络中，确定的第一关联表示集合和第二关联表示集合共同的第一关联区域T₁，从所述预设关联映射网络中，确定的第一关联表示和第二关联表示集合构成的第二关联区域T₂，所述第一关联区域T₁与所述第二关联区域T₂的目标关联区域为T，目标关联区域T表示如下：

其中，Λ_k表示中间位置点n_i到中间位置点n_j之间的第二关联表示和/>构成的区域；

基于所述目标关联区域构建第一最优规划路径结果。

6.根据权利要求5所述的基于机器人的感知导航方法，其特征在于，所述基于所述目标关联区域构建第一最优规划路径结果，包括：

从所述预设关联映射网络对应的映射网络评价矩阵中，剔除包括起始位置点的位置点对的相关信息，得到更新后的映射网络评价矩阵，所述起始位置点为处于所述目标关联区域之外的位置点，所述映射网络评价矩阵中包括所述预设关联映射网络中，所有存在直接关联关系的位置点对的相关信息，所述相关信息包括所述位置点对中各位置点的位置信息和得分，所述直接关联关系的位置信息和得分；

基于所述更新后的映射网络评价矩阵，从所述目标关联区域中确定所述第一最优规划路径结果。

7.根据权利要求2所述的基于机器人的感知导航方法，其特征在于，所述基于所述第一三维模型和所述第二三维模型，构建第二最优规划路径结果，包括：

获取路径集，所述路径集中包括多条已规划路径，基于所述第一三维模型和目标终点，以目标已规划路径的终点为起点生成至少一条样条曲线，将所述样条曲线分别与所述目标已规划路径连接，生成多条已规划路径加入至所述路径集中，并从所述路径集中删除所述目标已规划路径；

基于所述第二三维模型以及所述第一三维模型对所述路径集中的各个已规划路径进行碰撞测试，得到碰撞测试结果，基于所述碰撞测试结果对所述路径集中的各个已规划路径进行保留或删除操作，所述碰撞测试结果反映所述第二三维模型沿所述样条曲线行进时是否与所述第一三维模型中的障碍体产生碰撞；

重新执行所述获取路径集的操作，直至所述路径集中的已规划路径的终点为所述目标终点；

基于所述路径集中的已规划路径确定第二最优规划路径结果，所述第二最优规划路径结果包括所述行动对象从所述目标三维空间内的目标起点到所述目标终点的行进路径。

8.根据权利要求7所述的基于机器人的感知导航方法，其特征在于，所述基于所述第一三维模型和目标终点，以目标已规划路径的终点为起点生成至少一条样条曲线，包括：

基于所述目标已规划路径的终点、所述目标终点和所述第一三维模型，确定至少一个延长方向；

基于所述目标已规划路径的终点在所述第一三维模型中的位置，确定控制点密度；

将所述目标已规划路径的终点作为一个控制点，基于所述控制点密度在各个所述延长方向上确定控制点集，所述控制点集中包括除所述目标已规划路径的终点之外的至少一个控制点；

基于各个所述延长方向上的所述控制点集生成各个所述样条曲线，每个所述样条曲线的终点为所述样条曲线对应的所述延长方向上的所述控制点集中的最后一个控制点；

相应地，基于所述第二三维模型以及所述第一三维模型对所述路径集中的各个已规划路径进行碰撞测试，包括：

控制所述第二三维模型按照预设的姿态沿已规划路径行进；

若所述第二三维模型到达已规划路径的终点时，所述第二三维模型未与所述第一三维模型中的障碍体产生碰撞，则停止所述碰撞测试；

若所述第二三维模型到达到已规划路径的终点前，所述第二三维模型与所述第一三维模型中的障碍体产生碰撞，则选择新的姿态，重新执行所述控制所述第二三维模型按照预设的姿态沿已规划路径行进的步骤，直至停止所述碰撞测试或者已控制所述第二三维模型按照所有的姿态沿已规划路径行进。

9.根据权利要求2所述的基于机器人的感知导航方法，其特征在于，所述基于机器人的感知导航方法还包括：

建立状态方程的第一公式拓扑图，以及建立量测方程的第二公式拓扑图；

基于无迹卡尔曼滤波器以及所述第一公式拓扑图和所述第二公式拓扑图进行机器人状态观测；

所述建立状态方程的第一公式拓扑图，具体包括：

对于若干个要进行估计的量x₁,x₂,…,x_m，每次以其中一个量x_k为待求量，选择若干个已知量构成初始输入向量U_initial，选择机器人模型和机器人外皮模型，通过全局路径搜索得到状态方程的第一公式拓扑图T_xk，并根据状态方程的第一公式拓扑图T_xk确定包含所选待求量的状态向量X_k、由已知量构成的向量U_xk以及所需公式序列F_xk，并由X＝X₁∪X₂…∪X_k…∪X_m得到最终的状态向量X；

所述建立量测方程的第二公式拓扑图，具体包括：

选择若干个可由机器人传感器直接测量得到，或由前序观测器间接估计得到的物理量z₁,z₂,…,z_n构成量测向量Z；其中，量测向量Z的维数n不应少于状态向量X维数的1/2；依次以z_k作为待求量，以集合X∪U_initial作为已知量；选择机器人模型和机器人外皮模型，通过全局路径搜索得到z_k与已知量之间的公式拓扑图T_yk；由初始输入向量U_initial中的元素构成的局部输入向量U_yk，以及所需的公式序列F_yk，并由U＝U_x1∪U_x2…∪U_xm∪U_y1…∪U_yn得到最终的输入向量U；

所述基于无迹卡尔曼滤波器以及所述第一公式拓扑图和所述第二公式拓扑图进行机器人状态观测，具体包括：

公式序列F_x1～F_xm描述了状态向量X与输入向量U之间的数学关系；公式序列F_y1～F_yn描述了将量测向量Z与状态向量X、输入向量U之间的数学关系；将公式序列F_x1～F_xm和F_y1～F_yn应用于无迹卡尔曼滤波器进行机器人状态观测。

10.一种非暂态计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求2至9任一项所述基于机器人的感知导航方法。