From b3d744505935bc529881fb0eb39075b79d2d9bbf Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 27 Jan 2020 12:03:29 +0100 Subject: [PATCH] PositionControl: acceleration based control strategy --- .../PositionControl/PositionControl.cpp | 78 ++++++++----------- .../PositionControl/PositionControl.hpp | 1 + .../PositionControl/PositionControlTest.cpp | 77 +++++++++++++----- .../mc_pos_control/mc_pos_control_main.cpp | 10 +-- 4 files changed, 95 insertions(+), 71 deletions(-) diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp index 536c3ae997..792e411da5 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp @@ -40,6 +40,7 @@ #include #include #include +#include using namespace matrix; @@ -83,7 +84,6 @@ void PositionControl::setInputSetpoint(const vehicle_local_position_setpoint_s & _pos_sp = Vector3f(setpoint.x, setpoint.y, setpoint.z); _vel_sp = Vector3f(setpoint.vx, setpoint.vy, setpoint.vz); _acc_sp = Vector3f(setpoint.acceleration); - _thr_sp = Vector3f(setpoint.thrust); _yaw_sp = setpoint.yaw; _yawspeed_sp = setpoint.yawspeed; } @@ -114,7 +114,7 @@ bool PositionControl::update(const float dt) // x and y input setpoints always have to come in pairs const bool valid = (PX4_ISFINITE(_pos_sp(0)) == PX4_ISFINITE(_pos_sp(1))) && (PX4_ISFINITE(_vel_sp(0)) == PX4_ISFINITE(_vel_sp(1))) - && (PX4_ISFINITE(_thr_sp(0)) == PX4_ISFINITE(_thr_sp(1))); + && (PX4_ISFINITE(_acc_sp(0)) == PX4_ISFINITE(_acc_sp(1))); _positionControl(); _velocityControl(dt); @@ -143,42 +143,16 @@ void PositionControl::_positionControl() void PositionControl::_velocityControl(const float dt) { - // Generate desired thrust setpoint. - // PID - // u_des = P(vel_err) + D(vel_err_dot) + I(vel_integral) - // Umin <= u_des <= Umax - // - // Anti-Windup: - // u_des = _thr_sp; r = _vel_sp; y = _vel - // u_des >= Umax and r - y >= 0 => Saturation = true - // u_des >= Umax and r - y <= 0 => Saturation = false - // u_des <= Umin and r - y <= 0 => Saturation = true - // u_des <= Umin and r - y >= 0 => Saturation = false - // - // Notes: - // - PID implementation is in NED-frame - // - control output in D-direction has priority over NE-direction - // - the equilibrium point for the PID is at hover-thrust - // - the maximum tilt cannot exceed 90 degrees. This means that it is - // not possible to have a desired thrust direction pointing in the positive - // D-direction (= downward) - // - the desired thrust in D-direction is limited by the thrust limits - // - the desired thrust in NE-direction is limited by the thrust excess after - // consideration of the desired thrust in D-direction. In addition, the thrust in - // NE-direction is also limited by the maximum tilt. - // PID velocity control Vector3f vel_error = _vel_sp - _vel; - Vector3f thr_sp_velocity = vel_error.emult(_gain_vel_p) + _vel_int + _vel_dot.emult(_gain_vel_d); - thr_sp_velocity -= Vector3f(0.f, 0.f, _hover_thrust); + Vector3f acc_sp_velocity = vel_error.emult(_gain_vel_p) + _vel_int + _vel_dot.emult(_gain_vel_d); - if (PX4_ISFINITE(_thr_sp(0)) && PX4_ISFINITE(_thr_sp(1)) && PX4_ISFINITE(thr_sp_velocity(2))) { - // Thrust set-point in NE-direction from FlightTaskManualAltitude is provided. Scaling by the maximum tilt is required. - _thr_sp.xy() = Vector2f(_thr_sp) * fabsf(thr_sp_velocity(2)) * tanf(_constraints.tilt); - } + // For backwards compatibility of the gains to non-acceleration based control, needs to be overcome with configuration conversion + acc_sp_velocity *= CONSTANTS_ONE_G / _hover_thrust; + // No control input from setpoints or corresponding states which are NAN + ControlMath::addIfNotNanVector3f(_acc_sp, acc_sp_velocity); - // Velocity and feed-forward thrust setpoints or velocity states being NAN results in them not having an influence - ControlMath::addIfNotNanVector3f(_thr_sp, thr_sp_velocity); + _accelerationControl(); // Integrator anti-windup in vertical direction if ((_thr_sp(2) >= -_lim_thr_min && vel_error(2) >= 0.0f) || @@ -186,23 +160,23 @@ void PositionControl::_velocityControl(const float dt) vel_error(2) = 0.f; } - // Saturate thrust setpoint in D-direction. - _thr_sp(2) = math::constrain(_thr_sp(2), -_lim_thr_max, -_lim_thr_min); + // Saturate maximal vertical thrust + _thr_sp(2) = math::max(_thr_sp(2), -_lim_thr_max); - // Get maximum allowed thrust in NE based on tilt and excess thrust. - float thrust_max_NE_tilt = fabsf(_thr_sp(2)) * tanf(_constraints.tilt); - float thrust_max_NE = sqrtf(_lim_thr_max * _lim_thr_max - _thr_sp(2) * _thr_sp(2)); - thrust_max_NE = math::min(thrust_max_NE_tilt, thrust_max_NE); + // Get allowed horizontal thrust after prioritizing vertical control + const float thrust_max_squared = _lim_thr_max * _lim_thr_max; + const float thrust_z_squared = _thr_sp(2) * _thr_sp(2); + float thrust_max_xy = sqrtf(thrust_max_squared - thrust_z_squared); - // Saturate thrust in NE-direction. + // Saturate thrust in horizontal direction const Vector2f thrust_sp_xy(_thr_sp); const float thrust_sp_xy_norm = thrust_sp_xy.norm(); - if (thrust_sp_xy_norm > thrust_max_NE) { - _thr_sp.xy() = thrust_sp_xy / thrust_sp_xy_norm * thrust_max_NE; + if (thrust_sp_xy_norm > thrust_max_xy) { + _thr_sp.xy() = thrust_sp_xy / thrust_sp_xy_norm * thrust_max_xy; } - // Use tracking Anti-Windup for NE-direction: during saturation, the integrator is used to unsaturate the output + // Use tracking Anti-Windup for horizontal direction: during saturation, the integrator is used to unsaturate the output // see Anti-Reset Windup for PID controllers, L.Rundqwist, 1990 const float arw_gain = 2.f / _gain_vel_p(0); vel_error.xy() = Vector2f(vel_error) - (arw_gain * (thrust_sp_xy - Vector2f(_thr_sp))); @@ -216,6 +190,19 @@ void PositionControl::_velocityControl(const float dt) _vel_int(2) = math::min(fabsf(_vel_int(2)), _lim_thr_max) * sign(_vel_int(2)); } +void PositionControl::_accelerationControl() +{ + // Assume standard acceleration due to gravity in vertical direction for attitude generation + Vector3f body_z = Vector3f(-_acc_sp(0), -_acc_sp(1), CONSTANTS_ONE_G).normalized(); + ControlMath::limitTilt(body_z, Vector3f(0, 0, 1), _constraints.tilt); + // Scale thrust assuming hover thrust produces standard gravity + float collective_thrust = _acc_sp(2) * (_hover_thrust / CONSTANTS_ONE_G) - _hover_thrust; + // Project thrust to planned body attitude + collective_thrust /= (Vector3f(0, 0, 1).dot(body_z)); + collective_thrust = math::min(collective_thrust, -_lim_thr_min); + _thr_sp = body_z * collective_thrust; +} + bool PositionControl::_updateSuccessful() { bool valid = true; @@ -231,8 +218,9 @@ bool PositionControl::_updateSuccessful() } } - // There has to be a valid output thrust setpoint otherwise there was no + // There has to be a valid output accleration and thrust setpoint otherwise there was no // setpoint-state pair for each axis that can get controlled + valid = valid && PX4_ISFINITE(_acc_sp(0)) && PX4_ISFINITE(_acc_sp(1)) && PX4_ISFINITE(_acc_sp(2)); valid = valid && PX4_ISFINITE(_thr_sp(0)) && PX4_ISFINITE(_thr_sp(1)) && PX4_ISFINITE(_thr_sp(2)); return valid; } diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp index 607ba65094..2cfab32ee4 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp @@ -189,6 +189,7 @@ private: void _positionControl(); ///< Position proportional control void _velocityControl(const float dt); ///< Velocity PID control + void _accelerationControl(); ///< Acceleration setpoint processing // Gains matrix::Vector3f _gain_pos_p; ///< Position control proportional gain diff --git a/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp b/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp index 4645241680..9a42f198e3 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp @@ -188,17 +188,40 @@ TEST_F(PositionControlBasicTest, VelocityLimit) EXPECT_LE(abs(_output_setpoint.vz), 1.f); } -TEST_F(PositionControlBasicTest, ThrustLimit) +TEST_F(PositionControlBasicTest, PositionControlMaxThrustLimit) { _input_setpoint.x = 10.f; _input_setpoint.y = 10.f; _input_setpoint.z = -10.f; - EXPECT_TRUE(runController()); - EXPECT_FLOAT_EQ(_attitude.thrust_body[0], 0.f); - EXPECT_FLOAT_EQ(_attitude.thrust_body[1], 0.f); - EXPECT_LT(_attitude.thrust_body[2], -.1f); - EXPECT_GE(_attitude.thrust_body[2], -0.9f); + runController(); + Vector3f thrust(_output_setpoint.thrust); + EXPECT_FLOAT_EQ(thrust(0), 0.f); + EXPECT_FLOAT_EQ(thrust(1), 0.f); + EXPECT_FLOAT_EQ(thrust(2), -0.9f); + + EXPECT_EQ(_attitude.thrust_body[0], 0.f); + EXPECT_EQ(_attitude.thrust_body[1], 0.f); + EXPECT_FLOAT_EQ(_attitude.thrust_body[2], -0.9f); + + EXPECT_FLOAT_EQ(_attitude.roll_body, 0.f); + EXPECT_FLOAT_EQ(_attitude.pitch_body, 0.f); +} + +TEST_F(PositionControlBasicTest, PositionControlMinThrustLimit) +{ + _input_setpoint.x = 10.f; + _input_setpoint.y = 0.f; + _input_setpoint.z = 10.f; + + runController(); + Vector3f thrust(_output_setpoint.thrust); + EXPECT_FLOAT_EQ(thrust.length(), 0.1f); + + EXPECT_FLOAT_EQ(_attitude.thrust_body[2], -0.1f); + + EXPECT_FLOAT_EQ(_attitude.roll_body, 0.f); + EXPECT_FLOAT_EQ(_attitude.pitch_body, -1.f); } TEST_F(PositionControlBasicTest, FailsafeInput) @@ -216,6 +239,17 @@ TEST_F(PositionControlBasicTest, FailsafeInput) EXPECT_LE(_attitude.thrust_body[2], -.1f); } +TEST_F(PositionControlBasicTest, IdleThrustInput) +{ + // High downwards acceleration to make sure there's no thrust + Vector3f(0.f, 0.f, 100.f).copyTo(_input_setpoint.acceleration); + + EXPECT_TRUE(runController()); + EXPECT_FLOAT_EQ(_output_setpoint.thrust[0], 0.f); + EXPECT_FLOAT_EQ(_output_setpoint.thrust[1], 0.f); + EXPECT_FLOAT_EQ(_output_setpoint.thrust[2], -.1f); +} + TEST_F(PositionControlBasicTest, InputCombinationsPosition) { _input_setpoint.x = .1f; @@ -259,16 +293,16 @@ TEST_F(PositionControlBasicTest, SetpointValiditySimple) EXPECT_FALSE(runController()); _input_setpoint.y = .2f; EXPECT_FALSE(runController()); - _input_setpoint.thrust[2] = .3f; + _input_setpoint.acceleration[2] = .3f; EXPECT_TRUE(runController()); } TEST_F(PositionControlBasicTest, SetpointValidityAllCombinations) { - // This test runs any position, velocity, thrust setpoint combination and checks if it gets accepted or rejected correctly - float *const setpoint_loop_access_map[] = {&_input_setpoint.x, &_input_setpoint.vx, &_input_setpoint.thrust[0], - &_input_setpoint.y, &_input_setpoint.vy, &_input_setpoint.thrust[1], - &_input_setpoint.z, &_input_setpoint.vz, &_input_setpoint.thrust[2] + // This test runs any combination of set and unset (NAN) setpoints and checks if it gets accepted or rejected correctly + float *const setpoint_loop_access_map[] = {&_input_setpoint.x, &_input_setpoint.vx, &_input_setpoint.acceleration[0], + &_input_setpoint.y, &_input_setpoint.vy, &_input_setpoint.acceleration[1], + &_input_setpoint.z, &_input_setpoint.vz, &_input_setpoint.acceleration[2] }; for (int combination = 0; combination < 512; combination++) { @@ -289,16 +323,17 @@ TEST_F(PositionControlBasicTest, SetpointValidityAllCombinations) const bool has_xy_pairs = (combination & 7) == ((combination >> 3) & 7); const bool expected_result = has_x_setpoint && has_y_setpoint && has_z_setpoint && has_xy_pairs; - EXPECT_EQ(runController(), expected_result) << "combination " << combination - << std::endl << "input" << std::endl - << "position " << _input_setpoint.x << ", " << _input_setpoint.y << ", " << _input_setpoint.z << std::endl - << "velocity " << _input_setpoint.vx << ", " << _input_setpoint.vy << ", " << _input_setpoint.vz << std::endl - << "thrust " << _input_setpoint.thrust[0] << ", " << _input_setpoint.thrust[1] << ", " << _input_setpoint.thrust[2] - << std::endl << "output" << std::endl - << "position " << _output_setpoint.x << ", " << _output_setpoint.y << ", " << _output_setpoint.z << std::endl - << "velocity " << _output_setpoint.vx << ", " << _output_setpoint.vy << ", " << _output_setpoint.vz << std::endl - << "thrust " << _output_setpoint.thrust[0] << ", " << _output_setpoint.thrust[1] << ", " << _output_setpoint.thrust[2] - << std::endl; + EXPECT_EQ(runController(), expected_result) << "combination " << combination << std::endl + << "input" << std::endl + << "position " << _input_setpoint.x << ", " << _input_setpoint.y << ", " << _input_setpoint.z << std::endl + << "velocity " << _input_setpoint.vx << ", " << _input_setpoint.vy << ", " << _input_setpoint.vz << std::endl + << "acceleration " << _input_setpoint.acceleration[0] << ", " + << _input_setpoint.acceleration[1] << ", " << _input_setpoint.acceleration[2] << std::endl + << "output" << std::endl + << "position " << _output_setpoint.x << ", " << _output_setpoint.y << ", " << _output_setpoint.z << std::endl + << "velocity " << _output_setpoint.vx << ", " << _output_setpoint.vy << ", " << _output_setpoint.vz << std::endl + << "acceleration " << _output_setpoint.acceleration[0] << ", " + << _output_setpoint.acceleration[1] << ", " << _output_setpoint.acceleration[2] << std::endl; } } diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index f5e850381b..a654b81a41 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -637,7 +637,7 @@ MulticopterPositionControl::Run() if (not_taken_off || flying_but_ground_contact) { // we are not flying yet and need to avoid any corrections reset_setpoint_to_nan(setpoint); - setpoint.thrust[0] = setpoint.thrust[1] = setpoint.thrust[2] = 0.0f; + Vector3f(0.f, 0.f, 100.f).copyTo(setpoint.acceleration); // High downwards acceleration to make sure there's no thrust // set yaw-sp to current yaw // TODO: we need a clean way to disable yaw control setpoint.yaw = _states.yaw; @@ -684,7 +684,7 @@ MulticopterPositionControl::Run() // Inform FlightTask about the input and output of the velocity controller // This is used to properly initialize the velocity setpoint when onpening the position loop (position unlock) _flight_tasks.updateVelocityControllerIO(Vector3f(local_pos_sp.vx, local_pos_sp.vy, local_pos_sp.vz), - Vector3f(local_pos_sp.thrust)); + Vector3f(local_pos_sp.acceleration)); vehicle_attitude_setpoint_s attitude_setpoint{}; attitude_setpoint.timestamp = time_stamp_now; @@ -949,7 +949,7 @@ MulticopterPositionControl::failsafe(vehicle_local_position_setpoint_s &setpoint if (PX4_ISFINITE(_states.velocity(0)) && PX4_ISFINITE(_states.velocity(1))) { // don't move along xy - setpoint.vx = setpoint.vy = 0.0f; + setpoint.vx = setpoint.vy = 0.f; if (warn) { PX4_WARN("Failsafe: stop and wait"); @@ -957,7 +957,7 @@ MulticopterPositionControl::failsafe(vehicle_local_position_setpoint_s &setpoint } else { // descend with land speed since we can't stop - setpoint.thrust[0] = setpoint.thrust[1] = 0.f; + setpoint.acceleration[0] = setpoint.acceleration[1] = 0.f; setpoint.vz = _param_mpc_land_speed.get(); if (warn) { @@ -974,7 +974,7 @@ MulticopterPositionControl::failsafe(vehicle_local_position_setpoint_s &setpoint } else { // emergency descend with a bit below hover thrust setpoint.vz = NAN; - setpoint.thrust[2] = _param_mpc_thr_hover.get() * .8f; + setpoint.acceleration[2] = .3f; if (warn) { PX4_WARN("Failsafe: blind descend");