Commit c93687a4 authored by Paul Beuchat's avatar Paul Beuchat
Browse files

Added nested LQR Angle Rate controller, tested to be workin

parent 505a0e44
......@@ -114,10 +114,11 @@
// LQR_MODE_ANGLE LQR controller based on the state vector:
// [position,velocity]
//
#define LQR_MODE_MOTOR 1
#define LQR_MODE_ACTUATOR 2
#define LQR_MODE_RATE 3 // (DEFAULT)
#define LQR_MODE_ANGLE 4
#define LQR_MODE_MOTOR 1
#define LQR_MODE_ACTUATOR 2
#define LQR_MODE_RATE 3 // (DEFAULT)
#define LQR_MODE_ANGLE 4
#define LQR_MODE_ANGLE_RATE_NESTED 5
// These constants define the method used for estimating the Inertial
......@@ -179,6 +180,9 @@ float gravity_force_quarter;
// VARIABLES FOR THE CONTROLLER
// Frequency at which the controller is running
float vicon_frequency;
// Frequency at which the controller is running
float control_frequency;
......@@ -200,21 +204,36 @@ std::vector<float> gainMatrixMotor3 (12,0.0);
std::vector<float> gainMatrixMotor4 (12,0.0);
// The LQR Controller parameters for "LQR_MODE_ACTUATOR"
std::vector<float> gainMatrixThrust_TwelveStateVector (12,0.0);
std::vector<float> gainMatrixRollTorque (12,0.0);
std::vector<float> gainMatrixPitchTorque (12,0.0);
std::vector<float> gainMatrixYawTorque (12,0.0);
std::vector<float> gainMatrixThrust_TwelveStateVector (12,0.0);
std::vector<float> gainMatrixRollTorque (12,0.0);
std::vector<float> gainMatrixPitchTorque (12,0.0);
std::vector<float> gainMatrixYawTorque (12,0.0);
// The LQR Controller parameters for "LQR_MODE_RATE"
std::vector<float> gainMatrixThrust_NineStateVector = { 0.00, 0.00, 0.25, 0.00, 0.00, 0.14, 0.00, 0.00, 0.00};
std::vector<float> gainMatrixRollRate = { 0.00,-1.72, 0.00, 0.00,-1.34, 0.00, 5.12, 0.00, 0.00};
std::vector<float> gainMatrixPitchRate = { 1.72, 0.00, 0.00, 1.34, 0.00, 0.00, 0.00, 5.12, 0.00};
std::vector<float> gainMatrixYawRate = { 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 2.84};
std::vector<float> gainMatrixThrust_NineStateVector (9,0.0);
std::vector<float> gainMatrixRollRate (9,0.0);
std::vector<float> gainMatrixPitchRate (9,0.0);
std::vector<float> gainMatrixYawRate (9,0.0);
// The LQR Controller parameters for "LQR_MODE_ANGLE"
std::vector<float> gainMatrixThrust_SixStateVector = { 0.00, 0.00, 0.31, 0.00, 0.00, 0.14};
std::vector<float> gainMatrixRollAngle = { 0.00,-0.20, 0.00, 0.00,-0.20, 0.00};
std::vector<float> gainMatrixPitchAngle = { 0.20, 0.00, 0.00, 0.20, 0.00, 0.00};
std::vector<float> gainMatrixThrust_SixStateVector (6,0.0);
std::vector<float> gainMatrixRollAngle (6,0.0);
std::vector<float> gainMatrixPitchAngle (6,0.0);
// The LQR Controller parameters for "LQR_MODE_ANGLE_RATE_NESTED"
std::vector<float> gainMatrixThrust_SixStateVector_50Hz (6,0.0);
std::vector<float> gainMatrixRollAngle_50Hz (6,0.0);
std::vector<float> gainMatrixPitchAngle_50Hz (6,0.0);
std::vector<float> gainMatrixRollRate_Nested (3,0.0);
std::vector<float> gainMatrixPitchRate_Nested (3,0.0);
std::vector<float> gainMatrixYawRate_Nested (3,0.0);
int lqr_angleRateNested_counter = 4;
float lqr_angleRateNested_prev_thrustAdjustment = 0.0;
float lqr_angleRateNested_prev_rollAngle = 0.0;
float lqr_angleRateNested_prev_pitchAngle = 0.0;
float lqr_angleRateNested_prev_yawAngle = 0.0;
// The 16-bit command limits
......@@ -365,11 +384,12 @@ ros::Subscriber xyz_yaw_to_follow_subscriber;
bool calculateControlOutput(Controller::Request &request, Controller::Response &response);
// > The various functions that implement an LQR controller
void calculateControlOutput_viaLQR( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
void calculateControlOutput_viaLQRforMotors( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
void calculateControlOutput_viaLQRforActuators(float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
void calculateControlOutput_viaLQRforRates( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
void calculateControlOutput_viaLQRforAngles( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
void calculateControlOutput_viaLQR( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
void calculateControlOutput_viaLQRforMotors( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
void calculateControlOutput_viaLQRforActuators( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
void calculateControlOutput_viaLQRforRates( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
void calculateControlOutput_viaLQRforAngles( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
void calculateControlOutput_viaLQRforAnglesRatesNested( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
// ESTIMATOR COMPUTATIONS
void performEstimatorUpdate_forStateInterial(Controller::Request &request);
......
......@@ -2,6 +2,7 @@
mass : 30
# Frequency of the controller, in hertz
vicon_frequency : 200
control_frequency : 200
# Quadratic motor regression equation (a0, a1, a2)
......@@ -37,7 +38,8 @@ shouldDisplayDebugInfo : false
# commands actuators of total force and bodz torques
# 3 - LQR controller based on the state vector: [position,velocity,angles]
# 4 - LQR controller based on the state vector: [position,velocity]
lqr_controller_mode : 3
# 5 - LQR Nester angle and rate controller
lqr_controller_mode : 5
# A flag for which estimator to use, defined as:
......@@ -50,7 +52,7 @@ lqr_controller_mode : 3
# each of (x,y,z,roll,pitch,yaw)
# 3 - Quad-rotor Model Based Method
# Uses the model of the quad-rotor and the previous inputs
estimator_method : 1
estimator_method : 2
# The LQR Controller parameters for "mode = 1"
......@@ -72,11 +74,20 @@ gainMatrixPitchRate : [ 1.80, 0.00, 0.00, 1.38, 0.00, 0.00, 0.0
gainMatrixYawRate : [ 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 2.30]
# The LQR Controller parameters for "mode = 4"
gainMatrixThrust_SixStateVector : [ 0.00, 0.00, 0.31, 0.00, 0.00, 0.14]
gainMatrixThrust_SixStateVector : [ 0.00, 0.00, 0.98, 0.00, 0.00, 0.25]
gainMatrixRollAngle : [ 0.00,-0.20, 0.00, 0.00,-0.20, 0.00]
gainMatrixPitchAngle : [ 0.20, 0.00, 0.00, 0.20, 0.00, 0.00]
# The LQR Controller parameters for "mode = 5"
gainMatrixThrust_SixStateVector_50Hz: [ 0.00, 0.00, 0.82, 0.00, 0.00, 0.22]
gainMatrixRollAngle_50Hz : [ 0.00,-0.31, 0.00, 0.00,-0.25, 0.00]
gainMatrixPitchAngle_50Hz : [ 0.31, 0.00, 0.00, 0.25, 0.00, 0.00]
gainMatrixRollRate_Nested : [ 4.00, 0.00, 0.00]
gainMatrixPitchRate_Nested : [ 0.00, 4.00, 0.00]
gainMatrixYawRate_Nested : [ 0.00, 0.00, 2.30]
# The max and minimum thrust for a 16-bit command
command_sixteenbit_min : 1000
command_sixteenbit_max : 60000
......
......@@ -428,6 +428,15 @@ void calculateControlOutput_viaLQR(float stateErrorBody[12], Controller::Request
break;
}
// LQR controller based on the state vector:
// [position,velocity,angles]
case LQR_MODE_ANGLE_RATE_NESTED:
{
// Call the function that performs the control computations for this mode
calculateControlOutput_viaLQRforAnglesRatesNested(stateErrorBody,request,response);
break;
}
default:
{
// Display that the "lqr_controller_mode" was not recognised
......@@ -808,7 +817,7 @@ void calculateControlOutput_viaLQRforAngles(float stateErrorBody[12], Controller
float thrustAdjustment = 0;
// Perform the "-Kx" LQR computation for the rates and thrust:
for(int i = 0; i < 9; ++i)
for(int i = 0; i < 6; ++i)
{
// BODY FRAME Y CONTROLLER
rollAngle_forResponse -= gainMatrixRollAngle[i] * stateErrorBody[i];
......@@ -829,6 +838,7 @@ void calculateControlOutput_viaLQRforAngles(float stateErrorBody[12], Controller
// > NOTE: remember that the thrust is commanded per motor, so you sohuld
// consider whether the "thrustAdjustment" computed by your
// controller needed to be divided by 4 or not.
thrustAdjustment = thrustAdjustment / 4.0;
// > NOTE: the "gravity_force_quarter" value was already divided by 4 when
// it was loaded/processes from the .yaml file.
response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
......@@ -847,6 +857,116 @@ void calculateControlOutput_viaLQRforAngles(float stateErrorBody[12], Controller
void calculateControlOutput_viaLQRforAnglesRatesNested( float stateErrorBody[12], Controller::Request &request, Controller::Response &response)
{
// PERFORM THE NESTED "u=Kx" LQR CONTROLLER COMPUTATION
// Increment the counter variable
lqr_angleRateNested_counter++;
if (lqr_angleRateNested_counter > 4)
{
//ROS_INFO("Outer called");
// Reset the counter to 1
lqr_angleRateNested_counter = 1;
// PERFORM THE OUTER "u=Kx" LQR CONTROLLER COMPUTATION
// Reset the class variable to zero for the following:
// > body frame roll angle,
// > body frame pitch angle,
// > body frame yaw angle,
// > total thrust adjustment.
// These will be requested from the "inner-loop" LQR controller below
lqr_angleRateNested_prev_rollAngle = 0;
lqr_angleRateNested_prev_pitchAngle = 0;
lqr_angleRateNested_prev_thrustAdjustment = 0;
// Perform the "-Kx" LQR computation for the rates and thrust:
for(int i = 0; i < 6; ++i)
{
// BODY FRAME Y CONTROLLER
lqr_angleRateNested_prev_rollAngle -= gainMatrixRollAngle_50Hz[i] * stateErrorBody[i];
// BODY FRAME X CONTROLLER
lqr_angleRateNested_prev_pitchAngle -= gainMatrixPitchAngle_50Hz[i] * stateErrorBody[i];
// > ALITUDE CONTROLLER (i.e., z-controller):
lqr_angleRateNested_prev_thrustAdjustment -= gainMatrixThrust_SixStateVector_50Hz[i] * stateErrorBody[i];
}
// BODY FRAME Z CONTROLLER
//lqr_angleRateNested_prev_yawAngle = setpoint[3];
lqr_angleRateNested_prev_yawAngle = stateErrorBody[8];
}
//ROS_INFO("Inner called");
// PERFORM THE INNER "u=Kx" LQR CONTROLLER COMPUTATION
// Instantiate the local variables for the following:
// > body frame roll rate,
// > body frame pitch rate,
// > body frame yaw rate,
// These will be requested from the Crazyflie's on-baord "inner-loop" controller
float rollRate_forResponse = 0;
float pitchRate_forResponse = 0;
float yawRate_forResponse = 0;
// Create the angle error to use for the inner controller
float temp_stateAngleError[3] = {
stateErrorBody[6] - lqr_angleRateNested_prev_rollAngle,
stateErrorBody[7] - lqr_angleRateNested_prev_pitchAngle,
lqr_angleRateNested_prev_yawAngle
};
// Perform the "-Kx" LQR computation for the rates and thrust:
for(int i = 0; i < 4; ++i)
{
// BODY FRAME Y CONTROLLER
rollRate_forResponse -= gainMatrixRollRate_Nested[i] * temp_stateAngleError[i];
// BODY FRAME X CONTROLLER
pitchRate_forResponse -= gainMatrixPitchRate_Nested[i] * temp_stateAngleError[i];
// BODY FRAME Z CONTROLLER
yawRate_forResponse -= gainMatrixYawRate_Nested[i] * temp_stateAngleError[i];
}
// UPDATE THE "RETURN" THE VARIABLE NAMED "response"
// Put the computed rates and thrust into the "response" variable
// > For roll, pitch, and yaw:
response.controlOutput.roll = rollRate_forResponse;
response.controlOutput.pitch = pitchRate_forResponse;
response.controlOutput.yaw = yawRate_forResponse;
// > For the thrust adjustment we must add the feed-forward thrust to counter-act gravity.
// > NOTE: remember that the thrust is commanded per motor, so you sohuld
// consider whether the "thrustAdjustment" computed by your
// controller needed to be divided by 4 or not.
float thrustAdjustment = lqr_angleRateNested_prev_thrustAdjustment / 4.0;
// > NOTE: the "gravity_force_quarter" value was already divided by 4 when
// it was loaded/processes from the .yaml file.
response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
response.controlOutput.motorCmd2 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
// Specify that this controller is a rate controller
// response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTOR;
response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
// response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
ROS_INFO_STREAM("thrust =" << lqr_angleRateNested_prev_thrustAdjustment );
ROS_INFO_STREAM("rollrate =" << response.controlOutput.roll );
ROS_INFO_STREAM("pitchrate =" << response.controlOutput.pitch );
ROS_INFO_STREAM("yawrate =" << response.controlOutput.yaw );
}
......@@ -1171,6 +1291,10 @@ void fetchYamlParameters(ros::NodeHandle& nodeHandle)
// Display one of the YAML parameters to debug if it is working correctly
//ROS_INFO_STREAM("DEBUGGING: mass leaded from loacl file = " << cf_mass );
// > The frequency at which the "computeControlOutput" is being called, as determined
// by the frequency at which the Vicon system provides position and attitude data
vicon_frequency = getParameterFloat(nodeHandle_for_demoController, "vicon_frequency");
// > The frequency at which the "computeControlOutput" is being called, as determined
// by the frequency at which the Vicon system provides position and attitude data
control_frequency = getParameterFloat(nodeHandle_for_demoController, "control_frequency");
......@@ -1228,15 +1352,25 @@ void fetchYamlParameters(ros::NodeHandle& nodeHandle)
getParameterFloatVector(nodeHandle_for_demoController, "gainMatrixYawTorque", gainMatrixYawTorque, 12);
// The LQR Controller parameters for "LQR_MODE_RATE"
getParameterFloatVector(nodeHandle_for_demoController, "gainMatrixThrust_NineStateVector", gainMatrixThrust_NineStateVector, 9);
getParameterFloatVector(nodeHandle_for_demoController, "gainMatrixRollRate", gainMatrixRollRate, 9);
getParameterFloatVector(nodeHandle_for_demoController, "gainMatrixPitchRate", gainMatrixPitchRate, 9);
getParameterFloatVector(nodeHandle_for_demoController, "gainMatrixYawRate", gainMatrixYawRate, 9);
getParameterFloatVector(nodeHandle_for_demoController, "gainMatrixThrust_NineStateVector", gainMatrixThrust_NineStateVector, 9);
// The LQR Controller parameters for "LQR_MODE_ANGLE"
getParameterFloatVector(nodeHandle_for_demoController, "gainMatrixThrust_SixStateVector", gainMatrixThrust_SixStateVector, 6);
getParameterFloatVector(nodeHandle_for_demoController, "gainMatrixRollAngle", gainMatrixRollAngle, 6);
getParameterFloatVector(nodeHandle_for_demoController, "gainMatrixPitchAngle", gainMatrixPitchAngle, 6);
getParameterFloatVector(nodeHandle_for_demoController, "gainMatrixThrust_SixStateVector", gainMatrixThrust_SixStateVector, 6);
// The LQR Controller parameters for "LQR_MODE_ANGLE_RATE_NESTED"
getParameterFloatVector(nodeHandle_for_demoController, "gainMatrixThrust_SixStateVector_50Hz", gainMatrixThrust_SixStateVector_50Hz, 6);
getParameterFloatVector(nodeHandle_for_demoController, "gainMatrixRollAngle_50Hz", gainMatrixRollAngle_50Hz, 6);
getParameterFloatVector(nodeHandle_for_demoController, "gainMatrixPitchAngle_50Hz", gainMatrixPitchAngle_50Hz, 6);
getParameterFloatVector(nodeHandle_for_demoController, "gainMatrixRollRate_Nested", gainMatrixRollRate_Nested, 3);
getParameterFloatVector(nodeHandle_for_demoController, "gainMatrixPitchRate_Nested", gainMatrixPitchRate_Nested, 3);
getParameterFloatVector(nodeHandle_for_demoController, "gainMatrixYawRate_Nested", gainMatrixYawRate_Nested, 3);
// 16-BIT COMMAND LIMITS
......@@ -1276,7 +1410,7 @@ void processFetchedParameters()
gravity_force_quarter = cf_mass * 9.81/(1000*4);
// Set that the estimator frequency is the same as the control frequency
estimator_frequency = control_frequency;
estimator_frequency = vicon_frequency;
// Look-up which agent should be followed
if (shouldFollowAnotherAgent)
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment