I Precision landing overview

1. General
UAVs are gradually gaining greater use in production and life. JD's logistics UAVs are expected to solve the problem of the last minute of user express delivery, and the requirements for accurate identification and landing of UAVs are becoming higher and higher. After careful research, I decided to use openmv and pixhawk flight control combined with apriltag image recognition to complete accurate landing. The firmware of flight control terminal is apm, not px4, which needs attention, but this does not mean that px4 firmware can not achieve accurate landing. It just means that there is perfect logic of accurate landing in apm firmware, and openmv routine also provides corresponding code.

2.apriltag label
The so-called apriltag is actually a recognized object composed of black and white color blocks. Its essence is the simplest two-dimensional code. Apriltag is divided into different families. Here are the apriltag tags of several families

3.openmv
Openmv is a low-cost image processing module, which can easily realize the common image processing and serial communication functions. When we first contact openmv, we can only care about the function without understanding the implementation of the underlying algorithm, so as to simplify our steps. Openmv can recognize the apriltag tag and calculate its position in the lens. In this project, openmv only serves as the transmission of information related to the landing location, that is, the data is transmitted back to the flight control through serial communication, and the specific logic to guide the landing is completed by the flight control. Openmv acts as a sensor, so it can be replaced, such as the official IRLOCK module.

4.mavlink communication protocol
Mavlink communication protocol is a communication protocol specially formulated for UAV data communication. openmv transmits data to flight control through mavlink communication protocol. If you are interested, you can study the meaning of each representative in the protocol message packet.

II Precise landing process

1. Multi rotor UAV commissioning
UAV debugging is to calibrate the sensors, electrical regulators, remote controls and other components of the UAV in the ground station, so that the UAV can take off and land relatively smoothly. This is very important. The serious yaw of UAV will also affect the realization of accurate landing

2. Hardware connection between openmv and flight control pixhawk
Openmv and pixhwak need to be connected with three wires, two power wires and one communication wire. There are TELEM1 and TELEM2 on the flight control. Either of them can be connected, but the subsequent parameter settings are slightly different. As for the connection mode, as long as it can be in good contact, it can be connected with DuPont wire or welded. The focal length of openmv lens should also be adjusted before flying to ensure clarity.

3.openmv code burning
Directly use the official routine, open the openmv IDE, and select mavlink in the example_ apriltags_ landing_ target. Py file, which can be saved in openmv. In order to easily judge whether the UAV is recognized or not during flight, the code of LED light can be added to determine whether the recognized position is on. You can refer to the control function of LED in openmv function library.

4. Parameter setting of UAV Ground Station
There are three parameters to be set, PLND_ENABLED is set to 1(enabled), PLND_TYPE is set to 1. If TELEM1 is used, Serial1 will be set_ Baud is set to 115 (115200). If TELEM2 is used, serial2 will be set_ Baud is set to 115 (115200), and the parameters can be searched in all parameter tables of the missionplaner ground station. If some parameters cannot be found, you can try to re brush the firmware of a newer version to solve the problem. As for why to set PLND_ENABLED and PLND_ The type parameter is introduced at the code logic level in the third part, while SERIAL_BAUD parameter is the setting of baud rate.

5. Ground station mavlink message reception
This step can be used to judge whether the hardware connection and communication functions are effective. Connect the UAV to the ground station after connecting it to openmv. Hold the apriltag image in hand and let openmv recognize it. After openmv recognizes it, two 84HZ messages will be generated in the mavlink message window of the ground station, LANDING_TARGET and DISTANCE_SENSOR, if you can receive these two messages, it proves that there is no problem with communication and hardware connection.

5. Test
After the UAV takes off, control it near the oblique top of the target image, switch to LAND mode, and the UAV begins to LAND accurately. In this step, it is recommended to go to the outdoor open area for testing.

III Code logic flow

1.openmv side code

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())

Here I only intercept the main logic part, and the complete code can be found in openmv ide. In this code, if the identified tag is the target tag, two messages send are sent_ landing_ target_ Packet and send_distance_sensor_packet, did you find that it is actually the message received in the mavlink message window on the ground mentioned in the previous directory. In fact, it is like the flight control sending the abscissa and ordinate and distance information of the target position.

2. Flight control terminal code
Download the source code of apm firmware (ardupilot) from github. The codes related to precision landing are distributed in the following two locations
ardupilot\libraries\AC_ Precouport \ landmode folder CPP source file

3.AC_PrecLand folder
This folder contains the following header files and source files

Those with C + + foundation should be able to see that in fact, a base class AC is defined in this folder_ Preland and others inherited from AC_ The derived class of preland. In fact, we can understand why this precise landing process can use openmv, irlock or other substitutes. The reason is that the interfaces applied to different hardware are defined here. In fact, in this folder, we can see Companion, irlock, SITL and SITL_Gazebo and other different precise landing methods, and Companion should refer to those unofficial hardware, that is, Companion computers, such as openmv, raspberry pie, etc

This code is taken from AC_Preland.h header file

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

This code defines the enumeration variable PrecLandType, which corresponds to 0 to 1 respectively. In fact, it corresponds to the hardware type we choose. From here, we should understand why the parameter PLND should be set in the ground station_ Type is set to 1.

This code is taken from AC_Preland.h header file

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[];

In this code, the methods needed in the precise landing process are defined, including state safety detection, update time detection, target acquisition time detection, target three-dimensional vector, target two-dimensional vector, and functions related to extended Kalman filter (EKF).

This code is taken from AC_Preland.h header file

AP_Int8 _ estimator_type;//Precise landing estimator type

AP_Float _ yaw_align;//Yaw angle from body X-axis to sensor x-axis.

AP_Float _ land_ofs_cm_x;//The desired landing position of the camera in front of the target in the vehicle body frame

AP_Float _ land_ofs_cm_y;//Ideal landing position of the camera on the right side of the target in the vehicle body frame

AP_Float _ accel_noise;//Accelerometer process noise

AP_Vector3f _ cam_offset;//Camera position relative to CG

uint32_t _ last_update_ms;//System time in milliseconds when update was last called

bool _ target_acquired;//true if the target was recently seen

uint32_t _ last_backend_los_meas_ms;//Last occurrence of system time target

PosVelEKF _ ekf_x _ekf_y;//x and y axes of Kalman filter

uint32_t _ outlier_reject_count;// Outlier counter of mini EKF (3 consecutive outliers cause EKF to accept update)

Vector3f _ target_pos_rel_meas_NED;//The relative position of the target is used as a 3D vector

Vector2f _ target_pos_rel_est_NE;//The position of the target relative to the IMU does not compensate for the delay

Vector2f _ target_vel_rel_est_NE;//The speed of the target relative to the IMU has no compensation lag

Vector2f _ target_pos_rel_out_NE;//The position of the target relative to the camera is fed into the position controller

Vector2f _ target_vel_rel_out_NE;//The speed of the target relative to CG is fed into the position controller

This code defines the variables to be used in the precise landing process

This code is taken from AC_Preland_Companion.cpp source file

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;
}

This is the mavlink message processing function
_ distance_to_target = packet.distance assigns the distance information in the message packet to_ distance_to_target
_ los_meas_body = Vector3f(-tanf(packet.angle_y), tanf(packet.angle_x), 1.0f); Calculate and assign the X and Y coordinates in the message package to a three-dimensional vector for landing. This also corresponds to the data in the mavlink message window in the ground station.

This code is taken from mode CPP source file

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
}

This code defines the control function in the vertical direction,
#if PRECISION_ The preprocessing code of landing = = ENABLED is used to judge whether the precise landing mode is ENABLED. After reading this, we should also understand why the parameter PLND is set in the ground station_ ENABLED is set to ENABLED. In fact, it is to enter the entrance of accurate landing through this sign.

This code is taken from mode CPP source file

#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);
    }

This code is mode Horizontal control function in CPP
target_pos.x = inertial_nav.get_position().x;
target_pos.y = inertial_nav.get_position().y;
Set the target position as the waypoint position
pos_control->set_xy_target(target_pos.x, target_pos.y);
Conduct position control to guide the UAV to the target

IV Summary, reflection and improvement

1. The initial commissioning of UAV is very important. Flying UAV stably is the guarantee of accurate landing

2. It is recommended to test outdoors. The position control function in the code is called, which may need the support of GPS(GPS has many functions)

3. The size of apritag QR code directly affects the effective recognition distance. It is recommended to be larger. I test that the recognition height of A3 QR code can be up to 2.5m and the horizontal range is 1.5m. Of course, the horizontal range is affected by the height.

4. The connection between openmv and pixhawk flight control shall be stable, and it is easy to have poor contact

5. In the later stage, the function of identifying the target's autonomous landing can be realized by modifying the flight control code. At present, the remote control mode is adopted to realize the landing

6. Thank you for reading and welcome communication