From c93687a4b4dc1f4f38c8061b8837413e459a68ff Mon Sep 17 00:00:00 2001
From: Paul Beuchat <beuchatp@control.ee.ethz.ch>
Date: Fri, 20 Apr 2018 14:03:45 +0200
Subject: [PATCH] Added nested LQR Angle Rate controller, tested to be workin
---
.../include/DemoControllerService.h | 60 +++++---
.../src/d_fall_pps/param/DemoController.yaml | 17 ++-
.../d_fall_pps/src/DemoControllerService.cpp | 144 +++++++++++++++++-
3 files changed, 193 insertions(+), 28 deletions(-)
diff --git a/pps_ws/src/d_fall_pps/include/DemoControllerService.h b/pps_ws/src/d_fall_pps/include/DemoControllerService.h
index e3a9d084..b788a8b5 100644
--- a/pps_ws/src/d_fall_pps/include/DemoControllerService.h
+++ b/pps_ws/src/d_fall_pps/include/DemoControllerService.h
@@ -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);
diff --git a/pps_ws/src/d_fall_pps/param/DemoController.yaml b/pps_ws/src/d_fall_pps/param/DemoController.yaml
index dcde9927..875e84dc 100644
--- a/pps_ws/src/d_fall_pps/param/DemoController.yaml
+++ b/pps_ws/src/d_fall_pps/param/DemoController.yaml
@@ -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
diff --git a/pps_ws/src/d_fall_pps/src/DemoControllerService.cpp b/pps_ws/src/d_fall_pps/src/DemoControllerService.cpp
index 6a3ab506..5e351d37 100644
--- a/pps_ws/src/d_fall_pps/src/DemoControllerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/DemoControllerService.cpp
@@ -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)
--
GitLab