无人机航线规划思路剖析,基于凸多边形地块往复式运动

写在前面

嗨!很高兴看到你点进来阅读这篇文章,请别介意,标题有点长有点啰嗦(完全是为了seo考虑),但也算是概括了这篇文章的内容。如果你是要开发如下图所示的场景,但又苦于没什么好的思路,那么这篇文章一定会帮助到你!

往复式运动航线

基于不规则凸多边形地块的往复式航线规划

哦,对了,本文的实现是基于web平台的地图,使用javascript。如果你也是在web平台上开发,而且任务时间非常紧急,没有时间阅读完全文的话。。。我已经将本文的思路封装成一个库了,你可以猛戳下面的链接,开箱即用:
https://github.com/Char-Ten/cpRPA
兼容各大地图平台api(其实不同平台api差异的影响很低)哦,不信的话戳demo:
百度地图demo
高德地图demo
leaflet地图demo
觉得好用的话记得给个star~原创不易,谢谢支持

正文!

其实也是套公式

其实这种问题,实际上是数学几何应用题,既然是数学题啦,那按照考试的套路第一步肯定是套公式啊,这种场景,核心的公式不多,就两条:

  • 一次函数两点表达式
  • 绕(tx,ty)点旋转n度之后缩放SxSy倍的变换矩阵

第一条没什么可以说的,初二数学就开始教一次函数的知识,这一条是用来计算航线与地块边界的交点的。
第二条就是很经典的复合变换矩阵了,分别是位移矩阵叉乘旋转矩阵叉乘位移缩放矩阵,我们设其叉乘结果为A,那么我们就可以列出下面的等式:



计算过程就是。。。横乘竖横乘竖横乘竖横乘竖横乘竖横乘竖横乘竖横乘竖。。。。最后化为用于程序的代数式就是:



通过这条公式,就可以计算出航线旋转后的坐标点。
然后我们把它们分别封装一下,弄成一个函数调用先:
  function transform(x, y, tx, ty, deg, sx, sy) {
        var deg = deg * Math.PI / 180;
        if (!sy) sy = 1;
        if (!sx) sx = 1;
        return [
            sx * ((x - tx) * Math.cos(deg) - (y - ty) * Math.sin(deg)) + tx,
            sy * ((x - tx) * Math.sin(deg) + (y - ty) * Math.cos(deg)) + ty
        ]
  }

  function calcPointInLineWithY(p1,p2,y){
        var s = p1[1] - p2[1];
        var x;
        if (s) {
            x = (y - p1[1]) * (p1[0] - p2[0]) / s + p1[0]
        } else {
            return false
        }
        
        /**判断x是否在p1,p2在x轴的投影里,不是的话返回false*/
        if (x > p1[0] && x > p2[0]) {
            return false
        }
        if (x < p1[0] && x < p2[0]) {
            return false
        }
        return [x, y]
  }

看到这里,恭喜你,你已经完成了50%的工作量!如果是在考试,你把这两条公式列出来,不写答案也有一半的分数(

先从最简单的场景开始

一个矩形地块,航线水平于x轴:


这是一个大概是200*200大小的矩形,左上角的顶点经纬度为nw(西北),右上角的顶点经纬度为ne(东北),右下角的顶点经纬度为se(东南),左下角的顶点经纬度为sw(西南),其中设置无人机飞行的间隔为10。你先不考虑折线的连接顺序,就单单考虑一下,每一根横线如何生成。观察一下你会发现以下规律:

  1. 两条横线的间隔是20
  2. 每一条横线都可以表示为 y=NN为常数,表示某个纬度值
  3. 每一条横线段都是y=N与矩形相交产生,也就是每一条横线段都是该矩形地块与维度相交的结果

那么,现在矩形的四个顶点的经纬度是已知的,无人机飞行的间隔也是已知的,这个矩形需要与多少条纬度线相交是未知的,每一条横线的N是未知的,每一条横线段左右两个点的纬度是未知的。根据已知求未知,你的目标已经很明确了,一道很简单的几何题:

  • 该矩形需要与多少条纬度线相交:
/**@method 获取两个经纬度点的距离
 * @param {Object} p1,p2 - 两个经纬度点
 * @return {Number} 单位 米
*/
function distance(p1,p2){
  /**leaflet提供的方法*/
  return L.latLng(p1.lat,p1.lng).distanceTo(L.latLng(p2.lat,p2.lng))
}

/** nw到sw的距离*/
var dist = distance(nw,sw);

/** 得出答案*/
var lines = parseInt(dist / 20)
  • 求每一条横线的N:
var N=[];
var stepLat=(nw.lat-sw.lat)/lines;
for(var i=0;i<lines;i++){
  N.push(nw.lat - i * stepLat)
}
  • 因为矩形的两条边是垂直的,所以,横线段左右两个点的经度分别为nw.lngne.lng。这样我们就可以绘制出来了:
for(var i=0;i<N.length;i++){
  L.polyline([
    {
      lat:N[i],
      lng:nw.lng
    },{
      lat:N[i],
      lng:ne.lng
    }]).addTo(map)
}

场景开始变形!

锵锵,我们把矩形上面的边往东挪50米,得到一个平行四边形:

聪明的你一定发现了,平行四边形在Y轴上的投影根本没有发生变化嘛,即使变了之后,穿过地块的纬度线数目还是不变嘛,只不过,这次因为两条边不是垂直的,所以,我们需要计算斜边与纬度线的交点。等等,你这时候想起了,最开始50%工作量里面所封装的那个calcPointInLineWithY函数!
你已经知道斜边两个点的坐标,然后你又知道y=N,那你通过一次函数的两点表达式,完全就可以知道x,也就是经度是多少啦:

/**这里注意 纬度lat对应y,经度lng对应x*/
for(var i=0;i<N.length;i++){
  var westPoint=calcPointInLineWithY(
    [nw.lng,nw.lat],
    [sw.lng,sw.lat],
    N[i]
  );
  var eastPoint=calcPointInLineWithY(
    [ne.lng,ne.lat],
    [se.lng,se.lat],
    N[i]
  );
  if(eastPoint&&westPoint){
    L.polyline([westPoint,eastPoint]).addTo(map)
  }
}

那你可以再变一变,让y轴上的投影也发生变化,就像这样:

好了,这下你观察到,每条边都跟纬度线相交了,也就是说,这次你要遍历一下这个平行四边形四个顶点。等等,你似乎忘记了一个问题,这个四边形在y轴上的投影发生了变化,相交纬度线数目也跟着发生变化了。这时候你想到,要不给这个多边形做个外接矩形?就像这样:

这样是不是又回归了最开始的场景?只是把calcPointInLineWithY函数加上去之后,你可以得到任意凸多边形与纬度线相交的模型。

  • 首先是创建多边形外接的矩形,将多边形顶点全部遍历一遍,取到最大和最小的经纬度值。经度最大为东,经度最小为西。纬度最大为北,纬度最小为南。将这几个组合一下,获得西北点,东北点,东南点和西南点,就可以弄出一个矩形出来
  /**@method 创建多边形外接矩形
 * @param {Array} latlngs - 格式为[{lat,lng}]的经纬度数组,多边形的顶点集合
 * @return {Array} .rect - 返回外接矩形四顶点
 * @return {Object} .center - 返回外接矩形的中心的 
*/
function createPolygonBounds(latlngs){
  var lats=[];
  var lngs=[];
  for(var i=0;i<latlngs.length;i++){
    lats.push(latlngs[i].lat);
    lngs.push(latlngs[i].lng);
  }
  var maxLat=Math.max.apply(Math,lats);
  var maxLng=Math.max.apply(Math,lngs);
  var minLat=Math.min.apply(Math,lats);
  var minLng=Math.min.apply(Math,lngs);
  return {
    center:{
      lat:(maxLat+minLat)/2,
      lng:(maxLng+minLng)/2
    },
    latlngs:[{
      lat:maxLat,
      lng:minLng//西北
    },{
      lat:maxLat,
      lng:maxLng//东北
    },{
      lat:minLat,
      lng:maxLng//东南
    },{
      lat:minLat,
      lng:minLng//西南
    }]
  }
}
  • 然后是计算这个外接矩形穿过了多少条纬度线,跟之前那个场景是一样的。rect是上面那个函数创建的数组,可以看到西北点的索引是0,西南点的索引是3,所以计算西北到西南的点,也就是这个外接矩形的高度。这个方法返回有len条纬度线穿过,而且穿过的纬度相差lat。
/**@method 计算有多少条纬度线穿过
 * @param {Array} rect - 外接矩形
 * @param {Number} space - 间隔
*/
function calcLatsInPolygon(rect,space){
  var lines=parseInt(distance(rect[0],rect[3])/space/2);
  var lat=(rect[0].lat-rect[3].lat)/lines;
  return {
    len:lines,
    lat:lat
  }
}
  • 最后就是结合上面几个场景,改写一下最终的生成函数,这里你是直接将生成的横线画了出来,如果需要做折线连接顺序处理,还需要声明一个数组去存储生成的点,比如当纬度线的索引是奇数时,横线从西往东画,也就是先放西边的点再放东边的点;如果纬度线索引是偶数时,横线从东往西画,也就是反过来。这里就留给你自己处理了
/**任意多边形顶点集*/
var polygon=[{lat:23.13184566463993,lng:113.25901150703432},/**...其他点*/];

/**飞行间隔*/
var space=10;

/**外接矩形*/
var outRect=createPolygonBounds(polygon);

/**纬度线*/
var latLines=calcLatsInPolygon(outRect.latlngs,space);

var line=[];
/**遍历每一条纬度线*/
for(var i=0;i<latLines.len;i++){
  line=[];
  /**遍历每一个多边形顶点*/
  for(var j=0;j<polygon.length;j++){
    var point=calcPointInLineWithY([
       polygon[i].lng,
       polygon[i].lat,
    ],[
      polygon[si(i+1,polygon.length)].lng
      polygon[si(i+1,polygon.length)].lat
    ],outRect.latlngs[0].lat - i*latLines.lat)
    if(point){
      line.push(point)
    }
  }
  
  /**去掉只有一个交点的纬度线*/
  if(line.length<2){
    continue
  }
  
  /**去掉两个交点重合的纬度线*/
  if(line[0][0] === line[1][0]){
    continue
  }
  
  /**leaflet 绘制*/
  L.polyline([
    {lat:line[0][1],lng:line[0][0]},
    {lat:line[1][1],lng:line[1][0]}
  ]).addTo(map)
}

/**防止索引溢出*/
function si(i,l){
  if (i > l - 1) {
    return i - l;
  }
  if (i < 0) {
    return l + i;
  }
  return i;
}

到这里,基本上已经完成了90%,剩下10%那部分最简单了。简单么?你歪着头问,到现在为止只是多边形与纬度线相交啊,现实中无人机又不可能都是沿着纬度在地图上横着飞!它可以在地图上沿任意方向飞行的,可上面的做法,只能让无人机横着走啊!
别急,你这时候要淡定,你要想想开头不是还留了一个transform函数么?
这个transform的作用是让坐标(x,y)绕着(tx,ty)旋转deg度后在缩放SySx倍得到一个的新坐标(_x,_y)。没错,这次我们要拿这些坐标进行旋转运动。
而且牛顿告诉过你:

运动都是相对的

先放一张gif图,看看如何绘制一个斜45度角的航线:


GIF.gif

先让多边形绕着中心点旋转想要的角度,将得到的新多边形再与纬度线做相交操作,获取到那些交点之后,再将那些交点旋转回来。换句话说,变换前它是一个任意多边形,变换后,它还是一个任意多边形,都是满足上面已经预设好的场景的。这样的好处显而易见,你不需要修改上面的任何一个函数,也不需要去多写一条两个一次函数求交点的公式。把问题化到最简单的场景去,只需要添加变换的代码:

/**@method 创建一个旋转后的多边形
 * @param {Array}  latlngs - 经纬度顶点集
 * @param {Object} bounds - 由上文  createPolygonBounds 函数创建的对象
 * @param {Number} rotate - 旋转角度
 * @return {Array} - 变换后的经纬度数组
 */
function createRotatePolygon(latlngs, bounds, rotate){
  var res=[];
  for(var i=0;i<latlngs.length;i++){
    var tr=transform(
      latlngs[i].lng,
      latlngs[i].lat,
      bounds.center.lng,
      bounds.center.lat,
      rotate
    );
    res.push({lat:tr[1],lng:tr[0]});
  }
  return res
}

这里你一定要牢记,lng是经度,对应x;lat是纬度,对应y(被leaflet框架坑哭的我QuQ。。。
然后,将原来的代码加上去

var line=[];

/**折线*/
var polyline=[];

/**旋转45度*/
var rotate=45;

/**创建变换后的多边形*/
var rPolygon=createRotatePolygon(polygon,outRect,-rotate)
/**遍历每一条纬度线*/
for(var i=0;i<latLines.len;i++){
  line=[];
  /**遍历每一个多边形顶点*/
  for(var j=0;j<rPolygon.length;j++){
    var point=calcPointInLineWithY([
      rPolygon[i].lng,
      rPolygon[i].lat,
    ],[
      rPolygon[si(i+1,rPolygon.length)].lng
      rPolygon[si(i+1,rPolygon.length)].lat
    ],outRect.latlngs[0].lat - i*latLines.lat)
    if(point){
      line.push(point)
    }
  }
  
  /**去掉只有一个交点的纬度线*/
  if(line.length<2){
    continue
  }
  
  /**去掉两个交点重合的纬度线*/
  if(line[0][0] === line[1][0]){
    continue
  }
  
  /**这里不绘制了,先塞进polyline这个数组*/
  polyline.push(
    {lat:line[0][1],lng:line[0][0]},
    {lat:line[1][1],lng:line[1][0]}
  )
}

/**然后就可以直接转回来绘制*/
L.polyline(
  createRotatePolygon(polyline,outRect,rotate)
).addTo(map)

小瑕疵

也许细心的你发现了,gif图里面转是转了,斜45度的角也是画出来了,但是旋转后的图形好像是被拉长了!旋转回来时又会被压肥回来。这个嘛。。。
这个问题其实我在写验证的时候也发现了,粗略计算多边形面积和航线扫过的面积的比值,总是发现0度的比值最接近于1,90度的比值最小,不管多边形是什么形状。
最开始我一直以为是面积算法的原因,直到写这篇文章的时候去写那个gif动画后才发现,多边形变换后变形了,转换后与纬度相交的数量变多了。而我猜想变形的原因可能是,地球并不是一个平面,它是弯的你知道吧,这些经纬度虽然是投影到了平面上了,但实际上它们是在一个球上。直接拿经纬度变换相当于先在球上做了旋转,然后在投影到地图的平面上,这样看起来就像是被拉长了。
所以,你不能直接变换经纬度,而是要将经纬度换算成地图上的像素坐标,变换完之后再转回来,这样图像就不会被拉长了。
因此先来两个像素系与经纬度坐标系转换的方法压压惊:

/**@method 设置经纬度转换成页面像素坐标的方法
 * @param {Object} latlng - {lat,lng}
*/
function latlng2px(latlng){
    /**百度,map为 new BMap.Map() 对象*/
    return map.pointToPixel(new BMap.Point(latlng.lng, latlng.lat))

    /**
    * 高德,map为 new AMap.Map() 对象
    * return map.lngLatToContainer(new AMap.LngLat(latlng.lng, latlng.lat))
    *
    * leaflet map 为 L.map对象
    * return map.latLngToLayerPoint(L.latLng(latlng.lat, latlng.lng)) 
    */
}

/**@method 设置像素坐标转换成经纬度点的方法
 * @param {Array} px - [lng,lat] 
*/
function px2latlng(px){
    /**百度,map为 new BMap.Map() 对象*/
    return map.pixelToPoint(new BMap.Pixel(px[0], px[1]))

    /**
    * 高德,map为 new AMap.Map() 对象
    * return map.containerToLngLat(new AMap.Pixel(px[0], px[1]))
    * 
    * leaflet map 为 L.map对象
    * return map.layerPointToLatLng(L.point(px[0], px[1]))
    */
}

然后将原来的createRotatePolygon函数改为

function createRotatePolygon(latlngs,bounds,rotate){
  var res = [] , a , b;
  var c = latlng2Px(bounds.center);
  for (var i = 0; i < latlngs.length; i++) {
    a = latlng2Px(latlngs[i]);
    b = transform(a.x, a.y, c.x, c.y, rotate);
    res.push(px2Latlng(b));
  }
  return res;
}

这样就解决了拉长的问题:

GIF.gif

看到这里,相信你已经完全掌握了这种思路,即使你是使用iOS或者android的sdk,你也应该可以很快将思路“移植”过去。
至于那个折线的顺序,这个只是在push进polyline数组的时候判断一下i的奇偶修改不同的push顺序,很简单就得到那种折线效果,我相信你是会写的,这里就不着笔墨了。
更多的源码请访问:https://github.com/Char-Ten/cpRPA

写在最后

终于写完了此文,真是不容易。这个思路,从最开始思想混乱与Leaflet框架紧密耦合,到一步步解耦,到自己决定写一个适用于各个地图平台的库,到写这篇文章,差不多已经过去两个星期了。我发现,很多问题是你调试过程中发现不了的,等到你调试好了,决定写一篇装逼的文章,在写的过程中你就会发现各种调试中出现不了的问题,比如那个变换变形的问题。

在动手前,关于此类的教程文章少之又少,唯一可以找到比较符合场景的竟然是百度文库里面的一篇论文:
基于作业航向的不规则区域作业航线规划算法研究
论文懂吧,长倒是不长,就是臭,里面堆砌着各种奇奇怪怪的术语。之后看了两天后才明白他的实现思路,大同小异,只不过不知道他怎么搞的,新弄出一个坐标系出来,感觉这样是增加了思路的复杂度啊。所以在他的算法的基础上我做了简化,不做坐标系偏移,取代的是变换地块坐标,这样实现起来相对简单些。

因此在这里,小小贡献一下这个思路吧,也让以后有人像我这样被坑去写这种无人机航线规划的,能够很快地实现,不用再去看那些奇奇怪怪的论文了。。。

最后最后最后,安利一下https://github.com/Char-Ten/cpRPA 这个库(已经无耻到这个地步了。。。),可以接入百度、高德、leaflet,然后算航线!什么?你不需要。。。那你需要计算地图上多边形地块的面积不?我也提供算面积的方法,百度地图、高德地图都没有这种算面积的方法!好评给个star!谢谢大家!(够了喂!(╯‵□′)╯︵┻━┻)
如果你有更好的实现思路,或者新的使用场景,或者有使用问题,或者有bug,欢迎在下面评论。。。或者直接提issue:https://github.com/Char-Ten/cpRPA/issues

推荐阅读更多精彩内容