大疆无人机实现目标定位

大疆无人机实现目标定位

大疆无人机回传的实时视频可以帮助飞友观察发现美景，就称为目标吧，你偏偏对目标很感兴趣，迫切想知道目标的位置，最好是经纬度坐标。你查找大疆Document，欣喜地发现大疆提供了定位机体本身的定位方法（通过FlightController.setStateCallback重写Update函数即可），不过并没有定位目标的现成函数。
实际上，屏幕上的像素与实地坐标存在一一对应的关系，找到这个关系就能实现实地目标的定位。
建模条件：①地面为水平平面；②相机视角θ（大疆X3为94度）；③飞机相对高度droneHeight，经纬度（droneLat，droneLng），机头朝向droneHeading；④屏幕分辨率deviceWidth、deviceHeight；⑤屏幕以左上角为原点（0,0），向右向下为正；⑥相机垂直地面；⑦不考虑目标的高度和图像形变。

则任意屏幕的像素点（x,y），对应的实地经纬度坐标为唯一的，假设为（xLng,yLat），从而可以用大疆SDK提供的函数在地图中标定。
基于上述模型，得到从像素点到实地位置的计算过程：
1、根据飞机高度和相机视角，计算屏幕中的图像对应实地的长度（Height1）和宽度（Width1）:
Width1=droneHeighttan(Θ/2)
Height1=deviceHeightWidth1/deviceWidth；
2、计算目标像素在屏幕中相对于屏幕中心点的归一化位置（tmpX ,tmpY）,其中tmpX = droneHeightx2/Width1， tmpY = -y/x*tmpX;

3、根据飞机当前位置和机头方向，解算实地的顶点坐标：
xLng=droneLng+(tmpXcos(droneHeading)- tmpYsin(droneHeading))M_TO_LAT;
yLat=droneLat+(tmpYcos(droneHeading)+ tmpX*.sin(droneHeading))*M_TO_LAT;
以上可以计算屏幕像素与实地经纬度的一一对应关系。
实现函数也很简单：

private DJILatLng calcTargetLatLng(LocationCoordinate3D drone3D, PointF pointF,int droneHeading){
final double M_TO_LAT = 360.0/40000000;
        DJILatLng targetLatLng = new DJILatLng(0.0,0.0);
        double tmpX,tmpY;
        double Theta = -droneHeading*Math.PI/180;
        double Height =  drone3D.getAltitude();
        tmpX = Height*pointF.x*2/deviceWidth;
        tmpY = -pointF.y/pointF.x*tmpX;
        targetLatLng.longitude = drone3D.getLongitude() + (tmpX*Math.cos(Theta) - tmpY*Math.sin(Theta))*M_TO_LAT;
        targetLatLng.latitude  = drone3D.getLatitude() + (tmpY*Math.cos(Theta) + tmpX*Math.sin(Theta))*M_TO_LAT;

        return targetLatLng;
    }

参数说明：drone3D是飞机的高度、经度、纬度信息，droneHeading是机头朝向，以上两个参数均可以通过在OnCreate函数中，用FlightController.setStateCallback重写Update函数得到；pointF是目标像素点的位置，通过点击屏幕获取。
如果需要，可以通过DJIMap.addMarker将目标标定到地图中。测试效果，飞机在100米高度，有可能把机动车道标记到慢车道，这个误差可以接受。