Skip to content

Commit

Permalink
add dynamic mass correction, support automatic bias estimation
Browse files Browse the repository at this point in the history 
*mass estimation is enabled, correction is off by default
*bias estimation param type change to string: "auto" default
*add parameters to launch file
  • Loading branch information
@ajshank
ajshank committed Jan 6, 2021
1 parent 378c86b commit 09813f0
Show file tree
Hide file tree
Showing 3 changed files with 82 additions and 17 deletions.
32 changes: 17 additions & 15 deletions freyja_controller.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,26 +9,26 @@

<!-- allowed autopilots: "asctec" or "arducopter" -->
<!-- must provide vicon_topic -->
<arg name="autopilot" default="arducopter"/>
<arg name="autopilot" default="arducopter"/>
<arg name="vicon_topic"/>
<!-- use "device:baudrate" for apm, "device" for asctec -->
<arg name="comm_port" default="/dev/ttyUSB0:57600"/>
<arg name="total_mass" default="0.95"/>
<arg name="thrust_scaler" default="200.0"/>
<arg name="comm_port" default="/dev/ttyUSB0:57600"/>
<arg name="total_mass" default="0.95"/>
<arg name="thrust_scaler" default="200.0"/>

<!-- true if providing your own trajectory source -->
<arg name="use_external_trajectory" default="false"/>
<arg name="use_velctrl_only" default="false"/>
<arg name="bias_compensation" default="false" />
<arg name="bias_compensation" default="auto" />
<arg name="start_rosbag" default="false"/>


<!-- state source can be: "apm", "asctec", "vicon" or "onboard_camera" -->
<!-- implemented filters: "median", "gauss" and "lwma" -->
<node name="state_manager" pkg="state_manager" type="state_manager_node">
<param name="state_source" type="string" value="vicon" />
<param name="filter_type" type="string" value="median" />
<param name="vicon_topic" type="string" value="$(arg vicon_topic)" />
<param name="state_source" type="string" value="vicon" />
<param name="filter_type" type="string" value="median" />
<param name="vicon_topic" type="string" value="$(arg vicon_topic)" />
</node>

<!-- an example trajectory node for reference -->
Expand All @@ -41,17 +41,19 @@
<!-- control node (see velctrl flag above) -->
<node name="lqg_controller" pkg="lqg_control" type="lqg_control_node"
output="screen" if="$(eval not use_velctrl_only)">
<param name="controller_rate" type="int" value="45" />
<param name="total_mass" type="double" value="$(arg total_mass)"/>
<param name="use_stricter_gains" type="bool" value="false" />
<param name="estimator_rate" type="int" value="70" />
<param name="bias_compensation" type="bool" value="$(arg bias_compensation)" />
<param name="controller_rate" type="int" value="45" />
<param name="total_mass" type="double" value="$(arg total_mass)"/>
<param name="use_stricter_gains" type="bool" value="false" />
<param name="estimator_rate" type="int" value="10" />
<param name="bias_compensation" type="string" value="$(arg bias_compensation)" />
<param name="mass_estimation" type="bool" value="true" />
<param name="mass_correction" type="bool" value="false" />
</node>

<node name="lqr_vel_controller" pkg="lqr_control" type="lqr_vel_ctrl_node"
if="$(eval use_velctrl_only)">
<param name="controller_rate" type="int" value="30" />
<param name="total_mass" type="double" value="$(arg total_mass)" />
<param name="controller_rate" type="int" value="30" />
<param name="total_mass" type="double" value="$(arg total_mass)" />
</node>

<!-- autopilot communication node -->
Expand Down
9 changes: 9 additions & 0 deletions lqg_controller/include/lqr_control_bias.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@
#include <ros/ros.h>
#include <geometry_msgs/TransformStamped.h>
#include <std_srvs/SetBool.h>
#include <std_msgs/Float32.h>

#include <freyja_msgs/CurrentState.h>
#include <freyja_msgs/CtrlCommand.h>
Expand Down Expand Up @@ -70,6 +71,10 @@ class LQRController
Eigen::Matrix<double, 3, 1> f_biases_;
std::thread bias_handler_thread_;

/*Dynamic mass estimation and compensation */
bool enable_dyn_mass_correction_;
bool enable_dyn_mass_estimation_;

public:
LQRController( BiasEstimator & );
void initLqrSystem();
Expand All @@ -91,4 +96,8 @@ class LQRController

/* helper function to calculate yaw error */
inline double calcYawError( const double&, const double& ) __attribute__((always_inline));

/* estimate actual mass in flight */
void estimateMass( const Eigen::Matrix<double, 4, 1> &, ros::Time & );
ros::Publisher est_mass_pub_;
};
58 changes: 56 additions & 2 deletions lqg_controller/src/lqr_control_bias.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,6 +44,7 @@ LQRController::LQRController(BiasEstimator &b) : nh_(),
( "/rpyt_command", 1, true );
controller_debug_pub_ = nh_.advertise <freyja_msgs::ControllerDebug>
( "/controller_debug", 1, true );
est_mass_pub_ = nh_.advertise<std_msgs::Float32>( "/freyja_estimated_mass", 1, true );

/* Timer to run the LQR controller perdiodically */
float controller_period = 1.0/controller_rate_;
Expand All @@ -56,9 +57,25 @@ LQRController::LQRController(BiasEstimator &b) : nh_(),
have_reference_update_ = false;

/* Bias compensation parameters */
bool _bcomp = true;
priv_nh_.param( "bias_compensation", bias_compensation_req_, _bcomp );
std::string _bcomp = "auto";
priv_nh_.param( "bias_compensation", _bcomp, _bcomp );
if( _bcomp == "on" )
bias_compensation_req_ = true; // always on (be careful!!)
else if( _bcomp == "auto" || _bcomp == "off" )
bias_compensation_req_ = false; // off, or on by service call

f_biases_ << 0.0, 0.0, 0.0;


/* Mass estimation */
enable_dyn_mass_correction_ = false;
priv_nh_.param( "mass_correction", enable_dyn_mass_correction_, bool(false) );
priv_nh_.param( "mass_estimation", enable_dyn_mass_estimation_, bool(true) );
if( enable_dyn_mass_correction_ )
{
enable_dyn_mass_estimation_ = true;
ROS_WARN( "LQR: Mass correction active at init! This is discouraged." );
}
}

void LQRController::initLqrSystem()
Expand Down Expand Up @@ -240,6 +257,43 @@ void LQRController::computeFeedback( const ros::TimerEvent &event )
/* Tell bias estimator about new control input */
if( bias_compensation_req_ )
bias_est_.setControlInput( control_input );

/* Update the total flying mass if requested */
if( enable_dyn_mass_estimation_ )
estimateMass( control_input, tnow );
}

void LQRController::estimateMass( const Eigen::Matrix<double, 4, 1> &c, ros::Time &t )
{
/* Experimental module that estimates the true mass of the system in air.
It uses the provided mass and observes the deviation from expected output
of the controller - and attributes *all* of that error to an incorrect
mass parameter. This is recommended only if mass may be off by ~200-300g.
Errors larger than that can induce major second-order oscillations, and are
usually better addressed elsewhere in the architecture (or get a better scale).
*/
static float prev_estimated_mass = total_mass_;
static std_msgs::Float32 estmass;
static ros::Time t_last = ros::Time::now();

if( (t-t_last).toSec() < 3.0 )
return;

float ctrl_effort = c(2) + 9.81;
float estimated_mass = total_mass_*(9.81 - ctrl_effort)/9.81;
// basic low-pass filter to prevent wild jitter
estimated_mass = (prev_estimated_mass + estimated_mass)/2.0;

estmass.data = estimated_mass;
est_mass_pub_.publish( estmass );

/* if correction is allowed- clip between min and max */
if( enable_dyn_mass_correction_ )
total_mass_ = std::min( 2.0f, std::max( 0.8f, estimated_mass ) );

// book-keeping
t_last = t;
prev_estimated_mass = estimated_mass;
}

int main( int argc, char** argv )
Expand Down

0 comments on commit 09813f0

Please sign in to comment.