pixhawk无人机结合openmv之精准降落

一.精准降落概述

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.感谢阅读，欢迎交流