[发明专利]一种基于预规划路径优化RRT算法的三维避障航迹规划方法

权利要求书

1.一种基于预规划路径优化RRT算法的三维避障航迹规划方法，其特征在于，该方法包括以下步骤：

步骤1：在障碍物膨胀规则和起点到终点连线与障碍物相交规则下生成预规划路径；

步骤2：针对预规划路径从其中通过提取得到RRT搜索树的随机状态点；

步骤3：RRT搜索树由起点沿着预规划路径上的随机状态点进行扩展，直至终点，生成避障轨迹；

所述的步骤1包括以下分步骤：

步骤101：运用障碍物膨胀法建立以便于计算机识别的三维环境中的障碍物模型，具体方法为：利用最小的长方体完全包围障碍物，以长方体的对角线长度的1.2倍为直径做障碍物的外接球；

步骤102：初始化膨胀处理后的障碍物模型的球心、球的半径、无人机的起点和终点；

步骤103：连接起点和终点组成连线，并计算球心到该连线的垂足坐标；

步骤104：基于球心到该连线的垂足坐标计算各球心到连线的距离；

步骤105：基于各球心到连线的距离和球的半径检查连线是否与障碍物相交；

步骤106：当检查结果为直线与球不相交或相切时，该连线即为预规划路径；

步骤107；当检查结果为直线与球相交时，确定预规划路径的新起点，并返回步骤103；

所述的步骤107中的预规划路径的新起点，其计算公式为：

式中，X_i为预规划路径的新起点坐标，(x_i,y_i,z_i)为预规划路径的新起点坐标的具体x、y和z轴坐标值，X_oi为膨胀处理后的障碍物模型的球心坐标，R_oi为膨胀处理后的障碍物模型的球的半径，为膨胀处理后的障碍物模型的球心到连线的垂足坐标的向量。

2.根据权利要求1所述的一种基于预规划路径优化RRT算法的三维避障航迹规划方法，其特征在于，所述的步骤2包括以下分步骤：

步骤201：在预规划路径为检查结果为直线与球不相交或相切的情况下所生成的情况下，将预规划路径视为由连续的质点组成，并从该预规划路径中通过每单位长度提取来得到RRT搜索树的随机状态点；

步骤202：在预规划路径为检查结果为直线与球相交的情况下所生成的情况下，将预规划路径视为由多个分段函数组成，并从每段分段函数中通过每单位长度提取来得到RRT搜索树的随机状态点。

3.根据权利要求1所述的一种基于预规划路径优化RRT算法的三维避障航迹规划方法，其特征在于，所述的步骤103中的球心到该连线的垂足坐标，其计算公式为：

/

式中，x_ni、y_ni和z_ni为垂足坐标的x、y和z轴坐标，x_g、y_g和z_g为无人机的终点的x、y和z轴坐标，x₀、y₀和z₀为无人机的起点的x、y和z轴坐标，x_oi、y_oi和z_oi为膨胀处理后的障碍物模型的球心的x、y和z轴坐标。

4.根据权利要求1所述的一种基于预规划路径优化RRT算法的三维避障航迹规划方法，其特征在于，所述的步骤104中的各球心到连线的距离，其计算公式为：

式中，L_i为各球心到连线的距离。

5.根据权利要求2所述的一种基于预规划路径优化RRT算法的三维避障航迹规划方法，其特征在于，所述的步骤201包括以下分步骤：

步骤2011：根据已知的起点和终点，由三维空间两点式求出预规划路径的直线方程；

步骤2012：通过控制x、y或z轴按扩展树步长的比例取值以确定随机动态点的选取及坐标。

6.根据权利要求5所述的一种基于预规划路径优化RRT算法的三维避障航迹规划方法，其特征在于，所述的步骤2011中的预规划路径的直线方程，其描述公式为：

式中，X、Y和Z分别表示x、y或z轴。

7.根据权利要求5所述的一种基于预规划路径优化RRT算法的三维避障航迹规划方法，其特征在于，所述的步骤2012中的随机动态点的坐标，其描述公式为：

式中，X_rand(i)为随机动态点的坐标，x_rand(i)为随机动态点的x轴坐标值且x_rand(i)＝x₀+i×AL，A和i均为常数，L为RRT搜索树的步长。