ArduPilot源码阅读——上层运动模式篇（Guided for boat）
设置目标点
函数调用时机：这一目标点结束后调用；
如果目标点队列为[10,10];[20,20];[30,30]，
首先调用set_desired_location([10,10], [20,20])
表示从原点（默认[0,0]），运动到[10,10]，再运动到[20, 20]，_fast_waypoint = 1
当运动到[10,10]后，再次调用该函数
set_desired_location([20,20], [30, 30])
表示从[10,10]，运动到[20, 20]，再运动到[30, 30]，_fast_waypoint = 1
当运动到[20, 20]后，再调用
set_desired_location([30, 30], [])
表示[30,30]为终点，_fast_waypoint = 0
// set desired location and (optionally) next_destination
// next_destination should be provided if known to allow smooth cornering
// 该函数在达到当前目标点(this_leg)后调用
bool AR_WPNav::set_desired_location(const Location& destination, Location next_destination)
{
// 如果当前路径没有结束就调用该函数，则会先执行停止动作，以停止点为起点，重新开始计算路径
if (!is_active() || !_reached_destination || (_nav_control_type != NavControllerType::NAV_SCURVE)) {
if (!set_origin_and_destination_to_stopping_point()) {
return false;
}
// clear scurves
_scurve_prev_leg.init();
_scurve_this_leg.init();
_scurve_next_leg.init();
}
// shift this leg to previous leg
_scurve_prev_leg = _scurve_this_leg;
// initialise some variables
_origin = _destination;
_destination = destination;
_orig_and_dest_valid = true;
_reached_destination = false;
update_distance_and_bearing_to_destination();
// check if vehicle should pivot if vehicle stopped at previous waypoint
// or journey to previous waypoint was interrupted or navigation has just started
if (!_fast_waypoint) { //_fast_waypoint下一个航路点是否快速通过（不停止）
_pivot.deactivate();
_pivot.check_activation((_reversed ? wrap_360_cd(oa_wp_bearing_cd() + 18000) : oa_wp_bearing_cd()) * 0.01, _pivot_at_next_wp);
}
// convert origin and destination to offset from EKF origin
// 把起点终点的经纬度转换成二维坐标系，用m为单位表示
Vector2f origin_NE;
Vector2f destination_NE;
if (!_origin.get_vector_xy_from_origin_NE(origin_NE) ||
!_destination.get_vector_xy_from_origin_NE(destination_NE)) {
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
return false;
}
origin_NE *= 0.01f;
destination_NE *= 0.01f;
// calculate track to destination
if (_fast_waypoint && !_scurve_next_leg.finished()) {
//下一点为快速点，并且下一段路径没有结束，表示下一段为连续路径，直接把计算好的路径拷贝当前路径
// skip recalculating this leg by simply shifting next leg
_scurve_this_leg = _scurve_next_leg;
} else {
_scurve_this_leg.
calculate_track(Vector3f{origin_NE.x, origin_NE.y, 0.0f}, // origin
Vector3f{destination_NE.x, destination_NE.y, 0.0f}, // destination
_pos_control.get_speed_max(),
_pos_control.get_speed_max(), // speed up (not used) 垂直方向 2D用不到
_pos_control.get_speed_max(), // speed down (not used) 垂直方向 2D用不到
_pos_control.get_accel_max(), // forward back acceleration
_pos_control.get_accel_max(), // vertical accel (not used)垂直方向 2D用不到
AR_WPNAV_SNAP_MAX, // snap 加加加速度
_pos_control.get_jerk_max()); // jerk 加加速度
}
// handle next destination
_scurve_next_leg.init();
_fast_waypoint = false;
_pivot_at_next_wp = false;
if (next_destination.initialised()) { //检查下一个目标点是否有效？如果有效则计算下一路径
// check if vehicle should pivot at next waypoint
const float next_wp_yaw_change = get_corner_angle(_origin, destination, next_destination);
_pivot_at_next_wp = _pivot.would_activate(next_wp_yaw_change);
if (!_pivot_at_next_wp) { //如果下一个目标点不进行原地转向，则计算路径
// convert next_destination to offset from EKF origin
Vector2f next_destination_NE;
if (!next_destination.get_vector_xy_from_origin_NE(next_destination_NE)) {
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
return false;
}
next_destination_NE *= 0.01f;
_scurve_next_leg.
calculate_track(Vector3f{destination_NE.x, destination_NE.y, 0.0f},
Vector3f{next_destination_NE.x, next_destination_NE.y, 0.0f},
_pos_control.get_speed_max(),
_pos_control.get_speed_max(), // speed up (not used)
_pos_control.get_speed_max(), // speed down (not used)
_pos_control.get_accel_max(), // forward back acceleration
_pos_control.get_accel_max(), // vertical accel (not used)
AR_WPNAV_SNAP_MAX, // snap
_pos_control.get_jerk_max());
// next destination provided so fast waypoint
_fast_waypoint = true; //同时把下一路径标记为快速路径
}
}
// scurves used for navigation to destination
_nav_control_type = NavControllerType::NAV_SCURVE;
update_distance_and_bearing_to_destination();
return true;
}运动模式
void ModeGuided::update()
{
switch (_guided_mode) {
case SubMode::WP: {...}
case SubMode::HeadingAndSpeed: {...}
case SubMode::TurnRateAndSpeed: {...}
case SubMode::Loiter: {...}
case SubMode::SteeringAndThrottle: {...}
case SubMode::Stop:
stop_vehicle();
break;
default:
gcs().send_text(MAV_SEVERITY_WARNING, "Unknown GUIDED mode");
break;
}
}目标点模式——WayPoint(WP)
整体流程
case SubMode::WP:
{
// check if we've reached the destination
if (!g2.wp_nav.reached_destination()) {
// update navigation controller
navigate_to_waypoint();
} else {
// send notification
if (send_notification) {
send_notification = false;
rover.gcs().send_mission_item_reached_message(0);
}
// we have reached the destination so stay here
if (rover.is_boat()) {
if (!start_loiter()) {
stop_vehicle();
}
} else {
stop_vehicle();
}
// update distance to destination
_distance_to_destination = \
rover.current_loc.get_distance(g2.wp_nav.get_destination());
}
break;
}具体细节
navigate_to_waypoint
void Mode::navigate_to_waypoint()
{
/* .... */
// 上面可以忽略，用遥控器调速
// update navigation controller
g2.wp_nav.update(rover.G_Dt); //***核心更新，重点研读***
_distance_to_destination = g2.wp_nav.get_distance_to_destination(); //更新离航向点的距离
#if AP_AVOIDANCE_ENABLED
/* 避障相关 */
#endif
// pass desired speed to throttle controller
// do not do simple avoidance because this is already handled in the position controller
calc_throttle(g2.wp_nav.get_speed(), false); //从期望速度，计算油门 -- 输出1
float desired_heading_cd = g2.wp_nav.oa_wp_bearing_cd(); //从控制器获取期望航向角
if (g2.sailboat.use_indirect_route(desired_heading_cd)) { //如果是帆船 跳过不看
/* 帆船控制 忽略 */
} else {
// retrieve turn rate from waypoint controller
float desired_turn_rate_rads = g2.wp_nav.get_turn_rate_rads(); //从控制器获取期望角速度
// if simple avoidance is active at very low speed do not attempt to turn
#if AP_AVOIDANCE_ENABLED
/* 避障相关 */
#endif
// call turn rate steering controller
calc_steering_from_turn_rate(desired_turn_rate_rads); //从期望角速度，计算转向 -- 输出2
}
}
// update navigation
void AR_WPNav_OA::update(float dt)
{
#if AP_OAPATHPLANNER_ENABLED
/* 避障算法 该章节暂时不考虑 */
#endif // AP_OAPATHPLANNER_ENABLED
// call parent update
AR_WPNav::update(dt); //***核心更新，重点研读***
}AR_WPNav::update
更新位置，方位角；更新最大速度；根据当前位置更新下一迭代期望位置
输出throttle和steer控制信号
// update navigation
void AR_WPNav::update(float dt)
{
// exit immediately if no current location, origin or destination
Location current_loc;
float speed;
if (!hal.util->get_soft_armed() || !_orig_and_dest_valid || !AP::ahrs().get_location(current_loc) || !_atc.get_forward_speed(speed)) {
_desired_speed_limited = _atc.get_desired_speed_accel_limited(0.0f, dt);
_desired_lat_accel = 0.0f;
_desired_turn_rate_rads = 0.0f;
_cross_track_error = 0;
return;
}
// if no recent calls initialise desired_speed_limited to current speed
if (!is_active()) {
_desired_speed_limited = speed;
}
_last_update_ms = AP_HAL::millis();
update_distance_and_bearing_to_destination(); //根据目前位置计算到目标点的距离和方位
// handle change in max speed
update_speed_max(); //更新最大速度
// advance target along path unless vehicle is pivoting
if (!_pivot.active()) {
switch (_nav_control_type) {
case NavControllerType::NAV_SCURVE:
advance_wp_target_along_track(current_loc, dt); //核心代码
break;
case NavControllerType::NAV_PSC_INPUT_SHAPING:
update_psc_input_shaping(dt);
break;
}
}
// update_steering_and_speed
update_steering_and_speed(current_loc, dt);
}
advance_wp_target_along_track
根据当前位置速度，缩放dt
根据advance_target_along_track输出位置，速度，加速度，传入位置控制器
// move target location along track from origin to destination using SCurves navigation
void AR_WPNav::advance_wp_target_along_track(const Location ¤t_loc, float dt)
{
// exit immediately if no current location, destination or disarmed
Vector2f curr_pos_NE;
Vector3f curr_vel_NED;
if (!AP::ahrs().get_relative_position_NE_origin(curr_pos_NE) || !AP::ahrs().get_velocity_NED(curr_vel_NED)) {
return;
}
// exit immediately if we can't convert waypoint origin to offset from ekf origin
Vector2f origin_NE;
if (!_origin.get_vector_xy_from_origin_NE(origin_NE)) { //获取origin航点相对于起始位置的的坐标（NE）
return;
}
// convert from cm to meters
origin_NE *= 0.01f; //以米为单位
// use _track_scalar_dt to slow down S-Curve time to prevent target moving too far in front of vehicle
Vector2f curr_target_vel = _pos_control.get_desired_velocity(); //获取期望速度
float track_scaler_dt = 1.0f;
if (is_positive(curr_target_vel.length())) {
Vector2f track_direction = curr_target_vel.normalized();
const float track_error = _pos_control.get_pos_error().tofloat().dot(track_direction);
float track_velocity = curr_vel_NED.xy().dot(track_direction);
// set time scaler to be consistent with the achievable vehicle speed with a 5% buffer for short term variation.
const float time_scaler_dt_max = _overspeed_enabled ? AR_WPNAV_OVERSPEED_RATIO_MAX : 1.0f;
track_scaler_dt = constrain_float(0.05f + (track_velocity - _pos_control.get_pos_p().kP() * track_error) / curr_target_vel.length(), 0.0f, time_scaler_dt_max);
}
// change s-curve time speed with a time constant of maximum acceleration / maximum jerk
float track_scaler_tc = 1.0f;
if (is_positive(_pos_control.get_jerk_max())) {
track_scaler_tc = _pos_control.get_accel_max() / _pos_control.get_jerk_max();
}
_track_scalar_dt += (track_scaler_dt - _track_scalar_dt) * (dt / track_scaler_tc);
// target position, velocity and acceleration from straight line or spline calculators
Vector3f target_pos_3d_ftype{origin_NE.x, origin_NE.y, 0.0f};
Vector3f target_vel, target_accel;
// update target position, velocity and acceleration
const float wp_radius = MAX(_radius, _turn_radius);
bool s_finished = _scurve_this_leg.advance_target_along_track(_scurve_prev_leg, _scurve_next_leg, wp_radius,
_pos_control.get_lat_accel_max(), _fast_waypoint, _track_scalar_dt * dt,
target_pos_3d_ftype, target_vel, target_accel);
// pass new target to the position controller
init_pos_control_if_necessary();
Vector2p target_pos_ptype{target_pos_3d_ftype.x, target_pos_3d_ftype.y};
_pos_control.set_pos_vel_accel_target(target_pos_ptype, target_vel.xy(), target_accel.xy());
// check if we've reached the waypoint
if (!_reached_destination && s_finished) {
// "fast" waypoints are complete once the intermediate point reaches the destination
if (_fast_waypoint) {
_reached_destination = true;
} else {
// regular waypoints also require the vehicle to be within the waypoint radius or past the "finish line"
const bool near_wp = current_loc.get_distance(_destination) <= _radius;
const bool past_wp = current_loc.past_interval_finish_line(_origin, _destination);
_reached_destination = near_wp || past_wp;
}
}
}advance_target_along_track
核心函数，根据时间计算在路径上每一点的位置，速度，加速度
// move target location along path from origin to destination
// prev_leg and next_leg are the paths before and after this path
// wp_radius is max distance from the waypoint at the apex of the turn
// fast_waypoint should be true if vehicle will not stop at end of this leg
// dt is the time increment the vehicle will move along the path
// target_pos should be set to this segment's origin and it will be updated to the current position target
// target_vel and target_accel are updated with new targets
// returns true if vehicle has passed the apex of the corner
// 按dt为时间间隔，根据上一条路径和当前路径计算路径上每一时间点的路径位置，路径速度，路径加速度
// 输入参数： wp_radius 判定转弯开始的最大距离 fast_waypoint 标识下一个路径点为快速通过点（不停止通过）
// 输入输出参数：target_pos输入为当前路径（current_leg）的起始点（origin）， 输出为当前累计时间点时的位置
// 输入输出参数：target_vel target_accel同上target_pos
// 返回参数： 真——当前路径结束
bool SCurve::advance_target_along_track(SCurve &prev_leg, SCurve &next_leg, float wp_radius, float accel_corner, bool fast_waypoint, float dt, Vector3f &target_pos, Vector3f &target_vel, Vector3f &target_accel)
{
// 根据前路径信息，更新当前路径的位置速度加速度
prev_leg.move_to_pos_vel_accel(dt, target_pos, target_vel, target_accel);
move_from_pos_vel_accel(dt, target_pos, target_vel, target_accel); // 根据现路径信息，更新当前路径的位置速度加速度
bool s_finished = finished(); //现路径结束判断——时间，路径长度
// check for change of leg on fast waypoint
const float time_to_destination = get_time_remaining();
/**
* 判断何时进行转弯
* fast_waypoint 下一点为快速通过
* is_zero(next_leg.get_time_elapsed()) 下一段路径尚未开始
* (get_time_elapsed() >= time_turn_out() - next_leg.time_turn_in()) 允许转弯的时间段为 加加速完成-匀速结束
* (position_sq >= 0.25 * track.length_squared()) 当前路径已经经过总路径长度的四分之一
*
* 满足上诉所有条件，方可进入转弯计算
*/
if (fast_waypoint
&& is_zero(next_leg.get_time_elapsed())
&& (get_time_elapsed() >= time_turn_out() - next_leg.time_turn_in())
&& (position_sq >= 0.25 * track.length_squared())) {
Vector3f turn_pos = -get_track();
Vector3f turn_vel, turn_accel;
move_from_time_pos_vel_accel(get_time_elapsed() + time_to_destination * 0.5f, turn_pos, turn_vel, turn_accel);
//进入转弯计算时，已经开始下一段路径
next_leg.move_from_time_pos_vel_accel(time_to_destination * 0.5f, turn_pos, turn_vel, turn_accel);
const float speed_min = MIN(get_speed_along_track(), next_leg.get_speed_along_track());
if ((get_time_remaining() < next_leg.time_end() * 0.5f) && (turn_pos.length() < wp_radius) &&
(Vector2f{turn_vel.x, turn_vel.y}.length() < speed_min) &&
(Vector2f{turn_accel.x, turn_accel.y}.length() < accel_corner)) {
next_leg.move_from_pos_vel_accel(dt, target_pos, target_vel, target_accel);
}
} else if (!is_zero(next_leg.get_time_elapsed())) { // 计算转弯后，已经开始下一段路径
next_leg.move_from_pos_vel_accel(dt, target_pos, target_vel, target_accel);
if (next_leg.get_time_elapsed() >= get_time_remaining()) { // 如果下一路径的时间进度超过当前路径的剩余时间，则判定当前路径完成
s_finished = true;
}
}
return s_finished;
}MATLAB仿真
way_points = [10,10;20,30;30,30]; %路径点队列
wp_controller = WPNav();
wp_controller.speed_max = 5; %限制最大速度
wp_controller.accel_max = 2; %限制最大加速度
wp_controller.jerk_max = 4; %限制最大加加速度
wp_controller.snap_max = 8; %限制最大加加加速度
wp_radius = 2; %到达终点最大感知距离
accel_corner = 2; %横向加速度（向心加速度）
dt = 0.1; %时间间隔0.1s
plot_vel =[];
plot_accel = [];
plot_pos = [];
for i = 1:size(way_points, 1)
%处理路径点
if i < size(way_points, 1)
way_point = way_points(i, :);
next_point = way_points(i+1, :);
else
way_point = way_points(i, :);
next_point = NaN;
end
if (i == 1)||(wp_controller.reached_destination)
wp_controller.set_desired_location(way_point, next_point); %设置路径点
while ~wp_controller.reached_destination
%迭代计算路径中每一点的信息，直到该路径结束
target_vel = zeros(1,2);
target_accel = zeros(1,2);
target_pos = wp_controller.origin;
[s_finished, prev_leg, next_leg, target_pos, target_vel, target_accel] = ...
wp_controller.scurve_this_leg.
advance_target_along_track(wp_controller.scurve_prev_leg, wp_controller.scurve_next_leg, ...
wp_radius, accel_corner, wp_controller.fast_waypoint, dt, target_pos, target_vel, target_accel);
%记录每一点信息，方便作图
plot_accel = [plot_accel;target_accel];
plot_vel = [plot_vel;target_vel];
plot_pos = [plot_pos;target_pos];
%检测是否达到目标点
if (~wp_controller.reached_destination && s_finished)
if wp_controller.fast_waypoint
wp_controller.reached_destination = true;
else
wp_controller.reached_destination = norm(way_point - target_pos) <= wp_radius;
end
end
end
end
end
