mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-25 16:56:25 +08:00
PositionControl: acceleration based control strategy
This commit is contained in:
Matthias Grob
@@ -40,6 +40,7 @@
|
||||
#include <float.h>
|
||||
#include <mathlib/mathlib.h>
|
||||
#include <px4_platform_common/defines.h>
|
||||
#include <ecl/geo/geo.h>
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -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");
|
||||
|
||||
Reference in New Issue
Block a user