QGC避障实现

避障功能紧紧是加到植保功能上,植保功能没有分块模式,由于植保中碰到的障碍物比较小,采用的方法是绕过障碍物继续飞行,因此需要把原路径删除,添加新航迹点。

障碍物目前就考虑是凸多边形,航线和凸多边形只有两个交点,

多边形需要首先进行外扩

QGC避障实现

QGC避障实现QGC避障实现

 

若是有两个交点,需要把障碍物按这条航线把左右划分,并把左边的点和右边的按逆时针排序

分别计算左边点列和右边点列的长度,取距离最短的方向, 作为环绕方向。

 

效果如下图

QGC避障实现QGC避障实现QGC避障实现QGC避障实现