ECL_L1_Pos_Controller::navigate_waypoints 函数详解：基于 L1 轨迹的航点导航算法

#include "ECL_L1_Pos_Controller.hpp" #include <lib/geo/geo.h> #include <px4_platform_common/defines.h> #include <float.h> using matrix::Vector2f; void ECL_L1_Pos_Controller::navigate_waypoints(const Vector2f &vector_A, const Vector2f &vector_B, const Vector2f &vector_curr_position, const Vector2f &ground_speed_vector) { \t/* this follows the logic presented in [1] / \tfloat eta = 0.0f; \t/ get the direction between the last (visited) and next waypoint / \tVector2f vector_P_to_B = vector_B - vector_curr_position; \tVector2f vector_P_to_B_unit = vector_P_to_B.normalized(); \t_target_bearing = atan2f(vector_P_to_B_unit(1), vector_P_to_B_unit(0)); \t/ enforce a minimum ground speed of 0.1 m/s to avoid singularities / \tfloat ground_speed = math::max(ground_speed_vector.length(), 0.1f); \t/ calculate the L1 length required for the desired period / \t_L1_distance = _L1_ratio * ground_speed; \t/ calculate vector from A to B / \tVector2f vector_AB = vector_B - vector_A; \t/ \t * check if waypoints are on top of each other. If yes, \t * skip A and directly continue to B \t / \tif (vector_AB.length() < 1.0e-6f) { \t\tvector_AB = vector_B - vector_curr_position; \t} \tvector_AB.normalize(); \t/ calculate the vector from waypoint A to the aircraft / \tVector2f vector_A_to_airplane = vector_curr_position - vector_A; \t/ calculate crosstrack error (output only) / \t_crosstrack_error = vector_AB % vector_A_to_airplane; \t/ \t * If the current position is in a +-135 degree angle behind waypoint A \t * and further away from A than the L1 distance, then A becomes the L1 point. \t * If the aircraft is already between A and B normal L1 logic is applied. \t / \tfloat distance_A_to_airplane = vector_A_to_airplane.length(); \tfloat alongTrackDist = vector_A_to_airplane * vector_AB; \t/ estimate airplane position WRT to B / \tVector2f vector_B_to_P = vector_curr_position - vector_B; \tVector2f vector_B_to_P_unit = vector_B_to_P.normalized(); \t/ calculate angle of airplane position vector relative to line) / \t// XXX this could probably also be based solely on the dot product \tfloat AB_to_BP_bearing = atan2f(vector_B_to_P_unit % vector_AB, vector_B_to_P_unit * vector_AB); \t/ extension from [2], fly directly to A / \tif (distance_A_to_airplane > _L1_distance && alongTrackDist / math::max(distance_A_to_airplane, 1.0f) < -0.7071f) { \t\t/ calculate eta to fly to waypoint A / \t\t/ unit vector from waypoint A to current position / \t\tVector2f vector_A_to_airplane_unit = vector_A_to_airplane.normalized(); \t\t/ velocity across / orthogonal to line / \t\tfloat xtrack_vel = ground_speed_vector % (-vector_A_to_airplane_unit); \t\t/ velocity along line / \t\tfloat ltrack_vel = ground_speed_vector * (-vector_A_to_airplane_unit); \t\teta = atan2f(xtrack_vel, ltrack_vel); \t\t/ bearing from current position to L1 point / \t\t_nav_bearing = atan2f(-vector_A_to_airplane_unit(1), -vector_A_to_airplane_unit(0)); \t\t/ \t\t * If the AB vector and the vector from B to airplane point in the same \t\t * direction, we have missed the waypoint. At +- 90 degrees we are just passing it. \t\t / \t} else if (fabsf(AB_to_BP_bearing) < math::radians(100.0f)) { \t\t/ \t\t * Extension, fly back to waypoint. \t\t * \t\t * This corner case is possible if the system was following \t\t * the AB line from waypoint A to waypoint B, then is \t\t * switched to manual mode (or otherwise misses the waypoint) \t\t * and behind the waypoint continues to follow the AB line. \t\t / \t\t/ calculate eta to fly to waypoint B / \t\t/ velocity across / orthogonal to line / \t\tfloat xtrack_vel = ground_speed_vector % (-vector_B_to_P_unit); \t\t/ velocity along line / \t\tfloat ltrack_vel = ground_speed_vector * (-vector_B_to_P_unit); \t\teta = atan2f(xtrack_vel, ltrack_vel); \t\t/ bearing from current position to L1 point / \t\t_nav_bearing = atan2f(-vector_B_to_P_unit(1), -vector_B_to_P_unit(0)); \t} else { \t\t/ calculate eta to fly along the line between A and B / \t\t/ velocity across / orthogonal to line / \t\tfloat xtrack_vel = ground_speed_vector % vector_AB; \t\t/ velocity along line / \t\tfloat ltrack_vel = ground_speed_vector * vector_AB; \t\t/ calculate eta2 (angle of velocity vector relative to line) / \t\tfloat eta2 = atan2f(xtrack_vel, ltrack_vel); \t\t/ calculate eta1 (angle to L1 point) / \t\tfloat xtrackErr = vector_A_to_airplane % vector_AB; \t\tfloat sine_eta1 = xtrackErr / math::max(_L1_distance, 0.1f); \t\t/ limit output to feasible values / \t\tsine_eta1 = math::constrain(sine_eta1, -1.0f, 1.0f); \t\tfloat eta1 = asinf(sine_eta1); \t\teta = eta1 + eta2; \t\t/ bearing from current position to L1 point / \t\t_nav_bearing = atan2f(vector_AB(1), vector_AB(0)) + eta1; \t} \t/ limit angle to +-90 degrees / \teta = math::constrain(eta, (-M_PI_F) / 2.0f, +M_PI_F / 2.0f); \t_lateral_accel = _K_L1 * ground_speed * ground_speed / _L1_distance * sinf(eta); } void ECL_L1_Pos_Controller::set_l1_period(float period) { \t_L1_period = period; \t/ calculate the ratio introduced in [2] / \t_L1_ratio = 1.0f / M_PI_F * _L1_damping * _L1_period; \t/ calculate normalized frequency for heading tracking / \t_heading_omega = sqrtf(2.0f) * M_PI_F / _L1_period; } void ECL_L1_Pos_Controller::set_l1_damping(float damping) { \t_L1_damping = damping; \t/ calculate the ratio introduced in [2] / \t_L1_ratio = 1.0f / M_PI_F * _L1_damping * _L1_period; \t/ calculate the L1 gain (following [2]) */ \t_K_L1 = 4.0f * _L1_damping * _L1_damping;