fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

rover: replace RX_MAX_THR_YAW_R with a correction factor RO_YAW_RATE_CORR

Browse Source
This commit is contained in:
chfriedrich98 2025-06-16 11:00:04 +02:00 committed by chfriedrich98
parent 3e8f054a1c
commit f35b92a487
16 changed files with 51 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover
View File

@ -15,7 +15,6 @@ param set-default NAV_ACC_RAD 0.5
# Differential Parameters
param set-default RD_WHEEL_TRACK 0.3
param set-default RD_MAX_THR_YAW_R 1.5
param set-default RD_TRANS_DRV_TRN 0.349066
param set-default RD_TRANS_TRN_DRV 0.174533
@ -31,6 +30,7 @@ param set-default RO_YAW_RATE_P 0.25
param set-default RO_YAW_RATE_LIM 180
param set-default RO_YAW_ACCEL_LIM 120
param set-default RO_YAW_DECEL_LIM 1000
param set-default RO_YAW_RATE_CORR 1.43
# Rover Attitude Control Parameters
param set-default RO_YAW_P 5

1
ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann
View File

@ -30,6 +30,7 @@ param set-default RO_MAX_THR_SPEED 3.1
param set-default RO_YAW_RATE_I 0.1
param set-default RO_YAW_RATE_P 1
param set-default RO_YAW_RATE_LIM 180
param set-default RO_YAW_RATE_CORR 1
# Rover Attitude Control Parameters
param set-default RO_YAW_P 3

2
ROMFS/px4fmu_common/init.d-posix/airframes/4015_gz_r1_rover_mecanum
View File

@ -15,7 +15,6 @@ param set-default NAV_ACC_RAD 0.5
# Mecanum Parameters
param set-default RM_WHEEL_TRACK 0.3
param set-default RM_MAX_THR_YAW_R 1.2
# Rover Control Parameters
param set-default RO_ACCEL_LIM 3
@ -29,6 +28,7 @@ param set-default RO_YAW_RATE_P 0.1
param set-default RO_YAW_RATE_LIM 120
param set-default RO_YAW_ACCEL_LIM 240
param set-default RO_YAW_DECEL_LIM 1000
param set-default RO_YAW_RATE_CORR 1.75
# Rover Attitude Control Parameters
param set-default RO_YAW_P 5

2
ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover
View File

@ -26,7 +26,6 @@ param set-default NAV_ACC_RAD 0.5
# Differential Parameters
param set-default RD_WHEEL_TRACK 0.3
param set-default RD_MAX_THR_YAW_R 0.7
param set-default RD_TRANS_DRV_TRN 0.785398
param set-default RD_TRANS_TRN_DRV 0.139626
@ -42,6 +41,7 @@ param set-default RO_YAW_RATE_P 0.1
param set-default RO_YAW_RATE_LIM 250
param set-default RO_YAW_ACCEL_LIM 600
param set-default RO_YAW_DECEL_LIM 600
param set-default RO_YAW_RATE_CORR 2.7
# Rover Attitude Control Parameters
param set-default RO_YAW_P 2.5

1
ROMFS/px4fmu_common/init.d/airframes/51001_axial_scx10_2_trail_honcho
View File

@ -33,6 +33,7 @@ param set-default RO_MAX_THR_SPEED 2.8
param set-default RO_YAW_RATE_I 0.1
param set-default RO_YAW_RATE_P 0.1
param set-default RO_YAW_RATE_LIM 120
param set-default RO_YAW_RATE_CORR 1
# Rover Attitude Control Parameters
param set-default RO_YAW_P 2.5

13
src/lib/rover_control/RoverControl.cpp
View File

@ -161,7 +161,8 @@ float speedControl(SlewRate<float> &speed_with_rate_limit, PID &pid_speed, const
}
float rateControl(SlewRate<float> &adjusted_yaw_rate_setpoint, PID &pid_yaw_rate, const float yaw_rate_setpoint,
const float vehicle_yaw_rate, const float max_thr_yaw_r, const float max_yaw_accel, const float max_yaw_decel,
const float vehicle_yaw_rate, const float max_thr_speed, const float yaw_rate_corr, const float max_yaw_accel,
const float max_yaw_decel,
const float wheel_track, const float dt)
{
// Apply acceleration and deceleration limit
@ -194,11 +195,11 @@ float rateControl(SlewRate<float> &adjusted_yaw_rate_setpoint, PID &pid_yaw_rate
// Transform yaw rate into speed difference
float speed_diff_normalized{0.f};
if (wheel_track > FLT_EPSILON && max_thr_yaw_r > FLT_EPSILON) { // Feedforward
const float speed_diff = adjusted_yaw_rate_setpoint.getState() * wheel_track /
2.f;
speed_diff_normalized = math::interpolate<float>(speed_diff, -max_thr_yaw_r,
max_thr_yaw_r, -1.f, 1.f);
if (wheel_track > FLT_EPSILON && max_thr_speed > FLT_EPSILON) { // Feedforward
const float speed_diff = (adjusted_yaw_rate_setpoint.getState() * wheel_track /
2.f) * yaw_rate_corr;
speed_diff_normalized = math::interpolate<float>(speed_diff, -max_thr_speed,
max_thr_speed, -1.f, 1.f);
}
// Feedback control

6
src/lib/rover_control/RoverControl.hpp
View File

@ -100,7 +100,8 @@ float speedControl(SlewRate<float> &speed_with_rate_limit, PID &pid_speed, float
* @param pid_yaw_rate Yaw rate PID (Updated by this function).
* @param yaw_rate_setpoint Yaw rate setpoint [rad/s].
* @param vehicle_yaw_rate Measured vehicle yaw rate [rad/s].
* @param max_thr_yaw_r Yaw rate turning left/right wheels at max speed in opposite directions [m/s].
* @param max_thr_speed Speed at maximum throttle [m/s].
* @param yaw_rate_corr Yaw rate correction factor to convert yaw rate to speed difference.
* @param max_yaw_accel Maximum allowed yaw acceleration [rad/s^2].
* @param max_yaw_decel Maximum allowed yaw deceleration [rad/s^2].
* @param wheel_track Distance from the center of the right wheel to the center of the left wheel [m].
@ -108,7 +109,8 @@ float speedControl(SlewRate<float> &speed_with_rate_limit, PID &pid_speed, float
* @return Normalized speed difference setpoint [-1, 1].
*/
float rateControl(SlewRate<float> &adjusted_yaw_rate_setpoint, PID &pid_yaw_rate,
float yaw_rate_setpoint, float vehicle_yaw_rate, float max_thr_yaw_r, float max_yaw_accel, float max_yaw_decel,
float yaw_rate_setpoint, float vehicle_yaw_rate, float max_thr_speed, float yaw_rate_corr, float max_yaw_accel,
float max_yaw_decel,
float wheel_track, float dt);
/**

16
src/lib/rover_control/rovercontrol_params.c
View File

@ -131,6 +131,22 @@ PARAM_DEFINE_FLOAT(RO_YAW_ACCEL_LIM, -1.f);
*/
PARAM_DEFINE_FLOAT(RO_YAW_DECEL_LIM, -1.f);
/**
* Yaw rate correction factor
*
* Multiplicative correction factor for the feedforward mapping of the yaw rate controller.
* Increase this value (x > 1) if the measured yaw rate is lower than the setpoint, decrease (0 < x < 1) otherwise.
* Note: Tuning this is particularly useful for skid-steered rovers, or rovers with misaligned wheels/steering axes
* that cause a lot of friction when turning.
*
* @min 0.01
* @max 10000
* @increment 0.01
* @decimal 2
* @group Rover Rate Control
*/
PARAM_DEFINE_FLOAT(RO_YAW_RATE_CORR, 1.f);
/**
* Proportional gain for closed loop yaw controller
*

3
src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.cpp
View File

@ -86,7 +86,8 @@ void AckermannRateControl::updateRateControl()
}
// Feed forward
steering_setpoint = atanf(_adjusted_yaw_rate_setpoint.getState() * _param_ra_wheel_base.get() / _estimated_speed);
steering_setpoint = atanf(_adjusted_yaw_rate_setpoint.getState() * _param_ra_wheel_base.get() / _estimated_speed) *
_param_ro_yaw_rate_corr.get();
// Feedback (Only when driving forwards because backwards driving is NMP and can introduce instability)
if (_estimated_speed > FLT_EPSILON) {

3
src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.hpp
View File

@ -123,6 +123,7 @@ private:
(ParamFloat<px4::params::RO_YAW_RATE_P>) _param_ro_yaw_rate_p,
(ParamFloat<px4::params::RO_YAW_RATE_I>) _param_ro_yaw_rate_i,
(ParamFloat<px4::params::RO_YAW_ACCEL_LIM>) _param_ro_yaw_accel_limit,
(ParamFloat<px4::params::RO_SPEED_TH>) _param_ro_speed_th
(ParamFloat<px4::params::RO_SPEED_TH>) _param_ro_speed_th,
(ParamFloat<px4::params::RO_YAW_RATE_CORR>) _param_ro_yaw_rate_corr
)
};

7
src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.cpp
View File

@ -78,7 +78,8 @@ void DifferentialRateControl::updateRateControl()
const float yaw_rate_setpoint = fabsf(_yaw_rate_setpoint) > _param_ro_yaw_rate_th.get() * M_DEG_TO_RAD_F ?
_yaw_rate_setpoint : 0.f;
const float speed_diff_normalized = RoverControl::rateControl(_adjusted_yaw_rate_setpoint, _pid_yaw_rate,
yaw_rate_setpoint, _vehicle_yaw_rate, _param_rd_max_thr_yaw_r.get(), _param_ro_yaw_accel_limit.get() * M_DEG_TO_RAD_F,
yaw_rate_setpoint, _vehicle_yaw_rate, _param_ro_max_thr_speed.get(), _param_ro_yaw_rate_corr.get(),
_param_ro_yaw_accel_limit.get() * M_DEG_TO_RAD_F,
_param_ro_yaw_decel_limit.get() * M_DEG_TO_RAD_F, _param_rd_wheel_track.get(), dt);
rover_steering_setpoint_s rover_steering_setpoint{};
rover_steering_setpoint.timestamp = _timestamp;
@ -103,12 +104,12 @@ bool DifferentialRateControl::runSanityChecks()
{
bool ret = true;
if ((_param_rd_wheel_track.get() < FLT_EPSILON || _param_rd_max_thr_yaw_r.get() < FLT_EPSILON)
if ((_param_rd_wheel_track.get() < FLT_EPSILON || _param_ro_max_thr_speed.get() < FLT_EPSILON)
&& _param_ro_yaw_rate_p.get() < FLT_EPSILON) {
ret = false;
events::send<float, float, float>(events::ID("differential_rate_control_conf_invalid_rate_control"), events::Log::Error,
"Invalid configuration for rate control: Neither feed forward nor feedback is setup", _param_rd_wheel_track.get(),
_param_rd_max_thr_yaw_r.get(), _param_ro_yaw_rate_p.get());
_param_ro_max_thr_speed.get(), _param_ro_yaw_rate_p.get());
}
return ret;

5
src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.hpp
View File

@ -107,11 +107,12 @@ private:
DEFINE_PARAMETERS(
(ParamFloat<px4::params::RD_WHEEL_TRACK>) _param_rd_wheel_track,
(ParamFloat<px4::params::RD_MAX_THR_YAW_R>) _param_rd_max_thr_yaw_r,
(ParamFloat<px4::params::RO_YAW_RATE_TH>) _param_ro_yaw_rate_th,
(ParamFloat<px4::params::RO_YAW_RATE_P>) _param_ro_yaw_rate_p,
(ParamFloat<px4::params::RO_YAW_RATE_I>) _param_ro_yaw_rate_i,
(ParamFloat<px4::params::RO_YAW_ACCEL_LIM>) _param_ro_yaw_accel_limit,
(ParamFloat<px4::params::RO_YAW_DECEL_LIM>) _param_ro_yaw_decel_limit
(ParamFloat<px4::params::RO_YAW_DECEL_LIM>) _param_ro_yaw_decel_limit,
(ParamFloat<px4::params::RO_YAW_RATE_CORR>) _param_ro_yaw_rate_corr,
(ParamFloat<px4::params::RO_MAX_THR_SPEED>) _param_ro_max_thr_speed
)
};

17
src/modules/rover_differential/module.yaml
View File

@ -15,23 +15,6 @@ parameters:
decimal: 3
default: 0
RD_MAX_THR_YAW_R:
description:
short: Yaw rate turning left/right wheels at max speed in opposite directions
long: |
This parameter is used to calculate the feedforward term of the closed loop yaw rate control.
The controller first calculates the required speed difference between the left and right motor to achieve the desired yaw rate.
This desired speed difference is then linearly mapped to normalized motor commands.
A good starting point is half the speed the rover drives at maximum throttle (RD_MAX_THR_SPD)).
Increase this parameter if the rover turns faster than the setpoint, and decrease if the rover turns slower.
type: float
unit: m/s
min: 0
max: 100
increment: 0.01
decimal: 2
default: 0
RD_TRANS_TRN_DRV:
description:
short: Yaw error threshhold to switch from spot turning to driving

10
src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.cpp
View File

@ -78,7 +78,8 @@ void MecanumRateControl::updateRateControl()
const float yaw_rate_setpoint = fabsf(_yaw_rate_setpoint) > _param_ro_yaw_rate_th.get() * M_DEG_TO_RAD_F ?
_yaw_rate_setpoint : 0.f;
const float speed_diff_normalized = RoverControl::rateControl(_adjusted_yaw_rate_setpoint, _pid_yaw_rate,
yaw_rate_setpoint, _vehicle_yaw_rate, _param_rm_max_thr_yaw_r.get(), _param_ro_yaw_accel_limit.get() * M_DEG_TO_RAD_F,
yaw_rate_setpoint, _vehicle_yaw_rate, _param_ro_max_thr_speed.get(), _param_ro_yaw_rate_corr.get(),
_param_ro_yaw_accel_limit.get() * M_DEG_TO_RAD_F,
_param_ro_yaw_decel_limit.get() * M_DEG_TO_RAD_F, _param_rm_wheel_track.get(), dt);
rover_steering_setpoint_s rover_steering_setpoint{};
rover_steering_setpoint.timestamp = _timestamp;
@ -110,13 +111,12 @@ bool MecanumRateControl::runSanityChecks()
}
if ((_param_rm_wheel_track.get() < FLT_EPSILON || _param_rm_max_thr_yaw_r.get() < FLT_EPSILON)
if ((_param_rm_wheel_track.get() < FLT_EPSILON || _param_ro_max_thr_speed.get() < FLT_EPSILON)
&& _param_ro_yaw_rate_p.get() < FLT_EPSILON) {
ret = false;
events::send<float, float, float>(events::ID("mecanum_rate_control_conf_invalid_rate_control"), events::Log::Error,
"Invalid configuration for rate control: Neither feed forward (RM_MAX_THR_YAW_R) nor feedback (RO_YAW_RATE_P) is setup",
_param_rm_wheel_track.get(),
_param_rm_max_thr_yaw_r.get(), _param_ro_yaw_rate_p.get());
"Invalid configuration for rate control: Neither feed forward (RO_MAX_THR_SPEED) nor feedback (RO_YAW_RATE_P) is setup",
_param_rm_wheel_track.get(), _param_ro_max_thr_speed.get(), _param_ro_yaw_rate_p.get());
}
return ret;

5
src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.hpp
View File

@ -107,12 +107,13 @@ private:
DEFINE_PARAMETERS(
(ParamFloat<px4::params::RM_WHEEL_TRACK>) _param_rm_wheel_track,
(ParamFloat<px4::params::RM_MAX_THR_YAW_R>) _param_rm_max_thr_yaw_r,
(ParamFloat<px4::params::RO_MAX_THR_SPEED>) _param_ro_max_thr_speed,
(ParamFloat<px4::params::RO_YAW_RATE_LIM>) _param_ro_yaw_rate_limit,
(ParamFloat<px4::params::RO_YAW_RATE_TH>) _param_ro_yaw_rate_th,
(ParamFloat<px4::params::RO_YAW_RATE_P>) _param_ro_yaw_rate_p,
(ParamFloat<px4::params::RO_YAW_RATE_I>) _param_ro_yaw_rate_i,
(ParamFloat<px4::params::RO_YAW_ACCEL_LIM>) _param_ro_yaw_accel_limit,
(ParamFloat<px4::params::RO_YAW_DECEL_LIM>) _param_ro_yaw_decel_limit
(ParamFloat<px4::params::RO_YAW_DECEL_LIM>) _param_ro_yaw_decel_limit,
(ParamFloat<px4::params::RO_YAW_RATE_CORR>) _param_ro_yaw_rate_corr
)
};

17
src/modules/rover_mecanum/module.yaml
View File

@ -15,23 +15,6 @@ parameters:
decimal: 3
default: 0
RM_MAX_THR_YAW_R:
description:
short: Yaw rate turning left/right wheels at max speed in opposite directions
long: |
This parameter is used to calculate the feedforward term of the closed loop yaw rate control.
The controller first calculates the required speed difference between the left and right motor to achieve the desired yaw rate.
This desired speed difference is then linearly mapped to normalized motor commands.
A good starting point is half the speed the rover drives at maximum throttle (RD_MAX_THR_SPD)).
Increase this parameter if the rover turns faster than the setpoint, and decrease if the rover turns slower.
type: float
unit: m/s
min: 0
max: 100
increment: 0.01
decimal: 2
default: 0
RM_COURSE_CTL_TH:
description:
short: Threshold to update course control in manual position mode