[发明专利]一种3DAR导航的路径规划方法和系统

权利要求书

1.一种3DAR导航的路径规划方法，其特征在于，所述方法包括：

根据2D凸多边形地图和2.5D多边形高度地图，确定导航地图，其中，所述2.5D多边形高度地图的可行区域可分为多个不同的任意多边形，其中，所述多边形的3D平面方程为：

a*X+b*Y+c*Z+d＝0

其中，X、Y、Z分别为空间3D点在x、y、z轴的坐标，a、b、c、d为3D平面方程的系数，且均为已知常数；

在确定导航地图之后，将所述导航地图和所述2.5D多边形高度地图进行部分解耦，兼容无高度地图的场景及平坦地形的场景，得到总可行区域多边形，所述总可行区域多边形的3D平面方程为：

Z+d＝0

其中，Z为空间3D点在z轴的坐标，d为3D平面方程的系数，且为已知常数；

根据所述导航地图中的地图类型确定路径规划算法，其中，在所述导航地图类型为2D凸多边形地图的情况下，通过路径规划算法对所述2D凸多边形地图进行路径规划，得到路径规划信息，在所述导航地图类型为2.5D多边形高度地图的情况下，根据路径规划信息获取导航路径高度，具体步骤包括：获取通过所述路径规划信息得到的节点X、Y的值，并计算得到所述节点所在的多边形区域；将所述节点的X、Y的值导入所述多边形区域对应的3D平面方程中，计算得到所述3D平面方程中的Z值，输出得到所述导航路径高度；

根据所述路径规划信息和所述导航路径高度，生成得到贴合地形的3D导航路径。

2.根据权利要求1所述的方法，其特征在于，所述计算得到所述节点所在的多边形区域包括：

通过ray_casting算法计算得到所述节点所在的多边形区域。

3.根据权利要求1所述的方法，其特征在于，所述通过路径规划算法对所述2D凸多边形地图进行路径规划包括：

通过NavMesh的漏斗算法对所述2D凸多边形地图进行路径规划。

4.一种3DAR导航的路径规划系统，其特征在于，所述系统包括：

地图模块，用于根据2D凸多边形地图和2.5D多边形高度地图，确定导航地图，其中，所述2.5D多边形高度地图的可行区域可分为多个不同的任意多边形，其中，所述多边形的3D平面方程为：

a*X+b*Y+c*Z+d＝0

其中，X、Y、Z分别为空间3D点在x、y、z轴的坐标，a、b、c、d为3D平面方程的系数，且均为已知常数；

在确定导航地图之后，解耦模块，用于将所述导航地图和所述2.5D多边形高度地图进行部分解耦，兼容无高度地图的场景及平坦地形的场景，得到总可行区域多边形，所述总可行区域多边形的3D平面方程为：

Z+d＝0

其中，Z为空间3D点在z轴的坐标，d为3D平面方程的系数，且为已知常数；

路径规划模块，用于根据所述导航地图中的地图类型确定路径规划算法，其中，在所述导航地图类型为2D凸多边形地图的情况下，通过路径规划算法对所述2D凸多边形地图进行路径规划，得到路径规划信息，在所述导航地图类型为2.5D多边形高度地图的情况下，根据路径规划信息获取导航路径高度，具体步骤包括：获取通过所述路径规划信息得到的节点X、Y的值，并计算得到所述节点所在的多边形区域；将所述节点的X、Y的值导入所述多边形区域对应的3D平面方程中，计算得到所述3D平面方程中的Z值，输出得到所述导航路径高度，

根据所述路径规划信息和所述导航路径高度，生成得到贴合地形的3D导航路径。