CN114004926B - 基于障碍物几何信息的星球车运动模式选择方法 - Google Patents

Abstract

本发明提供了一种基于障碍物几何信息的星球车运动模式选择方法，该方法包括：获取障碍物的三维点云数据，根据所述三维点云数据生成障碍物几何信息和确定所述障碍物与星球车的相对位置情况，其中，所述障碍物几何信息包括所述障碍物的高度特征、表面形貌特征和纵向轮廓特征；根据所述障碍物几何信息和所述相对位置情况确定所述星球车的运动模式，以进行越障。本发明的有益效果：能够基于障碍物几何信息的确定，选择合适的星球车运动模式进行障碍物的越障。

Description

基于障碍物几何信息的星球车运动模式选择方法

技术领域

本发明涉及越障控制技术领域，具体而言，涉及一种基于障碍物几何信息的星球车运动模式选择方法。

背景技术

在星球探测环境中，星球车，例如具有主动和被动悬架的火星车对于面前障碍的判断极为重要，它直接决定了星球车的通过方式。通常情况，对于大障碍物选择绕行通过，对于小障碍物选择翻越或爬越通过。但实际工程中，为避免发生危险，往往选择绕行，这主要因为缺少障碍物的具体信息。

相关技术中，基于视觉的障碍物描述从而进行星球车的越障，其中，对障碍物描述多使用包络的形式，即使用三角形、方形、圆形包络障碍物外形，从而大致得出障碍物形状。但该方式通常对障碍物形状的描述不够准确，且得到的形状也难以直接供星球车进行越障方式的评估。

发明内容

本发明旨在至少在一定程度上解决相关技术中的技术问题，为达上述目的，本发明提供了一种基于障碍物几何信息的星球车运动模式选择方法，包括步骤：

获取障碍物的三维点云数据；

根据所述三维点云数据生成障碍物几何信息和确定所述障碍物与星球车的相对位置情况，其中，所述障碍物几何信息包括所述障碍物的高度特征、表面形貌特征和纵向轮廓特征；

根据所述障碍物几何信息和所述相对位置情况确定所述星球车的运动模式，以进行越障。

进一步地，所述表面形貌特征包括所述障碍物的表面与理想接触面的形状偏差；所述根据所述三维点云数据生成障碍物几何信息包括：

确定所述三维点云数据中各个三维节点到所述理想接触面之间的垂直距离；

确定各个所述垂直距离平方和的平均值，其中，所述形状偏差为所述平均值。

进一步地，所述表面形貌特征包括所述障碍物的拟合表面与车轮理想运动平面的方向偏差，所述方向偏差包括俯仰偏差和横滚偏差；所述根据所述三维点云数据生成障碍物几何信息包括：

将所述三维点云数据的三维节点拟合得到所述拟合表面的拟合表面方程；

根据所述拟合表面方程确定所述俯仰偏差和横滚偏差。

进一步地，所述表面形貌特征包括所述障碍物的尖锐凸点偏差；所述根据所述三维点云数据生成障碍物几何信息包括：

根据所述三维点云数据的三维节点构建坐标点网格，其中，所述三维节点构成所述坐标点网格的节点；

确定所述障碍物的尖锐凸点，其中，当所述三维节点的z向坐标值与相邻的三维节点的z向坐标值的差值符合预设情况时，判定为所述尖锐凸点；

根据所述尖锐凸点的数量和/或锋利度，确定所述尖锐凸点偏差。

进一步地，所述尖锐凸点偏差包括尖锐凸点总数、尖锐凸点密度、最高尖锐凸点数量、尖锐凸点平均锋利度和最高尖锐点锋利度值变化范围中的至少一项，其中，当所述尖锐凸点位于所述星球车的前进方向处，且所述尖锐凸点为所述坐标点网格的同一行中高度最高的节点时，判定所述尖锐凸点为最高尖锐凸点；所述尖锐凸点的锋利度的确定方法包括：

确定所述尖锐凸点的z向坐标值与相邻的三维节点的z向坐标值的差值的最小值；

根据所述最小值和所述坐标点网格的网格间距确定所述锋利度。

进一步地，所述根据所述障碍物几何信息和所述相对位置情况确定所述星球车的运动模式，以进行越障包括：

当所述障碍物位于所述星球车的行进方向上车轮的单侧或双侧，且所述障碍物的最高高度小于或等于第一预设高度，以及所述表面形貌特征满足预设形貌要求时，和/或

当所述障碍物位于所述行进方向上所述车轮的内侧，且所述最高高度小于或等于第二预设高度时，确定所述星球车的运动模式为：维持基础运动模式进行越障；其中，所述第二预设高度大于所述第一预设高度；

否则，确定所述星球车的运动模式为：由所述基础运动模式切换至越障运动模式进行越障。

进一步地，所述越障运动模式包括单侧抬轮翻越模式，其中，当所述障碍物位于所述行进方向上所述车轮的单侧，且所述最高高度小于或等于所述第一预设高度，以及所述表面形貌特征不满足预设形貌要求，并且所述纵向轮廓特征满足预设轮廓要求时，和/或

当所述障碍物位于所述行进方向上所述车轮的单侧，且所述最高高度大于或等于第一预设高度且小于或等于第四预设高度时，确定所述星球车的运动模式为：由所述基础运动模式切换至所述单侧抬轮翻越模式进行越障。

进一步地，所述越障运动模式包括双侧抬轮翻越模式和双侧抬轮爬越模式，其中，当所述障碍物位于所述行进方向上所述车轮的双侧，且所述最高高度小于或等于第五预设高度，以及所述表面形貌特征不满足所述预设形貌要求，并且所述纵向轮廓特征满足所述预设轮廓要求时，确定所述星球车的运动模式为：由所述基础运动模式切换至所述双侧抬轮翻越模式进行越障，其中，所述第五预设高度大于所述第一预设高度；

当所述障碍物位于所述行进方向上所述车轮的双侧，且所述最高高度大于或等于所述第一预设高度及小于或等于第六预设高度，以及所述表面形貌特征满足所述预设形貌要求时，确定所述星球车的运动模式为：由所述基础运动模式切换至所述双侧抬轮爬越模式进行越障，其中，所述第六预设高度大于所述第五预设高度。

进一步地，所述越障运动模式包括蟹行模式和升降模式，其中，当所述表面形貌特征不满足所述预设形貌要求，并且所述纵向轮廓特征不满足所述预设轮廓要求时，和/或

当所述最高高度大于各所述相对位置情况对应的预设超高高度时，确定所述星球车的运动模式为：由所述基础运动模式切换至所述蟹行模式，以用于使所述星球车绕过所述障碍物进行越障；

当所述障碍物位于所述行进方向上所述车轮的内侧，且所述最高高度大于或等于所述第二预设高度及小于或等于第三预设高度时，确定所述星球车的运动模式为：由所述基础运动模式切换至所述升降模式进行越障。

进一步地，所述根据所述障碍物几何信息和所述相对位置情况确定所述星球车的运动模式，以进行越障前，还包括步骤：

获取车体姿态特征；

当所述车体姿态特征符合预设情况时，根据所述障碍物几何信息和所述相对位置情况确定所述星球车的运动模式，以进行越障；

其中，所述车体姿态特征包括所述星球车在所述障碍物表面行进时的车体倾斜偏差，所述车体倾斜偏差的确定方法包括：

根据所述三维点云数据确定所述星球车的两侧车轮和/或前后车轮在前进过程中各个阶段的高度差，其中，多个所述高度差的最大值为所述车体倾斜偏差。

本发明所述的基于障碍物几何信息的星球车运动模式选择方法，基于三维点云数据能够生成障碍物几何信息，例如障碍物的高度特征、表面形貌特征和纵向轮廓特征等障碍物几何信息，以更加详细准确地描述障碍物的外形，同时能够确定所述障碍物与星球车的相对位置情况，以此便于星球车对越障进行评估，以便于直接作为确定越障的运动模式的依据，进而星球车能够通过选择合适的星球车运动模式，以进行越障，以此提高星球车在不同地形下的通过性。

附图说明

图1为本发明实施例中基于障碍物几何信息的星球车运动模式选择方法的流程示意图；

图2为本发明实施例中星球车的运行过程示意图；

图3为本发明实施例中岩石与车轮接触面示意图；

图4为本发明实施例中坐标点网格的示意图一；

图5为本发明实施例中坐标点网格的示意图二；

图6为本发明实施例中岩石整体表面与车轮理想运动平面之间的偏斜程度的示意图；

图7为本发明实施例中车体两侧车轮与路面接触示意图；

图8为本发明实施例中岩石的仿真场景模型示意图；

图9为本发明实施例中岩石的三维平面几何结果示意图，图中1-4表示尖锐凸点；

图10为本发明实施例中右侧车轮与障碍物接触区域的三维点云数据的点云图及拟合得到的平面示意图；

图11为本发明实施例中左侧车轮与障碍物接触区域的三维点云数据的点云图及拟合得到的平面示意图；

图12为本发明实施例中所述星球车的运动模式选择的示意图。

具体实施方式

为使本发明的上述目的、特征和优点能够更为明显易懂，下面结合附图对本发明的具体实施例做详细的说明。

在本发明的描述中，需要说明的是，除非另有明确的规定和限定，术语“设置”、“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以具体情况理解上述术语在本发明中的具体含义。

在本说明书的描述中，参考术语“实施例”、“一个实施例”和“一个实施方式”等的描述意指结合该实施例或实施方式描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示实施方式中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或实施方式。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或实施方式以合适的方式结合。

参照图1所示，本发明实施例提出了一种基于障碍物几何信息的星球车运动模式选择方法，包括步骤：

S1、获取障碍物的三维点云数据；

S2、根据所述三维点云数据生成障碍物几何信息和确定所述障碍物与星球车的相对位置情况，其中，所述障碍物几何信息包括所述障碍物的高度特征、表面形貌特征和纵向轮廓特征；

S3、根据所述障碍物几何信息和所述相对位置情况确定所述星球车的运动模式，以进行越障。

相关技术中，使用三角形、方形、圆形包络障碍物外形，从而大致得出障碍物形状，进而用于供星球车根据得到的障碍物形状进行越障，但是，采用该方式往往对障碍物的实际形状特征描述不够准确，如障碍物极其不规则，并不能简单地采用多种形状组合得到障碍物的形状，若将障碍物划分为多个足够小的规则形状进行组合，则会极大地提高计算量。“并且形状包络，不能准确地保证运动平面的凹凸程度符合要求”，同时，基于包络得到的障碍物形状特征难以直接作为星球车进行越障判断的直接参数，以此不便于供星球车进行越障方式的评估。

本发明实施例中的基于障碍物几何信息的星球车运动模式选择方法，参照图2所示，在星球车，例如火星车可通过雷达等设备进行三维点云数据的获取，其中，可以理解，三维点云数据即为能够详细表示障碍物外观表面的点(下称三维节点)的数据集，并且每个三维节点均具有对应的三维坐标，由此可以根据该三维点云数据进行障碍物的几何信息的生成，障碍物几何信息可以是障碍物的高度特征、表面形貌特征和纵向(与星球车前进方向垂直的方向)轮廓特征等几何信息，以及可以根据三维点云数据确定障碍物与星球车的相对位置情况。由于三维点云数据能够较为准确地表现出障碍物的外形，且各个三维节点的三维坐标便于进行数学计算，由此，其能够确定便于进行越障策略的表面形貌特征等数学参数以及根据各三维坐标准确确定星球车与障碍物的相对位置情况，进而便于进行越障方式的评估。

由此，本实施例中，基于三维点云数据能够生成障碍物几何信息，例如障碍物的高度特征、表面形貌特征和纵向轮廓特征等障碍物几何信息，以更加详细准确地描述障碍物的外形，同时能够确定所述障碍物与星球车的相对位置情况，以此便于星球车对越障进行评估，以便于直接作为确定越障的运动模式的依据，进而星球车能够通过选择合适的星球车运动模式，以进行越障，以此提高星球车在不同地形下的通过性。

本发明实施例中，高度特征可以包括障碍物的最高高度。纵向轮廓特征可以包括障碍物纵向的起伏情况等。表面形貌特征为障碍物相对星球车的前视和俯视的表面特征，具体地，包括用于表示障碍物与星球车接触时的障碍物表面的形状偏差和方向偏差，以及包括能够表示障碍物上会对星球车进行干扰的凸起特征，如尖锐凸点偏差等，以此便于星球车评估车轮在越障时与障碍物的接触状态，例如若岩石等障碍物较尖则容易损伤车轮的机械结构，若障碍物表面倾斜过大，也容易使越障轮发生打滑或失稳。其中，方向偏差可包括俯仰偏差和横滚偏差，尖锐凸点偏差包括尖锐凸点总数、尖锐凸点密度、最高尖锐凸点数量、尖锐凸点平均锋利度和最高尖锐点锋利度值变化范围。以障碍物为岩石为例，默认岩石表面轮廓与星球车的车轮轴向剖面的外轮廓完全重合，岩石表面光滑无凸点，岩石整体不偏不斜为最佳的岩石表面形貌。而现实中不会存在如此理想的岩石，所以本发明实施例中采用量化偏差值来描述现实岩石表面形貌特征与理想岩石之间的差别，以此便于星球车进行越障的判断。

在不选择绕行的情况下进行越障时，星球车主要是以轮体与障碍物进行接触，通常情况下障碍物与星球车的相对位置情况可划分为在星球车前进方向上，车轮单侧具有障碍物、车轮双侧具有障碍物，以及还存在障碍物位于量车轮的内侧情况等等。在相关实施例中，也可确定当前星球车与障碍物相对距离等，以便于进行及时切换运动模式进行越障。

本发明实施例中，轮式模式为星球车行进的基础运动模式，在进行障碍物几何信息确定以及相对位置确定后，可将模式进行切换，如切换为双侧抬轮爬越运动模式、双侧抬轮翻越运动模式、单侧抬轮翻越运动模式、升降运动模式、蟹行模式等，在越过障碍物后，再切换为轮式模式，或保持轮式模式进行越障，由此即实现星球车的运动模式的选择，以能够合理进行越障。

在一可选的实施例中，获取障碍物的三维点云数据包括：获取障碍物在世界坐标系中的原始三维点云数据；根据转化矩阵将所述原始三维点云数据转换为在星球车的机器人运动坐标系中的所述三维点云数据。以此能够便于确定上述的障碍物几何信息以及相对位置情况。

在一个可选的实施例中，所述表面形貌特征包括所述障碍物的表面与理想接触面的形状偏差；所述根据所述三维点云数据生成障碍物几何信息包括：

确定所述三维点云数据中各个三维节点到所述理想接触面之间的垂直距离；

确定各个所述垂直距离平方和的平均值，其中，所述形状偏差为所述平均值。

参照图3所示，其为岩石的障碍物与车轮接触面示意图，其中N₁-N₆表示岩石表面的凸点，这些点同样也属于三维点云数据中的三维节点，h₁表示三维节点N₁与车轮理想接触面(曲面)的垂直距离。

其中，障碍物的表面与理想接触面的形状偏差用来表示岩石整个表面与理想接触面之间的形状差距，表征岩石表面形状与理想接触面表面形状的接近程度，本实施例中，该形状偏差为岩石表面三维节点与理想接触面之间垂直距离平方和的平均值，在一个可选的实施例中，具体的确定公式为：

其中，ffa表示所述形状偏差，为方便确定三维节点的位置以及数量，在本发明的一个可选的实施例中，参照图4和图5所示，根据三维节点构建坐标点网格，其中，所述三维节点构成所述坐标点网格的节点，对于图4和图5中大致呈矩形的坐标点网格，其具有的列数和行数即分别为n和m，此时n×m即三维节点总数量，h_i,j即表示第i行第j列三维节点与理想接触面的垂直距离。在相关实施例中，具体的确定公式可进行适应性调整。

在本发明的一个可选的实施例中，所述表面形貌特征包括所述障碍物的拟合表面与车轮理想运动平面的方向偏差，所述方向偏差包括俯仰偏差和横滚偏差；所述根据所述三维点云数据生成障碍物几何信息包括：

将所述三维点云数据的三维节点拟合得到所述拟合表面的拟合表面方程；

根据所述拟合表面方程确定所述俯仰偏差和横滚偏差。

参照图6所示，方向偏差用来描述岩石整体表面与车轮理想运动平面之间的偏斜程度，其中，车轮理想运动平面为车轮行驶理想平面，本实施例中为水平面，基于偏斜方向，方向偏差可分为俯仰偏差和横滚偏差。

本实施例中，基于三维点云数据的三维节点，可以拟合为以相关度较大的空间平面，即所述拟合表面，并具备有拟合表面方程，本实施例中，拟合表面方程为：

D＝Ax+By+Cz；

其中，A、B、C、D为常数，对应地，以车轮理想运动平面为水平面为例，确定的俯仰偏差为：f_tilt＝arctan(|A/C|)×180/π；确定的横滚偏差为：f_roll＝arctan(|B/C|)×180/π。

在本发明的一个可选的实施例中，所述表面形貌特征包括所述障碍物的尖锐凸点偏差；所述根据所述三维点云数据生成障碍物几何信息包括：

根据所述三维点云数据的三维节点构建坐标点网格，其中，所述三维节点构成所述坐标点网格的节点；

确定所述障碍物的尖锐凸点，其中，当所述三维节点的z向坐标值与相邻的三维节点的z向坐标值的差值符合预设情况时，判定为所述尖锐凸点；

根据所述尖锐凸点的数量和/或锋利度，确定所述尖锐凸点偏差。

由于车轮与障碍物，例如岩石的接触可以看做是两个刚体的接触，现实中两个刚体接触之间多为点接触，几乎没有面接触。所以岩石与车轮的局部接触状态取决于岩石表面凸点的几何分布状态，由于车轮本身存在一定的结构刚度，并不是所有的岩石“凸点”均会对车轮表面造成影响。只有当车轮与一定锋利度的凸点接触时，才会对车轮的行走稳定性或表面寿命产生影响。因此，本实施例中，确定的表面形貌特征包括障碍物的尖锐凸点偏差，其中尖锐凸点为锋利度较大的凸点，以此便于更直观准确地判断星球车车轮与障碍物的接触情况，进而便于更合理地确定出越障策略。

本发明实施例中，参照图4和图5所示，通过将三维点云数据的三维节点构建坐标点网格，以便于确定障碍物处的凸点，以及确定尖锐凸点。其中，以点A为例，若A点与坐标点网格中前后左右等四个相邻的点相比，其z向的坐标高于其它四个点，即该A点可以被视为凸点，本发明实施例中，用N_i，j表示凸点。若该A点的z向坐标减去弹性阈值k_h后，仍大于前后左右等四个相邻的z向坐标(符合预设情况)，则该A点可以被视为尖锐凸点本发明实施例中，用ΛN_i，j表示尖锐凸点。

可以理解，对于一个三维节点的三维坐标，其包括x、y、z三个方向的坐标值，z向即通常为竖直方向。

基于在三维节点中确定出尖锐凸点，可以进行尖锐凸点的尖锐度确定以及进行尖锐凸点数量的确定，以此用于确定尖锐凸点偏差，便于进行星球车与障碍物接触情况的评估，进而便于合理地确定越障策略。

本发明实施例中，凸点的尖锐度通过以下公式进行确定：

SN_i，j＝min(z_i，j-z_i+1，j，z_i，j-z_i-1，j，z_i，j-z_i，j+T，z_i，j-z_i，j-T)/step；

其中，SN_i，j表示第i行j列凸点N_i，j的锋利度，本实施例中，为了方便表述，若确定出凸点为尖锐凸点，则用^ΛSN_i，j表示尖锐凸点的锋利度；T表示与N_i，j点同列的点的周期数；step表示x方向(星球车的行进方向)上的网格间距。

在本发明的一个可选的实施例中，所述尖锐凸点偏差包括尖锐凸点总数、尖锐凸点密度、最高尖锐凸点数量、尖锐凸点平均锋利度和最高尖锐点锋利度值变化范围中的至少一项，其中，当所述尖锐凸点位于所述星球车的前进方向处，且所述尖锐凸点为所述坐标点网格的同一行中高度最高的节点时，判定所述尖锐凸点为最高尖锐凸点。

尖锐凸点的几何分布是影响车轮与岩石局部接触状态的主要因素，尖锐凸点总数、尖锐凸点密度可以表征尖锐凸点在岩石上“量”的分布，对于车轮与障碍物实际接触而言，若星球车的行进方向上，在坐标点网格中某行(与行进方向垂直的方向)的某个尖锐凸点为该行所有点的最高点时，则该尖锐凸点可视为最高尖锐凸点，本实施例中，用表示，当车轮行驶到该行时，该最高尖锐凸点为对车轮与障碍物接触影响最大的点，通过统计确定最高尖锐凸点数量能够更好地表征车轮与障碍物的接触情况。

本发明的一个可选的实施例中，尖锐凸点总数的确定公式包括：

H_k＝Number(^ΛN_i，j)i∈(1，m)，j∈(1，n)。

尖锐凸点密度的确定公式包括：

尖锐凸点的锋利度的确定方法包括：

确定所述尖锐凸点的z向坐标值与相邻的三维节点的z向坐标值的差值的最小值；

根据所述最小值和所述坐标点网格的网格间距确定所述锋利度。

本实施例中，尖锐凸点平均锋利度可以表征尖锐凸点在岩石上“质”的分布，以及根据最高尖锐点锋利度值变化范围能够便于确定车轮与障碍物的接触情况。

其中，尖锐凸点平均锋利度的确定公式包括：

其中，sm，sn表示尖锐凸点i，j向行列数。

最高尖锐点锋利度值变化范围可以基于最高尖锐点锋利度值确定后进行统计得到。

在本发明的一个可选的实施例中，所述根据所述障碍物几何信息和所述相对位置情况确定所述星球车的运动模式，以进行越障包括：

当所述障碍物位于所述星球车的行进方向上车轮的单侧或双侧，且所述障碍物的最高高度小于或等于第一预设高度，以及所述表面形貌特征满足预设形貌要求时，和/或

当所述障碍物位于所述行进方向上所述车轮的内侧，且所述最高高度小于或等于第二预设高度时，确定所述星球车的运动模式为：维持基础运动模式进行越障；其中，所述第二预设高度大于所述第一预设高度；

否则，确定所述星球车的运动模式为：由所述基础运动模式切换至越障运动模式进行越障。

参照图12所示，图中h1-h6分别表示下述实施例中的第一预设高度、第二预设高度、第三预设高度、第四预设高度、第五预设高度和第六预设高度，h表示障碍物的最高高度。

本实施例中的星球车在行进时，采用轮式模式等基础运动模式进行行进，在星球车运行时，对于部分障碍物，维持该基础运动模式同样即可确保越障成功。

基于此，具体地，本实施例中在进行越障时的运动模式确定时，若取得障碍物位于车轮单侧或双侧，且0≤h≤h1，以及表面形貌特征满足预设形貌要求，抑或者障碍物位于车轮内侧，且h≤h2，此时均可表示障碍物对星球车的干扰较小，由此可保持基础运动模式直接进行越障。否则，则判定障碍物对星球车会造成一定的影响，此时则需在越障时将进行星球车运行方式的切换，即采用基础运动模式切换至适合越障的越障运动模式的整体运动模式，进而实现越障，越障后再切换至基础运动模式。

其中，越障运动模式可包括单侧抬轮翻越模式、双侧抬轮翻越模式、双侧抬轮爬越模式、蟹行模式和升降模式等。

在本发明的一个可选的实施例中，所述越障运动模式包括单侧抬轮翻越模式，其中，当所述障碍物位于所述行进方向上所述车轮的单侧，且所述最高高度小于或等于所述第一预设高度，以及所述表面形貌特征不满足预设形貌要求，并且所述纵向轮廓特征满足预设轮廓要求时，和/或

当所述障碍物位于所述行进方向上所述车轮的单侧，且所述最高高度大于或等于第一预设高度且小于或等于第四预设高度时，确定所述星球车的运动模式为：由所述基础运动模式切换至所述单侧抬轮翻越模式进行越障。

本实施例中，当障碍物与星球车的相对位置情况为，所述障碍物位于所述行进方向上所述车轮的单侧时，可采用的越障运动模式包括对于该侧进行抬轮的单侧抬轮翻越模式，其中，参照图12所示，当h≤h1，即表示该障碍物高度较小，表面形貌特征不满足预设形貌要求，则表示该障碍物容易对车轮造成影响，以及所述纵向轮廓特征满足预设轮廓要求，以表示星球车容易翻越通过，此时，即可采用基础运动模式切换至所述单侧抬轮翻越模式进行越障。相对应地，当障碍物位于所述行进方向上所述车轮的单侧，且h1≤h≤h4，同样可采取基础运动模式切换至所述单侧抬轮翻越模式进行越障。

在本发明的一个可选的实施例中，所述越障运动模式包括双侧抬轮翻越模式和双侧抬轮爬越模式，其中，当所述障碍物位于所述行进方向上所述车轮的双侧，且所述最高高度小于或等于第五预设高度，以及所述表面形貌特征不满足所述预设形貌要求，并且所述纵向轮廓特征满足所述预设轮廓要求时，确定所述星球车的运动模式为：由所述基础运动模式切换至所述双侧抬轮翻越模式进行越障，其中，所述第五预设高度大于所述第一预设高度；

当所述障碍物位于所述行进方向上所述车轮的双侧，且所述最高高度大于或等于所述第一预设高度及小于或等于第六预设高度，以及所述表面形貌特征满足所述预设形貌要求时，确定所述星球车的运动模式为：由所述基础运动模式切换至所述双侧抬轮爬越模式进行越障，其中，所述第六预设高度大于所述第五预设高度。

本实施例中，对于障碍物与星球车的相对位置情况为，所述障碍物位于所述行进方向上所述车轮的两侧的情况。可采用基础运动模式切换至双侧抬轮翻越模式或双侧抬轮怕越模式的运动模式进行越障。

具体地，参照图12所示，当h0≤h≤h1，不满足预设形貌要求，但满足预设轮廓要求，则采用基础运动模式切换至双侧抬轮翻越模式的运动模式进行越障。

当h1≤h≤h5，满足预设形貌要求时，则采用基础运动模式切换至双侧抬轮爬越模式的运动模式进行越障。

当h1≤h≤h5，不满足预设形貌要求，但满足预设轮廓要求时，则采用基础运动模式切换至双侧抬轮翻越模式的运动模式进行越障。

当h1≤h≤h6，满足预设形貌要求时，同样采用基础运动模式切换至双侧抬轮爬越模式的运动模式进行越障。

在本发明的一个实施例中，所述越障运动模式包括蟹行模式和升降模式，其中，当所述表面形貌特征不满足所述预设形貌要求，并且所述纵向轮廓特征不满足所述预设轮廓要求时，和/或

当所述最高高度大于各所述相对位置情况对应的预设超高高度时，确定所述星球车的运动模式为：由所述基础运动模式切换至所述蟹行模式，以用于使所述星球车绕过所述障碍物进行越障；

当所述障碍物位于所述行进方向上所述车轮的内侧，且所述最高高度大于或等于所述第二预设高度及小于或等于第三预设高度时，确定所述星球车的运动模式为：由所述基础运动模式切换至所述升降模式进行越障。

本实施例中，星球车还可设定有蟹行模式，从而由基础运动模式切换至所述蟹行模式进行越障，当判定障碍物会对星球车造成较为严重的干扰时，则可切换至蟹行模式以使星球车绕过所述障碍物进行越障。

参照图12所示，其中，各所述相对位置情况对应的预设超高高度可包括对应为障碍物位于车轮单侧的第四预设高度h4、车轮双侧的第六预设高度h6、车轮内侧的第三预设高度h3。由此，当最高高度大于这些高度时，采用蟹行模式进行越障。

另外，当障碍物在车轮单侧，且h≤h1，此时不满足预设形貌要求以及不满足轮廓要求，可切换至蟹行模式进行越障。当在车轮双侧，h≤h1，此时不满足预设形貌要求以及不满足轮廓要求，可切换至蟹行模式进行越障，以及在h1≤h≤h5，并且不满足预设形貌要求以及不满足轮廓要求，可切换至蟹行模式进行越障。另外在车轮双侧，若障碍物高度较高，如h5≤h≤h6，此时仅不满足预设形貌要求即采用蟹行模式进行越障。

本实施例中的一种星球车的蟹行模式为：星球车的所有车轮由前进朝向顺时针或逆时针同向旋转90度，使车体可以横向移动，从而实现能够绕过所述障碍物。

相关实施例中，当障碍物在车轮内侧，h2≤h≤h3，表明障碍物高度较高，且小于车体可抬升的最大高度，此时可采用升降模式进行越障。

其中，h1-h6可根据实际星球车的结构形式以及影响因素进行实验后设定，例如对于判定是否采用升降模式越障的第三预设高度h3，其可以是星球车的车体可抬升的最大高度，或小于该最大高度等，在此不作限定。本实施例中，这些预设高度为相对地面的高度值，以作为各个越障运动模式的判断标准，结合火星模拟试验车机械结构，可分别设定为75、250、574、350、350、450mm。

在本发明的一个可选的实施例中，所述根据所述障碍物几何信息和所述相对位置情况确定所述星球车的运动模式，以进行越障前，还包括步骤：

获取车体姿态特征；

当所述车体姿态特征符合预设情况时，根据所述障碍物几何信息和所述相对位置情况确定所述星球车的运动模式，以进行越障；

其中，所述车体姿态特征包括所述星球车在所述障碍物表面行进时的车体倾斜偏差，所述车体倾斜偏差的确定方法包括：

根据所述三维点云数据确定所述星球车的两侧车轮和/或前后车轮在前进过程中各个阶段的高度差，其中，多个所述高度差的最大值为所述车体倾斜偏差。

本实施例中，在根据所述障碍物几何信息和所述相对位置情况确定所述星球车的运动模式，以进行越障前，可预估星球车在障碍物处的车体姿态特征，以便于运动模式的选择，其中，所述车体姿态特征包括车体倾斜偏差，当车体倾斜偏差小于预设值时，进行表面形貌特征的对比，而确定越障策略。即车体倾斜偏差小时，表明车体与地面处于平行姿态，根据障碍物表面形貌特征确定越障策略即可；车体偏差大时，越障过程中，可能出现个别车轮不能触碰地面，如车轮碾压失效等，造成车体倾覆或滑动风险，因此可直接选择进行绕过。

可以设定为越障模式策略选择表，通过实际试验，例如仿真试验，确定不同表面形貌特征参数值区间范围对应不同的越障策略进行仿真，从而在获得该表面形貌特征后确定较好的越障策略，最终形成表格数据形式，用于作为越障策略的对比选择。

其中，所述获取星球车的车体姿态特征包括：确定所述星球车的两侧车轮和/或前后车轮在前进过程中各个阶段的高度差，其中，多个所述高度差的最大值为所述车体倾斜偏差。

在星球车实际运行中，若在障碍物处的俯仰角过大，此时即(双)前轮和(双)后轮的高度差大，若星球车的翻滚角过大，此时即两侧车轮的高度差大。由此可以预估两侧车轮以及前后车轮在前进过程中的高度差情况，以用于确定星球车可能出现的车体倾斜偏差。

参照图7所示，本实施例中，对于车体倾斜偏差的确定可根据星球车左侧车轮和右侧车轮与障碍物接触的接触高度进行确定。

其中，可确定左侧车轮行至i行时的最高高度以及确定右侧车轮行至i行时的最高高度/>进而得到在该行左右车轮的最高高度差为：/>通常情况下，引起车轮碾压失效的取决于/>的最大值，若控制住/>在一定范围内，则可以保证整个车轮碾压过程的稳定，车体倾斜偏差的确定公式即为/>两侧车轮在前进过程中各个阶段的高度差的最大值。

其中，基于获得的三维点云数据，即能够便于确定车轮与障碍物的接触高度，具体地，即接触点z向坐标值的差值。

相对应地，对于前后车轮在前进过程中各个阶段的高度差，可以基于前车轮和后车轮的间距，预估星球车行进至障碍物处时前后的高度差，

由此本实施例中确定星球车的车体倾斜偏差即能够用便于更好地确定出星球车的越障策略，例如在预估星球车在障碍物上运行，而出现车体倾斜偏差较大的情况时，可以不需要再确定其它障碍物的几何信息，而直接选择蟹行模式躲避障碍。

相关实施例中车体姿态特征还可包括车体的当前姿态特征和/或预估的在所述障碍物前方的地面处行进时的姿态特征，这些姿态特征也可以为对应的倾斜偏差。星球车在越障前可能处于崎岖(或凹凸)地面，此时车体的俯仰角或翻滚角可能处于“可忽略”范围外，因此在确定运动模式进行越障前，也可以考虑星球车当前或越障前的倾斜偏差，以用于更准确地确定以何种运动模式进行越障。

其中，预设情况可根据实际情况进行设定，本发明实施例中，对应特定的星球车，如地面模拟火星试验探测车，可设定特定的判定值进行对比判断，如表1中所示，当低于该判定值时，根据所述障碍物几何信息和所述相对位置情况确定所述星球车的运动模式，以进行越障，否则直接判定车体不可进行“接触式”越障运动模式，而应当进行其它非接触式越障运动模式。

在一可选的实施例中，也可对于障碍物的表面形貌特征设定预设形貌要求，来判断采用哪种种越障运动模式进行障碍物的越障，如参照表1所示，当特定星球车确定的表面形貌特征中的任一个参数超过该表中的值时，车体不可进行“接触式”越障运动模式，而应当进行其它非接触式越障模式，如采用蟹行模式绕过障碍物等。

表1：障碍物表面形貌特征和车体倾斜偏差标准表

火星表面典型的岩石种类主要有块状岩石和页岩，块状岩石大小形状不一，多表现为凸起的几何形状。页岩上表面往往较为平整且面积较大，本发明实施例中，以火星表面岩石作为障碍物进行了实验。

其中，岩石的仿真模型如图8所示，通过建立岩石的基础几何模型，在此基础上对其外表面加入随机噪声，对噪声进行滤波后再进行三角剖分算法处理，便可以得到较为广义上的岩石仿真场景模型，以便于进行实验分析。

在障碍物位置和火星车车轮轨迹确定后，影响火星车采用何种越障策略前进的障碍物几何信息主要包括其高度信息和表面形状信息。若障碍物表面形状较为平整的条件下，障碍物高度由低到高，则火星车可依次选择采用轮式模式碾压过障碍、抬轮运动模式越过障碍、升降模式、蟹行模式躲避障碍。若障碍物表面形状极为崎岖，比如有大尖锐凸点的情况下，即便其高度较低，火星车也依然不可以采用轮式模式碾压障碍物来进行越障，因为过于崎岖的岩石表面会对车轮造成损伤以及降低车轮碾压过程中的稳定性，所以这种情况下则需要采用抬轮运动模式越过障碍物或者采用蟹行模式躲避障碍物。

其中，障碍物高度信息可以根据三维节点的坐标进行确定，即确定坐标点中z向的最大值。

本实施例在实验的障碍物表面形貌特征确定过程中，由于火星车轮剖面外缘的弧度较大，且在轮宽范围内由弧度引起的高度差仅为5.6mm，这种尺寸范围不会对轮岩接触状态造成明显影响，所以为了简化运算及分析过程，将理想轮-岩理想接触表面由弧度为车轮剖面半径的圆弧表面简化为平面，以图8中岩石上表面为例进行求解。

由于星球车在与障碍物接触时，主要是轮体与障碍物接触，因此在障碍物位置和火星车车轮轨迹确定后，在获取障碍物的三维点云数据时，可将岩石上表面与车轮宽度接触范围内的三维节点提取出来，后求解该三维点云的协方差矩阵，而后进行特征分解，求出协方差矩阵的最小特征值所对应的特征向量即为该三维点云数据的相关系数最大的拟合平面，最终得到岩石的三维平面几何结果如图9所示。

该岩石的拟合表面方程为：

-92.06＝-0.0056x-0.0341y-0.9994z；(88≤x≤312,88≤y≤312)；

进而确定得到的表面形貌特征中，形状偏差为0.8417mm，俯仰偏差为0.32°，横滚偏差为1.96°。

根据获得的三维点云数据，确定的尖锐凸点分布如图9所示，其中1-4分别为最高尖锐凸点。得到的表面形貌特征中，尖锐凸点总数为17、尖锐凸点密度为75.89，尖锐凸点平均锋利度为19.36。另外也可额外获得最高尖锐凸点锋利度为H_max＝[12.2 23.7 16.811.6]。

基于对岩石高度的判断，岩石的最高点为124.9mm，由高度判据可知该岩石在轮式模式越障范围之内。

根据岩石的表面形貌特征，岩石表面形状偏差ffa＝0.8417，整体上看岩石形状偏离理想接触形状较小。由俯仰偏差f_tilt＝0.32°，横滚偏差f_roll＝1.96°可知岩石表面倾斜程度符合安全通过范围。观察凸点状态信息，该岩石共有17个尖锐凸点的锋利值在预设的锋利度阈值之上，平均每米有75.89个锋利凸点，17个尖锐凸点的平均锋利值为19.36。确定该表面锋利凸点数量不多，但平均锋利度较高。

另外根据确定的最高尖锐凸点锋利度H_max＝[12.2 23.7 16.8 11.6]，可知前进方向上每行的最高点为锋利凸点的数量共有4个，这4个点是火星车车轮表面一定会接触到的点，并且是受力最大的点，且锋利值较高，会实质性地对火星车表面造成一定程度的伤害。由以上分析可以做出判断该岩石高度较低，但表面锋利凸点状态较差，可以采用非接触式运动模式通过，基于特定的运动模式策略选择表，可以确定选择抬轮越障运动模式通过该岩石。

在本发明的一个可选的实施例中，在障碍物位置和火星车车轮轨迹确定后，可以分别确定左右两侧车轮与障碍物的接触情况，以此便于更准确地确定越障策略。本实施例中，确定的右侧车轮和左侧车轮与障碍物接触区域的三维点云数据的点云图及拟合得到的平面如图10和11所示。

对此，分别确定的右侧车轮和左侧车轮接触的障碍物表面形貌特征及车体倾斜偏差如表2和表3所示。

表2：右侧车轮与障碍物接触区域的表面形貌特征和车体倾斜偏差

表3：左侧车轮与障碍物接触区域的表面形貌特征和车体倾斜偏差

最终，可以确定该星球车可由轮式模式—双侧抬轮爬越障碍模式—轮式模式进行运动模式切换选择，最终进行障碍物的越障。

虽然本发明披露如上，但本发明并非限定于此。任何本领域技术人员，在不脱离本发明的精神和范围内，均可作各种更动与修改，因此本发明的保护范围应当以权利要求所限定的范围为准。

Claims (6)

1.一种基于障碍物几何信息的星球车运动模式选择方法，其特征在于，包括：

获取障碍物的三维点云数据；

根据所述三维点云数据生成障碍物几何信息和确定所述障碍物与星球车的相对位置情况，其中，所述障碍物几何信息包括所述障碍物的高度特征、表面形貌特征和纵向轮廓特征；

根据所述障碍物几何信息和所述相对位置情况确定所述星球车的运动模式，以进行越障；

当所述障碍物位于所述星球车的行进方向上车轮的单侧或双侧，且所述障碍物的最高高度小于或等于第一预设高度，以及所述表面形貌特征满足预设形貌要求时，和/或

当所述障碍物位于所述行进方向上所述车轮的内侧，且所述最高高度小于或等于第二预设高度时，确定所述星球车的运动模式为：维持基础运动模式进行越障；其中，所述第二预设高度大于所述第一预设高度；

否则，确定所述星球车的运动模式为：由所述基础运动模式切换至越障运动模式进行越障；

所述表面形貌特征包括所述障碍物的尖锐凸点偏差，所述尖锐凸点偏差包括尖锐凸点总数、尖锐凸点密度、最高尖锐凸点数量、尖锐凸点平均锋利度和最高尖锐点锋利度值变化范围中的至少一项，其中，当所述尖锐凸点位于所述星球车的前进方向处，且所述尖锐凸点为坐标点网格的同一行中高度最高的节点时，判定所述尖锐凸点为最高尖锐凸点；

所述根据所述三维点云数据生成障碍物几何信息包括：

根据所述三维点云数据的三维节点构建坐标点网格，其中，所述三维节点构成所述坐标点网格的节点；

确定所述障碍物的尖锐凸点，其中，当所述三维节点的z向坐标值与相邻的三维节点的z向坐标值的差值符合预设情况时，判定为所述尖锐凸点；

根据所述尖锐凸点的数量和/或锋利度，确定所述尖锐凸点偏差；

所述尖锐凸点的锋利度的确定方法包括：

确定所述尖锐凸点的z向坐标值与相邻的三维节点的z向坐标值的差值的最小值；

根据所述最小值和所述坐标点网格的网格间距确定所述锋利度；

所述越障运动模式包括单侧抬轮翻越模式，其中，当所述障碍物位于所述行进方向上所述车轮的单侧，且所述最高高度小于或等于所述第一预设高度，以及所述表面形貌特征不满足预设形貌要求，并且所述纵向轮廓特征满足预设轮廓要求时，和/或当所述障碍物位于所述行进方向上所述车轮的单侧，且所述最高高度大于或等于第一预设高度且小于或等于第四预设高度时，确定所述星球车的运动模式为：由所述基础运动模式切换至所述单侧抬轮翻越模式进行越障。

2.根据权利要求1所述的基于障碍物几何信息的星球车运动模式选择方法，其特征在于，所述表面形貌特征包括所述障碍物的表面与理想接触面的形状偏差；所述根据所述三维点云数据生成障碍物几何信息包括：

确定所述三维点云数据中各个三维节点到所述理想接触面之间的垂直距离；

确定各个所述垂直距离平方和的平均值，其中，所述形状偏差为所述平均值。

3.根据权利要求1所述的基于障碍物几何信息的星球车运动模式选择方法，其特征在于，所述表面形貌特征包括所述障碍物的拟合表面与车轮理想运动平面的方向偏差，所述方向偏差包括俯仰偏差和横滚偏差；所述根据所述三维点云数据生成障碍物几何信息包括：

将所述三维点云数据的三维节点拟合得到所述拟合表面的拟合表面方程；

根据所述拟合表面方程确定所述俯仰偏差和横滚偏差。

4.根据权利要求1所述的基于障碍物几何信息的星球车运动模式选择方法，其特征在于，所述越障运动模式包括双侧抬轮翻越模式和双侧抬轮爬越模式，其中，当所述障碍物位于所述行进方向上所述车轮的双侧，且所述最高高度小于或等于第五预设高度，以及所述表面形貌特征不满足所述预设形貌要求，并且所述纵向轮廓特征满足所述预设轮廓要求时，确定所述星球车的运动模式为：由所述基础运动模式切换至所述双侧抬轮翻越模式进行越障，其中，所述第五预设高度大于所述第一预设高度；

当所述障碍物位于所述行进方向上所述车轮的双侧，且所述最高高度大于或等于所述第一预设高度及小于或等于第六预设高度，以及所述表面形貌特征满足所述预设形貌要求时，确定所述星球车的运动模式为：由所述基础运动模式切换至所述双侧抬轮爬越模式进行越障，其中，所述第六预设高度大于所述第五预设高度。

5.根据权利要求4所述的基于障碍物几何信息的星球车运动模式选择方法，其特征在于，所述越障运动模式包括蟹行模式和升降模式，其中，当所述表面形貌特征不满足所述预设形貌要求，并且所述纵向轮廓特征不满足所述预设轮廓要求时，和/或

当所述最高高度大于各所述相对位置情况对应的预设超高高度时，确定所述星球车的运动模式为：由所述基础运动模式切换至所述蟹行模式，以用于使所述星球车绕过所述障碍物进行越障；

当所述障碍物位于所述行进方向上所述车轮的内侧，且所述最高高度大于或等于所述第二预设高度及小于或等于第三预设高度时，确定所述星球车的运动模式为：由所述基础运动模式切换至所述升降模式进行越障。

6.根据权利要求1所述的基于障碍物几何信息的星球车运动模式选择方法，其特征在于，所述根据所述障碍物几何信息和所述相对位置情况确定所述星球车的运动模式，以进行越障前，还包括：

获取车体姿态特征；

当所述车体姿态特征符合预设情况时，根据所述障碍物几何信息和所述相对位置情况确定所述星球车的运动模式，以进行越障；

其中，所述车体姿态特征包括所述星球车在所述障碍物表面行进时的车体倾斜偏差，所述车体倾斜偏差的确定方法包括：

根据所述三维点云数据确定所述星球车的两侧车轮和/或前后车轮在前进过程中各个阶段的高度差，其中，多个所述高度差的最大值为所述车体倾斜偏差。