新闻中心

基于不规则图形的无人机航线规划

行业资讯 2017-10-13 18:07

  在无人机行业应用场景中,航线规划是一项十分重要的前置工作,这能让无人机按照既定的路线进行飞行并完成设定的无人机航拍录影或数据采集任务。市面上有不少现成的软件都只提供规则图形(比如矩形、平行四边形)的航线规划,但很多时候要勘测的目标的轮廓都是不规则的,按照规则图形设定航线往往会带来多余的飞行总距离和多余的覆盖面积,为减少这多余的距离和覆盖面积,同时也为了节省无人机电量的消耗或药液消耗(农业领域),基于不规则图形进行航线规划就很有必要了。


  如果你有基于地图API进行web开发的经验,不妨看看这篇文章对基于不规则图形进行航线规划的思路剖析。以下就是基于不规则图形的无人机航线规划!

无人机航线规划

  写在前面


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


  哦,对了,本文的实现是基于web平台的地图,使用javascript。如果你也是在web平台上开发,而且任务时间非常紧急,没有时间阅读完全文的话。。。我已经将本文的思路封装成一个库了


  正文!


  其实也是套公式


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

数学公式

  第一条没什么可以说的,初二数学就开始教一次函数的知识,这一条是用来计算航线与地块边界的交点的。


  第二条就是很经典的复合变换矩阵了,分别是位移矩阵叉乘旋转矩阵叉乘位移缩放矩阵,我们设其叉乘结果为A,那么我们就可以列出下面的等式:

复合变换矩阵算法

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

程序的代数式

  通过这条公式,就可以计算出航线旋转后的坐标点。


  然后我们把它们分别封装一下,弄成一个函数调用先:


  functiontransform(x,y,tx,ty,deg,sx,sy){


  vardeg=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


  ]


  }


  functioncalcPointInLineWithY(p1,p2,y){


  vars=p1[1]-p2[1];


  varx;


  if(s){


  x=(y-p1[1])*(p1[0]-p2[0])/s+p1[0]


  }else{


  returnfalse


  }


  /**判断x是否在p1,p2在x轴的投影里,不是的话返回false*/


  if(x>p1[0]&&x>p2[0]){


  returnfalse


  }


  if(x<p1[0]&&x<p2[0]){


  returnfalse


  }


  return[x,y]


  }


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


  先从最简单的场景开始


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

矩形地块

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


  两条横线的间隔是20


  每一条横线都可以表示为y=N,N为常数,表示某个纬度值


  每一条横线段都是y=N与矩形相交产生,也就是每一条横线段都是该矩形地块与维度相交的结果


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


  该矩形需要与多少条纬度线相交:


  /**@method获取两个经纬度点的距离


  *@param{Object}p1,p2-两个经纬度点


  *@return{Number}单位米


  */


  functiondistance(p1,p2){


  /**leaflet提供的方法*/


  returnL.latLng(p1.lat,p1.lng).distanceTo(L.latLng(p2.lat,p2.lng))


  }


  /**nw到sw的距离*/


  vardist=distance(nw,sw);


  /**得出答案*/


  varlines=parseInt(dist/20)


  求每一条横线的N:


  varN=[];


  varstepLat=(nw.lat-sw.lat)/lines;


  for(vari=0;i<lines;i++){


  N.push(nw.lat-i*stepLat)


  }


  因为矩形的两条边是垂直的,所以,横线段左右两个点的经度分别为nw.lng,ne.lng。这样我们就可以绘制出来了:


  for(vari=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(vari=0;i<N.length;i++){


  varwestPoint=calcPointInLineWithY(


  [nw.lng,nw.lat],


  [sw.lng,sw.lat],


  N[i]


  );


  vareastPoint=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-返回外接矩形的中心的


  */


  functioncreatePolygonBounds(latlngs){


  varlats=[];


  varlngs=[];


  for(vari=0;i<latlngs.length;i++){


  lats.push(latlngs[i].lat);


  lngs.push(latlngs[i].lng);


  }


  varmaxLat=Math.max.apply(Math,lats);


  varmaxLng=Math.max.apply(Math,lngs);


  varminLat=Math.min.apply(Math,lats);


  varminLng=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-间隔


  */


  functioncalcLatsInPolygon(rect,space){


  varlines=parseInt(distance(rect[0],rect[3])/space/2);


  varlat=(rect[0].lat-rect[3].lat)/lines;


  return{


  len:lines,


  lat:lat


  }


  }


  最后就是结合上面几个场景,改写一下最终的生成函数,这里你是直接将生成的横线画了出来,如果需要做折线连接顺序处理,还需要声明一个数组去存储生成的点,比如当纬度线的索引是奇数时,横线从西往东画,也就是先放西边的点再放东边的点;如果纬度线索引是偶数时,横线从东往西画,也就是反过来。这里就留给你自己处理了


  /**任意多边形顶点集*/


  varpolygon=[{lat:23.13184566463993,lng:113.25901150703432},/**...其他点*/];


  /**飞行间隔*/


  varspace=10;


  /**外接矩形*/


  varoutRect=createPolygonBounds(polygon);


  /**纬度线*/


  varlatLines=calcLatsInPolygon(outRect.latlngs,space);


  varline=[];


  /**遍历每一条纬度线*/


  for(vari=0;i<latLines.len;i++){


  line=[];


  /**遍历每一个多边形顶点*/


  for(varj=0;j<polygon.length;j++){


  varpoint=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)


  }


  /**防止索引溢出*/


  functionsi(i,l){


  if(i>l-1){


  returni-l;


  }


  if(i<0){


  returnl+i;


  }


  returni;


  }


  到这里,基本上已经完成了90%,剩下10%那部分最简单了。简单么?你歪着头问,到现在为止只是多边形与纬度线相交啊,现实中无人机又不可能都是沿着纬度在地图上横着飞!它可以在地图上沿任意方向飞行的,可上面的做法,只能让无人机横着走啊!


  别急,你这时候要淡定,你要想想开头不是还留了一个transform函数么?


  这个transform的作用是让坐标(x,y)绕着(tx,ty)旋转deg度后在缩放SySx倍得到一个的新坐标(_x,_y)。没错,这次我们要拿这些坐标进行旋转运动。


  而且牛顿告诉过你:


  运动都是相对的


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


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


  /**@method创建一个旋转后的多边形


  *@param{Array}latlngs-经纬度顶点集


  *@param{Object}bounds-由上文createPolygonBounds函数创建的对象


  *@param{Number}rotate-旋转角度


  *@return{Array}-变换后的经纬度数组


  */


  functioncreateRotatePolygon(latlngs,bounds,rotate){


  varres=[];


  for(vari=0;i<latlngs.length;i++){


  vartr=transform(


  latlngs[i].lng,


  latlngs[i].lat,


  bounds.center.lng,


  bounds.center.lat,


  rotate


  );


  res.push({lat:tr[1],lng:tr[0]});


  }


  returnres


  }


  这里你一定要牢记,lng是经度,对应x;lat是纬度,对应y(被leaflet框架坑哭的我QuQ。。。


  然后,将原来的代码加上去


  varline=[];


  /**折线*/


  varpolyline=[];


  /**旋转45度*/


  varrotate=45;


  /**创建变换后的多边形*/


  varrPolygon=createRotatePolygon(polygon,outRect,-rotate)


  /**遍历每一条纬度线*/


  for(vari=0;i<latLines.len;i++){


  line=[];


  /**遍历每一个多边形顶点*/


  for(varj=0;j<rPolygon.length;j++){


  varpoint=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}


  */


  functionlatlng2px(latlng){


  /**百度,map为newBMap.Map()对象*/


  returnmap.pointToPixel(newBMap.Point(latlng.lng,latlng.lat))


  /**


  *高德,map为newAMap.Map()对象


  *returnmap.lngLatToContainer(newAMap.LngLat(latlng.lng,latlng.lat))


  *


  *leafletmap为L.map对象


  *returnmap.latLngToLayerPoint(L.latLng(latlng.lat,latlng.lng))


  */


  }


  /**@method设置像素坐标转换成经纬度点的方法


  *@param{Array}px-[lng,lat]


  */


  functionpx2latlng(px){


  /**百度,map为newBMap.Map()对象*/


  returnmap.pixelToPoint(newBMap.Pixel(px[0],px[1]))


  /**


  *高德,map为newAMap.Map()对象


  *returnmap.containerToLngLat(newAMap.Pixel(px[0],px[1]))


  *


  *leafletmap为L.map对象


  *returnmap.layerPointToLatLng(L.point(px[0],px[1]))


  */


  }


  然后将原来的createRotatePolygon函数改为


  functioncreateRotatePolygon(latlngs,bounds,rotate){


  varres=[],a,b;


  varc=latlng2Px(bounds.center);


  for(vari=0;i<latlngs.length;i++){


  a=latlng2Px(latlngs[i]);


  b=transform(a.x,a.y,c.x,c.y,rotate);


  res.push(px2Latlng(b));


  }


  returnres;


  }


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


  看到这里,相信你已经完全掌握了这种思路,即使你是使用iOS或者android的sdk,你也应该可以很快将思路“移植”过去。


  至于那个折线的顺序,这个只是在push进polyline数组的时候判断一下i的奇偶修改不同的push顺序,很简单就得到那种折线效果,我相信你是会写的,这里就不着笔墨了。


上一篇:用无人机制作正射投影地图:软件使用心得
下一篇:无人机电力巡检模式快速巡检与精细巡检