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

写在前面

嗨！很高兴看到你点进来阅读这篇文章，请别介意，标题有点长有点啰嗦（完全是为了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=N，N为常数，表示某个纬度值
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.lng，ne.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