一.精準降落概述

1.概述
無人機在生產生活中逐漸獲得更大的用途，京東的物流無人機有望解決用戶快遞最后一分鐘的問題，對無人機的精準識別和降落的要求也就越來越高，在經過細致研究，我決定用openmv和pixhawk飛控結合apriltag影像識別來完成精準降落，飛控端的韌體是apm，而不是px4，這點是需要注意的，但這并不意味著px4韌體不可以實作精準降落，只是說在apm韌體中已經有完善的精準降落的邏輯了，而openmv例程也提供相應的代碼，

2.apriltag標簽
所謂的apriltag其實就是有黑白色塊組成的被識別物，本質就是最簡單的二維碼，apritag分不同的家族，下面就是幾個家族的apriltag標簽

3.openmv
openmv是一款低成本的影像處理模塊，可以輕松實作常用的影像處理，串口通訊功能，剛接觸openmv時我們可以只關心函式的作用而不去了解底層演算法實作，簡化我們的步驟，openmv可以識別apriltag標簽，并測算其位于鏡頭中的位置，openmv在本專案中，僅充當與降落位置有關資訊的傳輸，也就是通過串口通信將資料傳回飛控，而具體引導降落的邏輯則是飛控去完成，openmv充當傳感器的作用，所以是可以替代的，比如官方的IRLOCK模塊等，

4.mavlink通訊協議
mavlink通訊協議是專門為無人機資料通訊制定的一種通訊協議，openmv通過mavlink通訊協議向飛控傳輸資料，有興趣可以去研究一下協議訊息包中每一位代表的含義，

二.精準降落流程

1.多旋翼無人機除錯
無人機除錯也就是在地面站中校準無人機的傳感器，電調，遙控器等部件，使無人機可以相對平穩的起飛降落，這點至關重要，無人機偏航嚴重也會影響精準降落的實作

2.openmv與飛控pixhawk硬體連接
openmv與pixhwak需要連接三根線，兩根電源線，一根通訊線，飛控上有TELEM1和TELEM2，兩者可以任選其一連接，只是后續在引數設定方面略有不同，至于連接方式，只要能夠接觸良好就可以，可以用杜邦線進行連接，或者焊接，openmv鏡頭的焦距在飛行前也要調節，保證清晰，

3.openmv代碼燒錄
直接使用官方例程，打開openmvIDE，在示例中選擇mavlink_apriltags_landing_target.py檔案，將其保存在openmv中即可，為了無人機飛行程序中可以輕松判斷識別與否可以在其中判斷是否識別到的位置加入LED燈亮的代碼，可以參考openmv函式庫中有關LED的控制函式，

4.無人機地面站引數設定
有三個引數需要設定，PLND_ENABLED設定為1(enabled)，PLND_TYPE設定為1，若使用TELEM1，則將SERIAL1_BAUD設定為115(115200),若使用TELEM2，則將SERIAL2_BAUD設定為115(115200),引數在missionplanner地面站的全部引數表中搜索即可，若出現有些引數找不到的問題，可以嘗試重刷較新版本韌體來解決，至于為什么要設定PLND_ENABLED和PLND_TYPE引數，在第三部分代碼邏輯層面進行介紹,而SERIAL_BAUD引數則是波特率的設定，

5.地面站mavlink訊息接收
這一步可以用來判斷硬體連接和通訊功能是否生效，將無人機連接openmv后連接地面站，手拿apriltag影像讓openmv識別，openmv識別到后，會在地面站mavlink訊息視窗生成兩個84HZ的訊息，LANDING_TARGET和DISTANCE_SENSOR,如果可以收到這兩個訊息，那證明通訊和硬體連接沒有問題了，

5.測驗
無人機起飛后在，將其控制到目標影像斜上方附近，切換到LAND模式，無人機開始精準降落，這一步還是建議去室外空曠場地測驗，

三.代碼邏輯流程

1.openmv端代碼

while(True):
    clock.tick()
    img = sensor.snapshot()
    tags = sorted(img.find_apriltags(fx=f_x, fy=f_y, cx=c_x, cy=c_y), key = lambda x: x.w() * x.h(), reverse = True)

    if tags and (tags[0].id() in valid_tag_ids):
        if MAX_DISTANCE_SENSOR_enable: send_distance_sensor_packet(tags[0], valid_tag_ids[tags[0].id()])
        send_landing_target_packet(tags[0], img.width(), img.height(), valid_tag_ids[tags[0].id()])
        img.draw_rectangle(tags[0].rect())
        img.draw_cross(tags[0].cx(), tags[0].cy())
        print("Distance %f mm - FPS %f" % (z_to_mm(tags[0].z_translation(), valid_tag_ids[tags[0].id()]), clock.fps()))
    else:
        print("FPS %f" % clock.fps())

這里我只截取主要邏輯部分，完整代碼在openmvIDE中去找即可，在這個代碼中，如果識別到的標簽是目標標簽，那么則發送兩個訊息 send_landing_target_packet和send_distance_sensor_packet，有沒有發現，其實就是上個目錄中講的地面中mavlink訊息視窗收到的訊息，實際上就是向飛控發送了目標位置的橫縱坐標和距離資訊，

2.飛控端代碼
從github上下載apm韌體(ardupilot)原始碼，與精準降落有關的代碼分布在以下兩個檔案
ardupilot\libraries\AC_PrecLand檔案夾ardupilot\ArduCopter\mode.cpp源檔案

3.AC_PrecLand檔案夾
該檔案夾中包含以下頭檔案和源檔案

有C++基礎的應該可以看出來，其實在這個檔案夾中定義了一個基類AC_PreLand和其他繼承于AC_PreLand的派生類，其實到這里我們就應該可以理解為什么這個精準降落程序可以用openmv也可以用IRLOCK或者是其他替代品，原因就是這里定義好了應用于不同硬體的介面，事實上，在這個檔案夾中我們可以看出有Companion，IRLock，SITL和SITL_Gazebo等不同的精準降落實作方式，而Companion應該就是指代那些非官方提供的硬體，也就是伴侶計算機，例如openmv，樹莓派等

這段代碼取自AC_Preland.h頭檔案

    enum PrecLandType {
        PRECLAND_TYPE_NONE = 0,
        PRECLAND_TYPE_COMPANION,
        PRECLAND_TYPE_IRLOCK,
        PRECLAND_TYPE_SITL_GAZEBO,
        PRECLAND_TYPE_SITL,
    };

這段代碼定義了列舉變數PrecLandType，分別對應0到4其實也就對應了我們選擇的硬體型別，看到這里我們就應該明白了為什么地面站中要將引數PLND_TYPE設定為1了吧，

這段代碼取自AC_Preland.h頭檔案

void init(uint16_t update_rate_hz);

    // returns true if precision landing is healthy
    bool healthy() const { return _backend_state.healthy; }

    // returns true if precision landing is enabled (used only for logging)
    bool enabled() const { return _enabled.get(); }

    // returns time of last update
    uint32_t last_update_ms() const { return _last_update_ms; }

    // returns time of last time target was seen
    uint32_t last_backend_los_meas_ms() const { return _last_backend_los_meas_ms; }

    // returns estimator type
    uint8_t estimator_type() const { return _estimator_type; }

    // returns ekf outlier count
    uint32_t ekf_outlier_count() const { return _outlier_reject_count; }

    // give chance to driver to get updates from sensor, should be called at 400hz
    void update(float rangefinder_alt_cm, bool rangefinder_alt_valid);

    // returns target position relative to the EKF origin
    bool get_target_position_cm(Vector2f& ret);

    // returns target relative position as 3D vector
    void get_target_position_measurement_cm(Vector3f& ret);

    // returns target position relative to vehicle
    bool get_target_position_relative_cm(Vector2f& ret);

    // returns target velocity relative to vehicle
    bool get_target_velocity_relative_cms(Vector2f& ret);

    // returns true when the landing target has been detected
    bool target_acquired();

    // process a LANDING_TARGET mavlink message
    void handle_msg(const mavlink_message_t &msg);

    // parameter var table
    static const struct AP_Param::GroupInfo var_info[];

在這段代碼中，定義精準降落流程中需要用到的方法，包括狀態安全檢測，更新時間檢測，目標獲取時間檢測，目標三維向量，目標二維向量，與擴展卡爾曼濾波器(EKF)有關的函式，

這段代碼取自AC_Preland.h頭檔案

AP_Int8 _ estimator_type;//精確著陸估計器型別

AP_Float _ yaw_align;//身體x軸到傳感器x軸的偏航角，

AP_Float _ land_ofs_cm_x;//車體框架中目標前方攝像機期望的著陸位置

AP_Float _ land_ofs_cm_y;//目標右側攝像機在車體框架中的理想著陸位置

AP_Float _ accel_noise;//加速度計程序噪聲

AP_Vector3f _ cam_offset;//相機相對于CG的位置

uint32_t _ last_update_ms;//上次呼叫update時的系統時間(毫秒)

bool _ target_acquired;//如果目標最近被看到，則為true

uint32_t _ last_backend_los_meas_ms;//系統時間目標最后一次出現

PosVelEKF _ ekf_x _ekf_y;//卡爾曼濾波器的x和y軸

uint32_t _ outlier_reject_count;// mini-EKF的離群值計數器(連續3個離群值導致EKF接受更新)

Vector3f _ target_pos_rel_meas_NED;//目標的相對位置作為3D矢量

Vector2f _ target_pos_rel_est_NE;//目標相對于IMU的位置，沒有補償延遲

Vector2f _ target_vel_rel_est_NE;//目標相對于IMU的速度，沒有補償滯后

Vector2f _ target_pos_rel_out_NE;//目標相對于相機的位置，饋入位置控制器

Vector2f _ target_vel_rel_out_NE;//目標相對于CG的速度，饋入位置控制器

這段代碼定義了精準降落程序中需要使用的變數

這段代碼取自AC_Preland_Companion.cpp源檔案

void AC_PrecLand_Companion::handle_msg(const mavlink_message_t &msg)
{
    // parse mavlink message
    __mavlink_landing_target_t packet;
    mavlink_msg_landing_target_decode(&msg, &packet);

    _timestamp_us = packet.time_usec;
    _distance_to_target = packet.distance;

    // compute unit vector towards target
    _los_meas_body = Vector3f(-tanf(packet.angle_y), tanf(packet.angle_x), 1.0f);
    _los_meas_body /= _los_meas_body.length();

    _los_meas_time_ms = AP_HAL::millis();
    _have_los_meas = true;
}

這是mavlink訊息處理函式
_distance_to_target = packet.distance將訊息包中的距離資訊賦值給 _distance_to_target
_los_meas_body = Vector3f(-tanf(packet.angle_y), tanf(packet.angle_x), 1.0f); 將訊息包中的x,y坐標進行計算賦值給一個三維向量，用于降落，這也對應了地面站中mavlink訊息視窗中的資料，

4.mode.cpp檔案

這段代碼取自mode.cpp源檔案

void Mode::land_run_vertical_control(bool pause_descent)
{
#if PRECISION_LANDING == ENABLED
    const bool navigating = pos_control->is_active_xy();
    bool doing_precision_landing = !copter.ap.land_repo_active && copter.precland.target_acquired() && navigating;
#else
    bool doing_precision_landing = false;
#endif
}

這段代碼定義了垂直方向的控制函式，
#if PRECISION_LANDING == ENABLED這段預處理代碼用來判斷是否開啟精準降落模式，讀到這里我們應該也理解了地面站中為什么要將引數PLND_ENABLED設定為ENABLED了吧，其實就是為了通過這個標志來進入精準降落的入口，

這段代碼取自mode.cpp源檔案

#if PRECISION_LANDING == ENABLED
    bool doing_precision_landing = !copter.ap.land_repo_active && copter.precland.target_acquired();
    // run precision landing
    if (doing_precision_landing) {
        Vector2f target_pos, target_vel_rel;
        if (!copter.precland.get_target_position_cm(target_pos)) {
            target_pos.x = inertial_nav.get_position().x;
            target_pos.y = inertial_nav.get_position().y;
        }
        if (!copter.precland.get_target_velocity_relative_cms(target_vel_rel)) {
            target_vel_rel.x = -inertial_nav.get_velocity().x;
            target_vel_rel.y = -inertial_nav.get_velocity().y;
        }
        pos_control->set_xy_target(target_pos.x, target_pos.y);
        pos_control->override_vehicle_velocity_xy(-target_vel_rel);
    }

這段代碼是mode.cpp中水平方向的控制函式
target_pos.x = inertial_nav.get_position().x;
target_pos.y = inertial_nav.get_position().y;
將目標位置設定成為了航點位置
pos_control->set_xy_target(target_pos.x, target_pos.y);
進行位置控制，將無人機向目標引導

四.總結反思改進

1.無人機的初期除錯非常重要，將無人機飛穩是精準降落的保證

2.建議在室外測驗，代碼中位置控制函式被呼叫，可能需要GPS(GPS功能很多)支持

3.apritag二維碼大小直接影響有效識別距離，建議大一些，本人測驗，A3大小的二維碼識別高度可以到2.5米，水平范圍1.5米，當然，水平范圍受高度影響，

4.openmv與pixhawk飛控連接要穩固，容易出現接觸不良的現象

5.后期可以通過在飛控代碼中的修改實作識別到目標自主降落的功能，目前采用遙控器切land模式來實作降落

6.感謝閱讀，歡迎交流