dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Revise GotoControl

This commit is contained in:
Matthias Grob 2023-11-22 16:54:05 +01:00
parent 14b8afe972
commit 439d6c61e0
3 changed files with 93 additions and 106 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

84
src/modules/mc_pos_control/GotoControl/GotoControl.cpp
View File

@ -69,12 +69,12 @@ void GotoControl::update(const float dt, const matrix::Vector3f &position, const
const Vector3f feedforward_velocity{}; const Vector3f feedforward_velocity{};
const bool force_zero_velocity_setpoint = false; const bool force_zero_velocity_setpoint = false;
PositionSmoothing::PositionSmoothingSetpoints out_setpoints; PositionSmoothing::PositionSmoothingSetpoints out_setpoints;
_position_smoother.generateSetpoints(position, position_setpoint, feedforward_velocity, dt, _position_smoothing.generateSetpoints(position, position_setpoint, feedforward_velocity, dt,
force_zero_velocity_setpoint, out_setpoints); force_zero_velocity_setpoint, out_setpoints);
_position_smoother.getCurrentPosition().copyTo(trajectory_setpoint.position); _position_smoothing.getCurrentPosition().copyTo(trajectory_setpoint.position);
_position_smoother.getCurrentVelocity().copyTo(trajectory_setpoint.velocity); _position_smoothing.getCurrentVelocity().copyTo(trajectory_setpoint.velocity);
_position_smoother.getCurrentAcceleration().copyTo(trajectory_setpoint.acceleration); _position_smoothing.getCurrentAcceleration().copyTo(trajectory_setpoint.acceleration);
out_setpoints.jerk.copyTo(trajectory_setpoint.jerk); out_setpoints.jerk.copyTo(trajectory_setpoint.jerk);
if (goto_setpoint.flag_control_heading && PX4_ISFINITE(goto_setpoint.heading) && PX4_ISFINITE(heading)) { if (goto_setpoint.flag_control_heading && PX4_ISFINITE(goto_setpoint.heading) && PX4_ISFINITE(heading)) {
@ -83,10 +83,10 @@ void GotoControl::update(const float dt, const matrix::Vector3f &position, const
} }
setHeadingSmootherLimits(goto_setpoint); setHeadingSmootherLimits(goto_setpoint);
_heading_smoother.update(goto_setpoint.heading, dt); _heading_smoothing.update(goto_setpoint.heading, dt);
trajectory_setpoint.yaw = _heading_smoother.getSmoothedHeading(); trajectory_setpoint.yaw = _heading_smoothing.getSmoothedHeading();
trajectory_setpoint.yawspeed = _heading_smoother.getSmoothedHeadingRate(); trajectory_setpoint.yawspeed = _heading_smoothing.getSmoothedHeadingRate();
_controlling_heading = true; _controlling_heading = true;
@ -114,7 +114,7 @@ void GotoControl::resetPositionSmoother(const matrix::Vector3f &position)
const Vector3f initial_acceleration{}; const Vector3f initial_acceleration{};
const Vector3f initial_velocity{}; const Vector3f initial_velocity{};
_position_smoother.reset(initial_acceleration, initial_velocity, position); _position_smoothing.reset(initial_acceleration, initial_velocity, position);
_need_smoother_reset = false; _need_smoother_reset = false;
} }
@ -128,41 +128,41 @@ void GotoControl::resetHeadingSmoother(const float heading)
} }
const float initial_heading_rate{0.f}; const float initial_heading_rate{0.f};
_heading_smoother.reset(heading, initial_heading_rate); _heading_smoothing.reset(heading, initial_heading_rate);
} }
void GotoControl::setPositionSmootherLimits(const goto_setpoint_s &goto_setpoint) void GotoControl::setPositionSmootherLimits(const goto_setpoint_s &goto_setpoint)
{ {
// constrain horizontal velocity // constrain horizontal velocity
float max_horizontal_speed = _vehicle_constraints.max_horizontal_speed; float max_horizontal_speed = _goto_constraints.max_horizontal_speed;
float max_horizontal_accel = _vehicle_constraints.max_horizontal_accel; float max_horizontal_accel = _goto_constraints.max_horizontal_accel;
if (goto_setpoint.flag_set_max_horizontal_speed if (goto_setpoint.flag_set_max_horizontal_speed
&& PX4_ISFINITE(goto_setpoint.max_horizontal_speed)) { && PX4_ISFINITE(goto_setpoint.max_horizontal_speed)) {
max_horizontal_speed = math::constrain(goto_setpoint.max_horizontal_speed, 0.f, max_horizontal_speed = math::constrain(goto_setpoint.max_horizontal_speed, 0.f,
_vehicle_constraints.max_horizontal_speed); _goto_constraints.max_horizontal_speed);
// linearly scale horizontal acceleration limit with horizontal speed limit to maintain smoothing dynamic // linearly scale horizontal acceleration limit with horizontal speed limit to maintain smoothing dynamic
// only limit acceleration once within velocity constraints // only limit acceleration once within velocity constraints
if (_position_smoother.getCurrentVelocityXY().norm() <= max_horizontal_speed) { if (_position_smoothing.getCurrentVelocityXY().norm() <= max_horizontal_speed) {
const float speed_scale = max_horizontal_speed / _vehicle_constraints.max_horizontal_speed; const float speed_scale = max_horizontal_speed / _goto_constraints.max_horizontal_speed;
max_horizontal_accel = math::constrain(_vehicle_constraints.max_horizontal_accel * speed_scale, max_horizontal_accel = math::constrain(_goto_constraints.max_horizontal_accel * speed_scale,
VelocitySmoothing::kMinAccel, 0.f,
_vehicle_constraints.max_horizontal_accel); _goto_constraints.max_horizontal_accel);
} }
} }
_position_smoother.setCruiseSpeed(max_horizontal_speed); _position_smoothing.setCruiseSpeed(max_horizontal_speed);
_position_smoother.setMaxVelocityXY(max_horizontal_speed); _position_smoothing.setMaxVelocityXY(max_horizontal_speed);
_position_smoother.setMaxAccelerationXY(max_horizontal_accel); _position_smoothing.setMaxAccelerationXY(max_horizontal_accel);
// constrain vertical velocity // constrain vertical velocity
const bool pos_setpoint_is_below_smooth_pos = goto_setpoint.position[2] - _position_smoother.getCurrentPositionZ() > const bool pos_setpoint_is_below_smooth_pos = goto_setpoint.position[2] - _position_smoothing.getCurrentPositionZ() >
0.f; 0.f;
const float vehicle_max_vertical_speed = (pos_setpoint_is_below_smooth_pos) ? _vehicle_constraints.max_down_speed : const float vehicle_max_vertical_speed = (pos_setpoint_is_below_smooth_pos) ? _goto_constraints.max_down_speed :
_vehicle_constraints.max_up_speed; _goto_constraints.max_up_speed;
const float vehicle_max_vertical_accel = (pos_setpoint_is_below_smooth_pos) ? _vehicle_constraints.max_down_accel : const float vehicle_max_vertical_accel = (pos_setpoint_is_below_smooth_pos) ? _goto_constraints.max_down_accel :
_vehicle_constraints.max_up_accel; _goto_constraints.max_up_accel;
float max_vertical_speed = vehicle_max_vertical_speed; float max_vertical_speed = vehicle_max_vertical_speed;
float max_vertical_accel = vehicle_max_vertical_accel; float max_vertical_accel = vehicle_max_vertical_accel;
@ -173,38 +173,38 @@ void GotoControl::setPositionSmootherLimits(const goto_setpoint_s &goto_setpoint
// linearly scale vertical acceleration limit with vertical speed limit to maintain smoothing dynamic // linearly scale vertical acceleration limit with vertical speed limit to maintain smoothing dynamic
// only limit acceleration once within velocity constraints // only limit acceleration once within velocity constraints
if (fabsf(_position_smoother.getCurrentVelocityZ()) <= max_vertical_speed) { if (fabsf(_position_smoothing.getCurrentVelocityZ()) <= max_vertical_speed) {
const float speed_scale = max_vertical_speed / vehicle_max_vertical_speed; const float speed_scale = max_vertical_speed / vehicle_max_vertical_speed;
max_vertical_accel = math::constrain(vehicle_max_vertical_accel * speed_scale, VelocitySmoothing::kMinAccel, max_vertical_accel = math::constrain(vehicle_max_vertical_accel * speed_scale, 0.f,
vehicle_max_vertical_accel); vehicle_max_vertical_accel);
} }
} }
_position_smoother.setMaxVelocityZ(max_vertical_speed); _position_smoothing.setMaxVelocityZ(max_vertical_speed);
_position_smoother.setMaxAccelerationZ(max_vertical_accel); _position_smoothing.setMaxAccelerationZ(max_vertical_accel);
_position_smoother.setMaxJerkXY(_vehicle_constraints.max_jerk); _position_smoothing.setMaxJerkXY(_goto_constraints.max_jerk);
_position_smoother.setMaxJerkZ(_vehicle_constraints.max_jerk); _position_smoothing.setMaxJerkZ(_goto_constraints.max_jerk);
} }
void GotoControl::setHeadingSmootherLimits(const goto_setpoint_s &goto_setpoint) void GotoControl::setHeadingSmootherLimits(const goto_setpoint_s &goto_setpoint)
{ {
float max_heading_rate = _vehicle_constraints.max_heading_rate; float max_heading_rate = _goto_constraints.max_heading_rate;
float max_heading_accel = _vehicle_constraints.max_heading_accel; float max_heading_accel = _goto_constraints.max_heading_accel;
if (goto_setpoint.flag_set_max_heading_rate && PX4_ISFINITE(goto_setpoint.max_heading_rate)) { if (goto_setpoint.flag_set_max_heading_rate && PX4_ISFINITE(goto_setpoint.max_heading_rate)) {
max_heading_rate = math::constrain(goto_setpoint.max_heading_rate, HeadingSmoother::kMinHeadingRate, max_heading_rate = math::constrain(goto_setpoint.max_heading_rate, 0.f,
_vehicle_constraints.max_heading_rate); _goto_constraints.max_heading_rate);
// linearly scale heading acceleration limit with heading rate limit to maintain smoothing dynamic // linearly scale heading acceleration limit with heading rate limit to maintain smoothing dynamic
// only limit acceleration once within velocity constraints // only limit acceleration once within velocity constraints
if (fabsf(_heading_smoother.getSmoothedHeadingRate()) <= max_heading_rate) { if (fabsf(_heading_smoothing.getSmoothedHeadingRate()) <= max_heading_rate) {
const float rate_scale = max_heading_rate / _vehicle_constraints.max_heading_rate; const float rate_scale = max_heading_rate / _goto_constraints.max_heading_rate;
max_heading_accel = math::constrain(_vehicle_constraints.max_heading_accel * rate_scale, max_heading_accel = math::constrain(_goto_constraints.max_heading_accel * rate_scale,
HeadingSmoother::kMinHeadingAccel, _vehicle_constraints.max_heading_accel); 0.f, _goto_constraints.max_heading_accel);
} }
} }
_heading_smoother.setMaxHeadingRate(max_heading_rate); _heading_smoothing.setMaxHeadingRate(max_heading_rate);
_heading_smoother.setMaxHeadingAccel(max_heading_accel); _heading_smoothing.setMaxHeadingAccel(max_heading_accel);
} }

111
src/modules/mc_pos_control/GotoControl/GotoControl.hpp
View File

@ -37,15 +37,15 @@
* A class which smooths position and heading references from "go-to" setpoints * A class which smooths position and heading references from "go-to" setpoints
* for planar multicopters. * for planar multicopters.
* *
* Be sure to set constraints with setVehicleConstraints() before calling the update() method for the first time, otherwise * Be sure to set constraints with setGotoConstraints() before calling the update() method for the first time
* the default motion will be VERY slow.
*/ */
#pragma once #pragma once
#include <matrix/matrix/math.hpp> #include <lib/motion_planning/HeadingSmoothing.hpp>
#include <lib/motion_planning/PositionSmoothing.hpp> #include <lib/motion_planning/PositionSmoothing.hpp>
#include <lib/motion_planning/HeadingSmoother.hpp> #include <mathlib/math/Limits.hpp>
#include <matrix/matrix/math.hpp>
#include <uORB/topics/goto_setpoint.h> #include <uORB/topics/goto_setpoint.h>
#include <uORB/topics/trajectory_setpoint.h> #include <uORB/topics/trajectory_setpoint.h>
@ -55,34 +55,43 @@ public:
GotoControl() = default; GotoControl() = default;
~GotoControl() = default; ~GotoControl() = default;
/** /** @brief struct containing maximum vehicle translational and rotational constraints */
* @brief struct containing maximum vehicle translational and rotational constraints struct GotoConstraints {
* float max_horizontal_speed; // [m/s]
*/ float max_down_speed; // [m/s]
struct VehicleConstraints { float max_up_speed; // [m/s]
float max_horizontal_speed = kMinSpeed; // [m/s] float max_horizontal_accel; // [m/s^2]
float max_down_speed = kMinSpeed; // [m/s] float max_down_accel; // [m/s^2]
float max_up_speed = kMinSpeed; // [m/s] float max_up_accel; // [m/s^2]
float max_horizontal_accel = VelocitySmoothing::kMinAccel; // [m/s^2] float max_jerk; // [m/s^3]
float max_down_accel = VelocitySmoothing::kMinAccel; // [m/s^2] float max_heading_rate; // [rad/s]
float max_up_accel = VelocitySmoothing::kMinAccel; // [m/s^2] float max_heading_accel; // [rad/s^2]
float max_jerk = VelocitySmoothing::kMinJerk; // [m/s^3]
float max_heading_rate = HeadingSmoother::kMinHeadingRate; // [rad/s]
float max_heading_accel = HeadingSmoother::kMinHeadingAccel; // [rad/s^2]
}; };
/** /**
* @brief updates the smoothers with the current setpoints and outputs the "trajectory setpoint" for lower level * @brief sets the maximum vehicle translational and rotational constraints. note these can be more conservatively
* loops to track. * overriden (e.g. slowed down) via the speed scalers in the go-to setpoint.
* *
* @param[in] dt [s] time since last control update * @param vehicle_constraints Struct containing desired vehicle constraints
* @param[in] position [m] (NED) local vehicle position
* @param[in] heading [rad] (from North) vehicle heading
* @param[in] goto_setpoint struct containing current go-to setpoints
* @param[out] trajectory_setpoint struct containing trajectory (tracking) setpoints
*/ */
void update(const float dt, const matrix::Vector3f &position, const float heading, void setGotoConstraints(const GotoConstraints &vehicle_constraints)
const goto_setpoint_s &goto_setpoint, trajectory_setpoint_s &trajectory_setpoint); {
_goto_constraints.max_horizontal_speed = math::max(0.f, vehicle_constraints.max_horizontal_speed);
_goto_constraints.max_down_speed = math::max(0.f, vehicle_constraints.max_down_speed);
_goto_constraints.max_up_speed = math::max(0.f, vehicle_constraints.max_up_speed);
_goto_constraints.max_horizontal_accel = math::max(0.f,
vehicle_constraints.max_horizontal_accel);
_goto_constraints.max_down_accel = math::max(0.f, vehicle_constraints.max_down_accel);
_goto_constraints.max_up_accel = math::max(0.f, vehicle_constraints.max_up_accel);
_goto_constraints.max_jerk = math::max(0.f, vehicle_constraints.max_jerk);
_goto_constraints.max_heading_rate = math::max(0.f,
vehicle_constraints.max_heading_rate);
_goto_constraints.max_heading_accel = math::max(0.f,
vehicle_constraints.max_heading_accel);
}
/** @param error [m] position smoother's maximum allowed horizontal position error at which trajectory integration halts */
void setMaxAllowedHorizontalPositionError(const float error) { _position_smoothing.setMaxAllowedHorizontalError(error); }
/** /**
* @brief resets the position smoother at the current position with zero velocity and acceleration. * @brief resets the position smoother at the current position with zero velocity and acceleration.
@ -99,42 +108,19 @@ public:
void resetHeadingSmoother(const float heading); void resetHeadingSmoother(const float heading);
/** /**
* @brief sets the maximum vehicle translational and rotational constraints. note these can be more conservatively * @brief updates the smoothers with the current setpoints and outputs the "trajectory setpoint" for lower level
* overriden (e.g. slowed down) via the speed scalers in the go-to setpoint. * loops to track.
* *
* @param vehicle_constraints Struct containing desired vehicle constraints * @param[in] dt [s] time since last control update
* @param[in] position [m] (NED) local vehicle position
* @param[in] heading [rad] (from North) vehicle heading
* @param[in] goto_setpoint struct containing current go-to setpoints
* @param[out] trajectory_setpoint struct containing trajectory (tracking) setpoints
*/ */
void setVehicleConstraints(const VehicleConstraints &vehicle_constraints) void update(const float dt, const matrix::Vector3f &position, const float heading,
{ const goto_setpoint_s &goto_setpoint, trajectory_setpoint_s &trajectory_setpoint);
_vehicle_constraints.max_horizontal_speed = math::max(kMinSpeed, vehicle_constraints.max_horizontal_speed);
_vehicle_constraints.max_down_speed = math::max(kMinSpeed, vehicle_constraints.max_down_speed);
_vehicle_constraints.max_up_speed = math::max(kMinSpeed, vehicle_constraints.max_up_speed);
_vehicle_constraints.max_horizontal_accel = math::max(VelocitySmoothing::kMinAccel,
vehicle_constraints.max_horizontal_accel);
_vehicle_constraints.max_down_accel = math::max(VelocitySmoothing::kMinAccel, vehicle_constraints.max_down_accel);
_vehicle_constraints.max_up_accel = math::max(VelocitySmoothing::kMinAccel, vehicle_constraints.max_up_accel);
_vehicle_constraints.max_jerk = math::max(VelocitySmoothing::kMinJerk, vehicle_constraints.max_jerk);
_vehicle_constraints.max_heading_rate = math::max(HeadingSmoother::kMinHeadingRate,
vehicle_constraints.max_heading_rate);
_vehicle_constraints.max_heading_accel = math::max(HeadingSmoother::kMinHeadingAccel,
vehicle_constraints.max_heading_accel);
}
/**
* @brief Set the position smoother's maximum allowed horizontal position error at which trajectory integration halts
*
* @param error [m] horizontal position error
*/
void setMaxAllowedHorizontalPositionError(const float error)
{
_position_smoother.setMaxAllowedHorizontalError(error);
}
private: private:
// [m/s] minimum value of the maximum translational speeds
static constexpr float kMinSpeed{0.1f};
/** /**
* @brief optionally sets dynamic translational speed limits with corresponding scale on acceleration * @brief optionally sets dynamic translational speed limits with corresponding scale on acceleration
* *
@ -149,9 +135,10 @@ private:
*/ */
void setHeadingSmootherLimits(const goto_setpoint_s &goto_setpoint); void setHeadingSmootherLimits(const goto_setpoint_s &goto_setpoint);
VehicleConstraints _vehicle_constraints{}; GotoConstraints _goto_constraints{};
PositionSmoothing _position_smoother;
HeadingSmoother _heading_smoother; PositionSmoothing _position_smoothing;
HeadingSmoothing _heading_smoothing;
// flags that the next update() requires a valid current vehicle position to reset the smoothers // flags that the next update() requires a valid current vehicle position to reset the smoothers
bool _need_smoother_reset{true}; bool _need_smoother_reset{true};

4
src/modules/mc_pos_control/MulticopterPositionControl.cpp
View File

@ -386,7 +386,7 @@ void MulticopterPositionControl::Run()
_goto_control.resetHeadingSmoother(states.yaw); _goto_control.resetHeadingSmoother(states.yaw);
} }
const GotoControl::VehicleConstraints vehicle_constraints = { const GotoControl::GotoConstraints goto_constraints = {
_param_mpc_xy_cruise.get(), _param_mpc_xy_cruise.get(),
_param_mpc_z_v_auto_dn.get(), _param_mpc_z_v_auto_dn.get(),
_param_mpc_z_v_auto_up.get(), _param_mpc_z_v_auto_up.get(),
@ -398,7 +398,7 @@ void MulticopterPositionControl::Run()
_param_mpc_yawaauto_max.get() _param_mpc_yawaauto_max.get()
}; };
_goto_control.setVehicleConstraints(vehicle_constraints); _goto_control.setGotoConstraints(goto_constraints);
_goto_control.setMaxAllowedHorizontalPositionError(_param_mpc_xy_err_max.get()); _goto_control.setMaxAllowedHorizontalPositionError(_param_mpc_xy_err_max.get());
_goto_control.update(dt, states.position, states.yaw, _goto_setpoint, _setpoint); _goto_control.update(dt, states.position, states.yaw, _goto_setpoint, _setpoint);