版权声明:本文为博主原创博文,未经允许不得转载,若要转载,请说明出处并给出博文链接
本文详细讲解了ArduPilot中AP_L1_control库中的update_waypoint函数的具体实现!!!
下面是源代码:
// 更新L1 control的航点
void AP_L1_Control::update_waypoint(const struct Location &prev_WP, const struct Location &next_WP, float dist_min)
{
struct Location _current_loc;
float Nu;
float xtrackVel;
float ltrackVel;
uint32_t now = AP_HAL::micros();
float dt = (now - _last_update_waypoint_us) * 1.0e-6f;
if (dt > 0.1) {
dt = 0.1;
_L1_xtrack_i = 0.0f;
}
_last_update_waypoint_us = now;
// 用特定的阻尼来计算L1的增益
float K_L1 = 4.0f * _L1_damping * _L1_damping;
// 获取当前飞机的位置
if (_ahrs.get_position(_current_loc) == false) {
// if no GPS loc available, maintain last nav/target_bearing
_data_is_stale = true;
return;
}
Vector2f _groundspeed_vector = _ahrs.groundspeed_vector();
// 更新目标方位角
_target_bearing_cd = get_bearing_cd(_current_loc, next_WP);
//计算地速
float groundSpeed = _groundspeed_vector.length();
if (groundSpeed < 0.1f) {
// use a small ground speed vector in the right direction,
// allowing us to use the compass heading at zero GPS velocity
groundSpeed = 0.1f;
_groundspeed_vector = Vector2f(cosf(get_yaw()), sinf(get_yaw())) * groundSpeed;
}
// 计算时变控制参数
// 根据_L1_period计算特定L1长度
// 0.3183099 = 1/pi
_L1_dist = MAX(0.3183099f * _L1_damping * _L1_period * groundSpeed, dist_min);
// 计算航点A与B的北东位置长度
Vector2f AB = location_diff(prev_WP, next_WP);
float AB_length = AB.length();
//如果AB距离很短,那么就直接将B点记作当前目标点,径直朝向B
if (AB.length() < 1.0e-6f) {
AB = location_diff(_current_loc, next_WP);
if (AB.length() < 1.0e-6f) {
AB = Vector2f(cosf(get_yaw()), sinf(get_yaw()));
}
}
AB.normalize();
// 计算航点A到飞机的北东距离长度信息
Vector2f A_air = location_diff(prev_WP, _current_loc);
// 计算航迹跟踪误差
_crosstrack_error = A_air % AB;
float WP_A_dist = A_air.length();
float alongTrackDist = A_air * AB;
if (WP_A_dist > _L1_dist && alongTrackDist/MAX(WP_A_dist, 1.0f) < -0.7071f)
{
//飞机航向与A_air航向反方向夹角超过135度,且L1_dist < WP_A_dist,则指向A
//Calc Nu to fly To WP A
Vector2f A_air_unit = (A_air).normalized(); // Unit vector from WP A to aircraft
xtrackVel = _groundspeed_vector % (-A_air_unit); // Velocity across line
ltrackVel = _groundspeed_vector * (-A_air_unit); // Velocity along line
Nu = atan2f(xtrackVel,ltrackVel);
_nav_bearing = atan2f(-A_air_unit.y , -A_air_unit.x); // bearing (radians) from AC to L1 point
} else if (alongTrackDist > AB_length + groundSpeed*3) {
// 飞机超过了航点B3秒的时间,那么指向B
// Calc Nu to fly To WP B
Vector2f B_air = location_diff(next_WP, _current_loc);
Vector2f B_air_unit = (B_air).normalized(); // Unit vector from WP B to aircraft
xtrackVel = _groundspeed_vector % (-B_air_unit); // Velocity across line
ltrackVel = _groundspeed_vector * (-B_air_unit); // Velocity along line
Nu = atan2f(xtrackVel,ltrackVel);
_nav_bearing = atan2f(-B_air_unit.y , -B_air_unit.x); // bearing (radians) from AC to L1 point
} else { //Calc Nu to fly along AB line
//Calculate Nu2 angle (angle of velocity vector relative to line connecting waypoints)
xtrackVel = _groundspeed_vector % AB; // Velocity cross track
ltrackVel = _groundspeed_vector * AB; // Velocity along track
float Nu2 = atan2f(xtrackVel,ltrackVel);
//Calculate Nu1 angle (Angle to L1 reference point)
float sine_Nu1 = _crosstrack_error/MAX(_L1_dist, 0.1f);
//Limit sine of Nu1 to provide a controlled track capture angle of 45 deg
sine_Nu1 = constrain_float(sine_Nu1, -0.7071f, 0.7071f);
float Nu1 = asinf(sine_Nu1);
// 计算积分项,减少稳态误差
if (_L1_xtrack_i_gain <= 0 || !is_equal(_L1_xtrack_i_gain.get(), _L1_xtrack_i_gain_prev)) {
_L1_xtrack_i = 0;
_L1_xtrack_i_gain_prev = _L1_xtrack_i_gain;
} else if (fabsf(Nu1) < radians(5)) {
_L1_xtrack_i += Nu1 * _L1_xtrack_i_gain * dt;
// an AHRS_TRIM_X=0.1 will drift to about 0.08 so 0.1 is a good worst-case to clip at
_L1_xtrack_i = constrain_float(_L1_xtrack_i, -0.1f, 0.1f);
}
// to converge to zero we must push Nu1 harder
Nu1 += _L1_xtrack_i;
Nu = Nu1 + Nu2;
_nav_bearing = atan2f(AB.y, AB.x) + Nu1; // bearing (radians) from AC to L1 point
}
_prevent_indecision(Nu);
_last_Nu = Nu;
//Limit Nu to +-(pi/2)
Nu = constrain_float(Nu, -1.5708f, +1.5708f);
_latAccDem = K_L1 * groundSpeed * groundSpeed / _L1_dist * sinf(Nu);
// Waypoint capture status is always false during waypoint following
_WPcircle = false;
_bearing_error = Nu; // bearing error angle (radians), +ve to left of track
_data_is_stale = false; // status are correctly updated with current waypoint data
}
单纯看代码可能略显枯燥,可以结合ArduPilot——AP_L1_control——更新航点update_waypoint(一)与下图一起参考借鉴。
注意:update_waypoint<——>waypoint following。上图给出的就是飞机在追踪航点的一个较为直观,且提供了代码对应的数学公式,以及一些跟踪逻辑,这更加有助于理解L1 guidance。
_latAccDem最终会被用来计算飞机的目标roll—nav_roll_cd,见下面代码。从而将nav_roll_cd作为Roll controller的输入值,进一步通过PID控制器来控制roll角。
int32_t AP_L1_Control::nav_roll_cd(void) const
{
float ret;
ret = cosf(_ahrs.pitch)*degrees(atanf(_latAccDem * 0.101972f) * 100.0f); // 0.101972 = 1/9.81
ret = constrain_float(ret, -9000, 9000);
return ret;
}
画圈跟踪可以参看L1 control——ArduPilot——更新圆圈update_Loiter