[发明专利]一种无人机自主寻路避障方法及系统

权利要求书

1.一种无人机自主寻路避障方法，其特征在于，包括：如下步骤：

S1、采集障碍物的位置信息；

S2、通过栅格法进行三维环境建模，并将其分割成若干个栅格，分别对包含障碍物的栅格和不包含障碍物的栅格进行不同颜色的处理，通过过起始点及目标终点的判断平面分割栅格，得到二维栅格模型；

S3、基于A*算法在所述二维栅格模型上做出全局静态路径规划，用于寻找起始点与最终点之间的最优的全局参考路径；

S4、在全局静态路径规划后，进行局部动态路径规划，用于在处理完突发的动态环境变化后，尽快回归至原先规划好的路径；

S5、通过贝塞尔曲线对规划出的线路点进行轨迹跟踪，完成对路径轨迹的平滑处理；

动态路径规划如下算法所示：

f(n)＝g(n)+h(n)+x(n)

g(n)表示起始点到节点n的路径代价，g(n)采用一种基于闵可夫斯基距离的公式进行计算，计算各个方向的代价不同从而使得路径代价不一样，h(n)表示从节点到目标点的启发函数；h(n)表示的是曼哈顿距离|x₁-x₂|+|y₁-y₂|；

x(n)表示的是新规划的节点到原来静态规划的路径的欧氏距离。

2.根据权利要求1所述的方法，其特征在于，所述步骤S1具体包括：

S101、通过传感器获取障碍物在传感器坐标系下的位置信息；

S102、通过坐标变换将所述传感器坐标系下的位置信息转换到无人机坐标系下；

S103、通过导航解算将无人机坐标系下的位置信息导入惯性坐标系下，获得动态的障碍物位置信息。

3.根据权利要求2所述的方法，其特征在于，所述通过过起始点及目标终点的判断平面分割栅格，得到二维栅格模型具体包括：

S201、过起始点及目标终点，生成垂直于yz平面的判断平面；

S202、将xy平面的二维栅格映射至所述判断平面，

当起始点的水平高度高于目标终点的水平高度时：

当起始点的水平高度低于目标终点的水平高度时：

当起始点的水平高度高于目标终点的水平高度一致时，判断平面平行于xy平面，

P_z＝E_z＝S_z (3)

其中，P₁(P_y,P_z)表示判断平面上任一点P的坐标在yz平面的投影点，S₁(S_y,S_z)表示起始点S在yz平面的投影点，E₁(E_y,E_z)表示目标终点S在yz平面的投影点，

结合公式(1)(2)(3)得到判断平面中任一点的坐标，

S203、设判断平面中某点坐标P_x＝B_x，P_y＝B_y，通过公式(4)判断二维栅格中的障碍物栅格和自由栅格，其中：

若P_z＞B_z，则平面F上以点P(P_x,P_y,P_z)为中心的栅格为自由栅格，

若P_z＜B_z，则平面F上以点P(P_x,P_y,P_z)为中心的栅格为障碍物栅格，

B(B_x,B_y,B_z)表示三维障碍物栅格坐标。