[发明专利]一种障碍物告警方法和装置

权利要求书

1.一种障碍物告警方法，其特征在于，包括：

获取载机的安全包络面；

获取所述安全包络面所对应的经纬度范围内的障碍物信息；

根据所述安全包络面和所述障碍物信息进行碰撞计算，根据碰撞计算结果进行障碍物告警；

所述获取载机的安全包络面，包括：

建立载机坐标系，根据载机的经纬度、姿态、航向、速度和海拔高度，获取载机的安全包络线；

根据安全包络线，建立载机的安全包络面，所述安全包络面包括至少一个安全包络矩形；

所述获取所述安全包络面所对应的经纬度范围内的障碍物信息，包括：

获取各安全包络矩形的四个顶点的经纬度信息，根据各安全包络矩形的四个顶点的经纬度信息确定各安全包络矩形对应的经纬度范围；

在数据库中查找位于各安全包络矩形对应的经纬度范围内的障碍物信息，确定各安全包络矩形对应的障碍物信息；所述障碍物信息包括：障碍物水平对应点的海拔高度、经纬度，以及障碍物最高点的海拔高度、经纬度；

所述根据所述安全包络面和所述障碍物信息进行碰撞计算，包括：

对于存在对应的障碍物信息的任一安全包络矩形，获取所述安全包络矩形对应的第一点乘值D和第二点乘值N；

根据所述第一点乘值D和所述第二点乘值N，确定所述障碍物与所述安全包络矩形是否存在交点，若存在，则确定存在碰撞；

其中，所述第一点乘值D为所述安全包络矩形的矩形面法向量planeVector与障碍物方向向量lineVector的点乘值，所述第二点乘值N为所述安全包络矩形的矩形面法向量planeVector与第一方向向量wVector的点乘值，所述第一方向向量wVector为矩形任一顶点到障碍物的水平对应点的方向向量。

2.根据权利要求1所述的方法，其特征在于，所述根据所述第一点乘值D和所述第二点乘值N，确定所述障碍物与所述安全包络矩形是否存在交点，包括：

若fabs(D)小于第一预设数值，且fabs(N)＝0，则在NED坐标系下，确定所述障碍物水平对应点和/或障碍物最高点的NE平面投影是否位于所述安全包络矩形的NE平面投影中；

若在，则确定所述障碍物与所述安全包络矩形存在交点。

3.根据权利要求1所述的方法，其特征在于，所述根据所述第一点乘值D和所述第二点乘值N，确定所述障碍物与所述安全包络矩形是否存在交点，还包括：

若fabs(D)大于等于第一预设数值，且0N/D1，则获取障碍物方向向量lineVector与所述安全包络矩形所在平面的相交点JP；

确定所述相交点JP的NE平面投影是否位于所述安全包络矩形的NE平面投影中；

若在，则确定所述障碍物与所述安全包络矩形存在交点。

4.根据权利要求2或3所述的方法，其特征在于，确定点的NE平面投影是否位于所述安全包络矩形的NE平面投影中，包括：

对于任意点P，沿所述安全包络矩形的NE平面投影的对角线，将所述NE平面投影划分为两个三角形平面，记为ABC和ACD，A、B、C和D分别为所述安全包络矩形的NE平面投影的四个顶点；

获取三角形平面ABC和三角形平面ACD的面积；

计算任意点P与三角形平面ABC/三角形平面ACD的三个顶点中的每两个顶点构成的三角形的面积之和S；

在所述面积之和S与所述三角形平面ABC/三角形平面ACD的面积的差值在第一预设范围内时，确定点的NE平面投影是否位于所述安全包络矩形的NE平面投影中。

5.根据权利要求3所述的方法，其特征在于，所述获取障碍物方向向量lineVector与所述安全包络矩形所在平面的相交点JP，包括：

根据障碍物的水平对应点pa(x_pa，y_pa，z_pa)、障碍物方向向量lineVector(x_lv，y_lv，z_lv)、所述第一点乘值D和所述第二点乘值N，采用如下公式得到相交点JP(x_jp，y_jp，z_jp)；