之前，我们已经能够成功将地标识别出来，那么如何引导无人机向地标飞行呢？我们可以分为两部分：一、计算无人机与地标的相对位置；二、根据相对位置控制无人机飞行。这部分和之前几章不太一样，理论推导较多，请大家耐心看一看。若有不对的地方，也请指正~
这里要提一下，目前很主流的一种估计方法使PnP，也是我最开始尝试的一种方法。但是，PnP是基于点的估计方法，我们的地标中特征点（角点）不多，而且用来估计的点噪声比较大，所以效果不太好。后来，我决定用面积来估计相对位置，以提高估计的鲁棒性和精度。整个相对位置估计可以分为相机坐标系下的估计和绝对坐标系下的估计，我们先来做相机坐标系下的估计。
首先，我们来估计Zc，估计的思路是，图像中地标面积（看成矩形）与地标实际面积的比值与高度成正比例关系，如果我们能够知道这个比例，那么就能求得无人机高度。在理论推导这个比例之前，我们先要讲一讲摄像头的内参，直接上公式吧。
其中，K即为摄像头的内参（张友正标定法可得），第一个式子表示了图像平面坐标与相机平面坐标之间的关系，是我们后面进行推导的重要依据。如果有博友的不了解的话，建议先看一下高博的《视觉SLAM十四讲》。言归正传，为了后面的推导，我们先做三个假设。
（1）地标实际面积已知（的确，我们量一下就知道了）。
（2）相机内参中fx与fy相等（绝大多数实际情况下他们的确是十分接近的）。
（3）相机正对地标，即机体无滚转和俯仰（后来我会细讲，为了推导方便，暂时让它存在）。
推到直接上图了，公式打起来比较烦，请见谅。
之前由于假设（3）的限制，我们的推导只能在相机平面与地标平面平行的时候成立，这是不科学的，那么如何去掉这个假设呢？其实很简单，做一个仿射变换就可以了，公式如下：
其中，theta和phi分别为机体的俯仰角和滚转角。经过仿射变换的面积依旧是与Zc成正相关的，这样我们就成功地拿掉了假设（3）。然后，只要将求得的Zc直接代入相机模型公式就可以算出来Xc，Yc。
要将估计结果真正用于无人机导航，就要将相机坐标系下的估计结果转换到绝对坐标系。这边提一句，我把绝对坐标系的原点选为地标中心，方向是东北天（与mavros的导航坐标系一致）。如上文所说，如果相机平面与地标平面平行（即无俯仰滚转时），我们很容易就能实现坐标系的转换。但是，当出现滚转与俯仰的情况，我们需要做一些几何处理。以下举一种例子。
这种推导是我自己推的，所以当中有不严谨的地方，还请各位博友指正。还有一种Oc与Ow在异侧的情况，我就不展开啦。好吧，将了这么多上代码吧。
// td是一个结构体，用来储存之前识别的信息，比如中心的坐标，地标的面积
//current_att 表示了机体姿态
void EstimatePose( flag_detection_t *td, Euler_t current_att)
{
double detect_begin = clock();
double gama = acos(cos(current_att.pitch) * cos(current_att.roll));
float delta_l, L, H, X, Y;
bool direction;
if(gama / PI * 180 < 1)
gama = 0;
td->img_area = ComputeArea(td->corners[td->corners.size()-1]);
td->Pc.z = sqrt(td->square_area * K.at<float>(0,0) * K.at<float>(1,1) / (td->img_area * cos(current_att.pitch) * cos(current_att.roll)));
td->Pc.x = (td->center.x - K.at<float>(0,2)) * td->Pc.z / K.at<float>(0,0);
td->Pc.y = (td->center.y - K.at<float>(1,2)) * td->Pc.z / K.at<float>(1,1);
Oc.x = K.at<float>(0,2) + K.at<float>(0,0) * tan(current_att.roll);
Oc.y = K.at<float>(1,2) - K.at<float>(1,1) * tan(current_att.pitch);
delta_l = sqrt(td->Pc.x*td->Pc.x + td->Pc.y*td->Pc.y) * tan(fabs(gama));
if( ((Oc.x-K.at<float>(0,2)) * (td->center.x-K.at<float>(0,2)) + (Oc.y-K.at<float>(1,2)) * (td->center.y-K.at<float>(1,2))) > 0 )
{
L = td->Pc.z - delta_l;
direction = 1;
}
else
{
L = td->Pc.z + delta_l;
direction = 0;
}
H = L * cos(gama);
if(direction == 0)
{
Y = H * tan( current_att.pitch + atan((td->center.y - K.at<float>(1,2)) / K.at<float>(1,1)) );
X = H * tan( current_att.roll - atan((td->center.x - K.at<float>(0,2)) / K.at<float>(0,0)) );
}
else
{
Y = H * tan( current_att.pitch + atan((td->center.y - K.at<float>(1,2)) / K.at<float>(1,1)) );
X = H * tan( current_att.roll - atan((td->center.x - K.at<float>(0,2)) / K.at<float>(0,0)) );
}
td->Pw.x = X;
td->Pw.y = Y;
td->Pw.z = H;
double duration = ( clock() - detect_begin ) / CLOCKS_PER_SEC * 1000;
cout << "Estimate Time is " << duration << endl;
} 控制算法这种东西的话，是仁者见仁智者见智的，每个人都有不同的控制思路。我是用了速度控制，视觉伺服的方式，不断向目标点飞行，最后降落。我就直接贴代码吧。
while(ros::ok()){
ros::Time t0 = ros::Time::now();
ros::spinOnce();
att_stable = Att_stable(current_position);
arrive_flag = ArriveFlag(td->Pw);
//控制策略
//td->result为1表示识别到地标
if(td->result == 0)
if(td->prev_result == 0)
state = HOVER;
else if( arrive_flag == 1 )
state = LAND;
else
state = FLY;
else if( arrive_flag == 1 )
state = LAND;
else
state = FLY;
//控制具体执行
Point3d velocity;
switch(state)
{
case FLY:
//计算velocity，其实就是一个PID
velocity = ComputeLinearVelocity(td->Pw);
twist.twist.linear.x = velocity.x;
twist.twist.linear.y = velocity.y;
twist.twist.linear.z = 0;
twist.twist.angular.x = 0;
twist.twist.angular.y = 0;
twist.twist.angular.z = 0;
local_vel_pub.publish(twist);
break;
case HOVER:
twist.twist.linear.x = 0;
twist.twist.linear.y = 0;
twist.twist.linear.z = 0;
local_vel_pub.publish(twist);
break;
case LAND:
if( td->Pw.z > 150 )
{
twist.twist.linear.x = 0;
twist.twist.linear.y = 0;
twist.twist.linear.z = -1;
local_vel_pub.publish(twist);
}
else
auto_land = 1;
break;
default:
ROS_INFO("Unexpected Error!");
}
if(state == LAND && auto_land == 1){
if( current_state.mode != "AUTO.LAND" &&
(ros::Time::now() - last_request > ros::Duration(5.0))){
if( set_mode_client.call(offb_set_mode1) &&
offb_set_mode1.response.mode_sent){
ROS_INFO("autoland enabled");
}
last_request = ros::Time::now();
}
}
ros::Time t1 = ros::Time::now();
ros::Duration d(t1 - t0);
rate.sleep();
}