ArduCopter——ArduPilot——航點導航WPNav(一)

版權聲明：本文爲博主原創博文，未經允許不得轉載，若要轉載，請說明出處並給出博文鏈接 

 

       現如今，四旋翼飛行器已經從幾年前的遙控航模變成真正可以超視距操控的無人機，離不開偉大的通信協議mavlink和最近幾年大火的4G與5G。但是這些都要依託一個強大的飛控，開源飛控項目ArduPilot的分支ArduCopter就是一個非常優秀的多旋翼飛控，但是要真正實現無人機的航線規劃而後自主飛行，就要着實學習下本博客涉及的航點導航的內容，另外還有一篇ArduCopter——ArduPilot——航點導航WPNav(二)——Spline Navigation

 

①下邊代碼是航點導航的具體實現，被用於上層結構的調用。我們這裏不關心上層如何調用，只看如何實現

/// update_wpnav - run the wp controller - should be called at 100hz or higher
bool AC_WPNav::update_wpnav()
{
    bool ret = true;

    // calculate dt
    float dt = _pos_control.time_since_last_xy_update();
    if (dt >= 0.2f) {
        dt = 0.0f;
    }

    // allow the accel and speed values to be set without changing
    // out of auto mode. This makes it easier to tune auto flight
    _pos_control.set_accel_xy(_wp_accel_cmss);
    _pos_control.set_accel_z(_wp_accel_z_cmss);

    // advance the target if necessary
    if (!advance_wp_target_along_track(dt)) {   //航點導航的關鍵函數
        // To-Do: handle inability to advance along track (probably because of missing terrain data)
        ret = false;
    }

    // freeze feedforwards during known discontinuities
    if (_flags.new_wp_destination) {
        _flags.new_wp_destination = false;
        _pos_control.freeze_ff_z();
    }

    _pos_control.update_xy_controller(1.0f);
    check_wp_leash_length();

    _wp_last_update = AP_HAL::millis();

    return ret;
}

②函數advance_wp_target_along_track是航點導航實現的關鍵函數，主要是添加目標航點與origin航線中的中間航點還有track_leash的一些限制。

/// advance_wp_target_along_track - move target location along track from origin to destination
bool AC_WPNav::advance_wp_target_along_track(float dt)
{
    float track_covered;        // distance (in cm) along the track that the vehicle has traveled.  Measured by drawing a perpendicular line from the track to the vehicle.
    Vector3f track_error;       // distance error (in cm) from the track_covered position (i.e. closest point on the line to the vehicle) and the vehicle
    float track_desired_max;    // the farthest distance (in cm) along the track that the leash will allow
    float track_leash_slack;    // additional distance (in cm) along the track from our track_covered position that our leash will allow
    bool reached_leash_limit = false;   // true when track has reached leash limit and we need to slow down the target point

    // get current location
    Vector3f curr_pos = _inav.get_position();

    // calculate terrain adjustments
    float terr_offset = 0.0f;
    if (_terrain_alt && !get_terrain_offset(terr_offset)) {
        return false;
    }

    // calculate 3d vector from segment's origin  計算從origin到當前位置的三維向量
    Vector3f curr_delta = (curr_pos - Vector3f(0,0,terr_offset)) - _origin;

    // calculate how far along the track we are 計算飛機沿着航線到達最近點的距離
    track_covered = curr_delta.x * _pos_delta_unit.x + curr_delta.y * _pos_delta_unit.y + curr_delta.z * _pos_delta_unit.z;

    // calculate the point closest to the vehicle on the segment from origin to destination  將上述距離投射到XYZ三個方向上
    Vector3f track_covered_pos = _pos_delta_unit * track_covered;

    // calculate the distance vector from the vehicle to the closest point on the segment from origin to destination 計算飛機到最近點的距離從origin到目的地
    track_error = curr_delta - track_covered_pos;

    // calculate the horizontal error
    _track_error_xy = norm(track_error.x, track_error.y);

    // calculate the vertical error
    float track_error_z = fabsf(track_error.z);

    // get up leash if we are moving up, down leash if we are moving down
    float leash_z = track_error.z >= 0 ? _pos_control.get_leash_up_z() : _pos_control.get_leash_down_z();

    // use pythagoras's theorem calculate how far along the track we could move the intermediate target before reaching the end of the leash
    //   track_desired_max is the distance from the vehicle to our target point along the track.  It is the "hypotenuse" which we want to be no longer than our leash (aka _track_leash_length)
    //   track_error is the line from the vehicle to the closest point on the track.  It is the "opposite" side
    //   track_leash_slack is the line from the closest point on the track to the target point.  It is the "adjacent" side.  We adjust this so the track_desired_max is no longer than the leash
    // 運用 “勾股定理” 計算飛機在觸及leash尾端之前可以移動到的中間目標點的距離
    // track_desired_max 是飛機到目標航點的距離，是最長的斜邊
    // track_error 是飛機到最近點的直線，是對邊
    // track_leash_slack 是最近點到目標航點的距離，是鄰邊。我們只需要調整鄰邊，就可以保證斜邊不超過leash
    float track_leash_length_abs = fabsf(_track_leash_length);
    float track_error_max_abs = MAX(_track_leash_length*track_error_z/leash_z, _track_leash_length*_track_error_xy/_pos_control.get_leash_xy());
    track_leash_slack = (track_leash_length_abs > track_error_max_abs) ? safe_sqrt(sq(_track_leash_length) - sq(track_error_max_abs)) : 0;
    track_desired_max = track_covered + track_leash_slack;

    // check if target is already beyond the leash
    if (_track_desired > track_desired_max) {
        reached_leash_limit = true;
    }

    // get current velocity
    const Vector3f &curr_vel = _inav.get_velocity();
    // get speed along track
    float speed_along_track = curr_vel.x * _pos_delta_unit.x + curr_vel.y * _pos_delta_unit.y + curr_vel.z * _pos_delta_unit.z;

    // calculate point at which velocity switches from linear to sqrt
    float linear_velocity = _wp_speed_cms;
    float kP = _pos_control.get_pos_xy_p().kP();
    if (is_positive(kP)) {   // avoid divide by zero
        linear_velocity = _track_accel/kP;  //kP可以理解爲1/dt
    }

    // let the limited_speed_xy_cms be some range above or below current velocity along track
    if (speed_along_track < -linear_velocity) {
        // we are traveling fast in the opposite direction of travel to the waypoint so do not move the intermediate point
        _limited_speed_xy_cms = 0; //這裏的_limited_speed_xy_cms 可以理解爲將中間目標點推向最終航點的速度
    }else{
        // increase intermediate target point's velocity if not yet at the leash limit
        if(dt > 0 && !reached_leash_limit) {
            _limited_speed_xy_cms += 2.0f * _track_accel * dt;
        }
        // do not allow speed to be below zero or over top speed
        _limited_speed_xy_cms = constrain_float(_limited_speed_xy_cms, 0.0f, _track_speed);

        // check if we should begin slowing down
        if (!_flags.fast_waypoint) {
            float dist_to_dest = _track_length - _track_desired;
            if (!_flags.slowing_down && dist_to_dest <= _slow_down_dist) {
                _flags.slowing_down = true;
            }
            // if target is slowing down, limit the speed
            if (_flags.slowing_down) {
                _limited_speed_xy_cms = MIN(_limited_speed_xy_cms, get_slow_down_speed(dist_to_dest, _track_accel));
            }
        }

        // if our current velocity is within the linear velocity range limit the intermediate point's velocity to be no more than the linear_velocity above or below our current velocity
        if (fabsf(speed_along_track) < linear_velocity) {
            _limited_speed_xy_cms = constrain_float(_limited_speed_xy_cms,speed_along_track-linear_velocity,speed_along_track+linear_velocity);
        }
    }
    // advance the current target   向前推進當前的目標
    if (!reached_leash_limit) {
    	_track_desired += _limited_speed_xy_cms * dt;

    	// reduce speed if we reach end of leash  如果已經到達了leash的末端就減速
        if (_track_desired > track_desired_max) {
        	_track_desired = track_desired_max;
        	_limited_speed_xy_cms -= 2.0f * _track_accel * dt;
        	if (_limited_speed_xy_cms < 0.0f) {
        	    _limited_speed_xy_cms = 0.0f;
        	}
    	}
    }

    // do not let desired point go past the end of the track unless it's a fast waypoint
    // 除非是快速航點，否則不準理想的中間點超過最終目的地
    if (!_flags.fast_waypoint) {
        _track_desired = constrain_float(_track_desired, 0, _track_length);
    } else {
        _track_desired = constrain_float(_track_desired, 0, _track_length + WPNAV_WP_FAST_OVERSHOOT_MAX);
    }

    // recalculate the desired position
    Vector3f final_target = _origin + _pos_delta_unit * _track_desired;
    // convert final_target.z to altitude above the ekf origin
    final_target.z += terr_offset;
    _pos_control.set_pos_target(final_target);

    // check if we've reached the waypoint  通過reached_destination標誌來判斷是否到達了航點
    if( !_flags.reached_destination ) {
        if( _track_desired >= _track_length ) {
            // "fast" waypoints are complete once the intermediate point reaches the destination
            if (_flags.fast_waypoint) {
                _flags.reached_destination = true;
            }else{
                // regular waypoints also require the copter to be within the waypoint radius
                Vector3f dist_to_dest = (curr_pos - Vector3f(0,0,terr_offset)) - _destination;
                if( dist_to_dest.length() <= _wp_radius_cm ) {
                    _flags.reached_destination = true;
                }
            }
        }
    }

    // update the target yaw if origin and destination are at least 2m apart horizontally
    // 如果到達目的地的距離超過2m那麼就要更新當前目標航向角
    if (_track_length_xy >= WPNAV_YAW_DIST_MIN) {
        if (_pos_control.get_leash_xy() < WPNAV_YAW_DIST_MIN) {
            // if the leash is short (i.e. moving slowly) and destination is at least 2m horizontally, point along the segment from origin to destination
            set_yaw_cd(get_bearing_cd(_origin, _destination));
        } else {
            Vector3f horiz_leash_xy = final_target - curr_pos;
            horiz_leash_xy.z = 0;
            if (horiz_leash_xy.length() > MIN(WPNAV_YAW_DIST_MIN, _pos_control.get_leash_xy()*WPNAV_YAW_LEASH_PCT_MIN)) {
                set_yaw_cd(RadiansToCentiDegrees(atan2f(horiz_leash_xy.y,horiz_leash_xy.x)));
            }
        }
    }

    // successfully advanced along track
    return true;
}

③最後這段代碼就是根據位置控制器裏面輸出的是否需要重新計算leash長度，如果不需要就返回，需要就將位置控制器計算好的leash_xy和leash_z,與根據_pos_delta_unit的水平xy與高度z的相比較的最小數值來計算leash

// check_wp_leash_length - check if waypoint leash lengths need to be recalculated
//  should be called after _pos_control.update_xy_controller which may have changed the position controller leash lengths
void AC_WPNav::check_wp_leash_length()
{
    // exit immediately if recalc is not required
    if (_flags.recalc_wp_leash) {
        calculate_wp_leash_length();
    }
}

/// calculate_wp_leash_length - calculates horizontal and vertical leash lengths for waypoint controller
void AC_WPNav::calculate_wp_leash_length()
{
    // length of the unit direction vector in the horizontal
    float pos_delta_unit_xy = norm(_pos_delta_unit.x, _pos_delta_unit.y);
    float pos_delta_unit_z = fabsf(_pos_delta_unit.z);

    float speed_z;
    float leash_z;
    if (_pos_delta_unit.z >= 0.0f) {
        speed_z = _wp_speed_up_cms;
        leash_z = _pos_control.get_leash_up_z();
    }else{
        speed_z = _wp_speed_down_cms;
        leash_z = _pos_control.get_leash_down_z();
    }

    // calculate the maximum acceleration, maximum velocity, and leash length in the direction of travel
    if(is_zero(pos_delta_unit_z) && is_zero(pos_delta_unit_xy)){
        _track_accel = 0;
        _track_speed = 0;
        _track_leash_length = WPNAV_LEASH_LENGTH_MIN;
    }else if(is_zero(_pos_delta_unit.z)){
        _track_accel = _wp_accel_cmss/pos_delta_unit_xy;
        _track_speed = _wp_speed_cms/pos_delta_unit_xy;
        _track_leash_length = _pos_control.get_leash_xy()/pos_delta_unit_xy;
    }else if(is_zero(pos_delta_unit_xy)){
        _track_accel = _wp_accel_z_cmss/pos_delta_unit_z;
        _track_speed = speed_z/pos_delta_unit_z;
        _track_leash_length = leash_z/pos_delta_unit_z;
    }else{
        _track_accel = MIN(_wp_accel_z_cmss/pos_delta_unit_z, _wp_accel_cmss/pos_delta_unit_xy);
        _track_speed = MIN(speed_z/pos_delta_unit_z, _wp_speed_cms/pos_delta_unit_xy);
        _track_leash_length = MIN(leash_z/pos_delta_unit_z, _pos_control.get_leash_xy()/pos_delta_unit_xy);
    }

    // calculate slow down distance (the distance from the destination when the target point should begin to slow down)
    calc_slow_down_distance(_track_speed, _track_accel);

    // set recalc leash flag to false
    _flags.recalc_wp_leash = false;
}

④ 這裏給出pos_delta_unit的由來，其實就是目的地與origin距離的三維向量與該距離的比值

    Vector3f pos_delta = _destination - _origin;

    _track_length = pos_delta.length(); // get track length
    _track_length_xy = safe_sqrt(sq(pos_delta.x)+sq(pos_delta.y));  // get horizontal track length (used to decide if we should update yaw)

    // calculate each axis' percentage of the total distance to the destination
    if (is_zero(_track_length)) {
        // avoid possible divide by zero
        _pos_delta_unit.x = 0;
        _pos_delta_unit.y = 0;
        _pos_delta_unit.z = 0;
    }else{
        _pos_delta_unit = pos_delta/_track_length;
    }