Paparazzi UAS
v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
|
This module implements optical flow landings in which the divergence is kept constant. More...
#include "std.h"
Go to the source code of this file.
Data Structures | |
struct | OpticalFlowLanding |
Macros | |
#define | MAX_SAMPLES_LEARNING 25000 |
#define | GUIDANCE_H_MODE_MODULE_SETTING GUIDANCE_H_MODE_MODULE |
#define | GUIDANCE_V_MODE_MODULE_SETTING GUIDANCE_V_MODE_MODULE |
Functions | |
void | guidance_h_module_init (void) |
Initialization of horizontal guidance module. More... | |
void | guidance_h_module_enter (void) |
Entering the horizontal module (user switched to module) More... | |
void | guidance_h_module_run (bool in_flight) |
Main guidance loop. More... | |
void | guidance_h_module_read_rc (void) |
Read the RC commands. More... | |
void | guidance_v_module_init (void) |
void | guidance_v_module_enter (void) |
Entering the vertical module (user switched to module) More... | |
void | guidance_v_module_run (bool in_flight) |
void | save_texton_distribution (void) |
void | load_texton_distribution (void) |
void | fit_linear_model_OF (float *targets, float **samples, uint8_t D, uint16_t count, float *parameters, float *fit_error) |
Fit a linear model from samples to target values. More... | |
void | learn_from_file (void) |
float | predict_gain (float *distribution) |
void | save_weights (void) |
void | load_weights (void) |
void | recursive_least_squares_batch (float *targets, float **samples, uint8_t D, uint16_t count, float *params, float *fit_error) |
Recursively fit a linear model from samples to target values - batch mode, possibly for initialization. More... | |
void | recursive_least_squares (float target, float *sample, uint8_t length_sample, float *params) |
Variables | |
struct OpticalFlowLanding | of_landing_ctrl |
This module implements optical flow landings in which the divergence is kept constant.
When using a fixed gain for control, the covariance between thrust and divergence is tracked, so that the drone knows when it has arrived close to the landing surface. Then, a final landing procedure is triggered. It can also be set to adaptive gain control, where the goal is to continuously gauge the distance to the landing surface. In this mode, the drone will oscillate all the way down to the surface.
de Croon, G.C.H.E. (2016). Monocular distance estimation with optical flow maneuvers and efference copies: a stability-based strategy. Bioinspiration & biomimetics, 11(1), 016004. http://iopscience.iop.org/article/10.1088/1748-3190/11/1/016004
Based on the above theory, we have also developed a new strategy for landing that consists of two phases: (1) while hovering, the drone determines the optimal gain by increasing the gain until oscillation (2) the drone starts landing while exponentially decreasing the gain over time
This strategy leads to smooth, high-performance constant divergence landings, as explained in the article: H.W. Ho, G.C.H.E. de Croon, E. van Kampen, Q.P. Chu, and M. Mulder (submitted) Adaptive Control Strategy for Constant Optical Flow Divergence Landing, https://arxiv.org/abs/1609.06767
The module also contains code for learning to map the visual appearance of the landing surface with self-supervised learning (SSL) to the corresponding control gains. This leads to smooth optical flow landings where the "height" expressed in the control gain is known and can be used to shut down the motors. This has been published in the following paper:
de Croon, Guido CHE, Christophe De Wagter, and Tobias Seidl. "Enhancing optical-flow-based control by learning visual appearance cues for flying robots." Nature Machine Intelligence 3.1 (2021): 33-41.
Finally, horizontal flow control has been added, so that the drone can hover without optitrack. In fact, this module also formed the basis for the experiments in which the attitude is estimated with only optical flow and gyro measurements (without accelerometers), published in Nature:
De Croon, G.C.H.E., Dupeyroux, J.J., De Wagter, C., Chatterjee, A., Olejnik, D. A., & Ruffier, F. (2022). Accommodating unobservability to control flight attitude with optic flow. Nature, 610(7932), 485-490.
Definition in file optical_flow_landing.h.
struct OpticalFlowLanding |
Definition at line 52 of file optical_flow_landing.h.
Data Fields | ||
---|---|---|
uint32_t | active_motion | Method for actively inducing motion. |
float | agl | agl = height from sonar (only used when using "fake" divergence) |
float | agl_lp | low-pass version of agl |
float | close_to_edge | if abs(cov_div - reference cov_div) < close_to_edge, then texton distributions and gains are stored for learning |
uint32_t | CONTROL_METHOD | type of divergence control: 0 = fixed gain, 1 = adaptive gain, 2 = exponential time landing control. 3 = learning control |
float | cov_limit | for fixed gain control, what is the cov limit triggering the landing |
uint32_t | COV_METHOD | method to calculate the covariance: between thrust and div (0) or div and div past (1) |
float | cov_set_point | for adaptive gain control, setpoint of the covariance (oscillations) |
float | d_err | difference of error for the D-gain |
uint32_t | delay_steps | number of delay steps for div past |
float | dgain | D-gain for constant divergence control. |
float | dgain_adaptive | D-gain for adaptive gain control. |
float | divergence | Divergence estimate. |
float | divergence_setpoint | setpoint for constant divergence approach |
bool | elc_oscillate | Whether or not to oscillate at beginning of elc to find optimum gain. |
float | front_div_threshold | Threshold when the front div gets above this value, it will command a horizontal stop. |
float | igain | I-gain for constant divergence control. |
float | igain_adaptive | I-gain for adaptive gain control. |
float | igain_horizontal_factor | factor multiplied with the vertical I-gain for horizontal ventral-flow-based control |
volatile bool | learn_gains | set to true if the robot needs to learn a mapping from texton distributions to the p-gain |
bool | load_weights | load the weights that were learned before |
float | lp_const | low-pass value for divergence |
float | lp_cov_div_factor | low-pass factor for the covariance of divergence in order to trigger the second landing phase in the exponential strategy. |
float | lp_factor_prediction | low-pass value for the predicted P-value |
float | nominal_thrust | nominal thrust around which the PID-control operates |
float | omega_FB | Set point for the front-back ventral flow. |
float | omega_LR | Set point for the left-right ventral flow. |
float | p_land_threshold | if during the exponential landing the gain reaches this value, the final landing procedure is triggered |
float | pgain | P-gain for constant divergence control (from divergence error to thrust) |
float | pgain_adaptive | P-gain for adaptive gain control. |
float | pgain_horizontal_factor | factor multiplied with the vertical P-gain for horizontal ventral-flow-based control |
float | pitch_trim | Pitch trim angle in degrees. |
float | previous_err | Previous divergence tracking error. |
float | ramp_duration | ramp duration in seconds for when the divergence setpoint changes |
float | reduction_factor_elc | reduction factor - after oscillation, how much to reduce the gain... |
float | roll_trim | Roll trim angle in degrees. |
bool | snapshot | if true, besides storing a texton distribution, an image will also be stored (when unstable) |
float | sum_err | integration of the error for I-gain |
float | t_transition | how many seconds the drone has to be oscillating in order to transition to the hover phase with reduced gain |
bool | use_bias | if true, a bias 1.0f will be added to the feature vector (texton distribution) - this typically leads to very large weights |
float | vel | vertical velocity as determined with sonar (only used when using "fake" divergence) |
uint32_t | VISION_METHOD | whether to use vision (1) or Optitrack / sonar (0) |
uint32_t | window_size | number of time steps in "window" used for getting the covariance |
#define GUIDANCE_H_MODE_MODULE_SETTING GUIDANCE_H_MODE_MODULE |
Definition at line 106 of file optical_flow_landing.h.
#define GUIDANCE_V_MODE_MODULE_SETTING GUIDANCE_V_MODE_MODULE |
Definition at line 109 of file optical_flow_landing.h.
#define MAX_SAMPLES_LEARNING 25000 |
Definition at line 50 of file optical_flow_landing.h.
void fit_linear_model_OF | ( | float * | targets, |
float ** | samples, | ||
uint8_t | D, | ||
uint16_t | count, | ||
float * | params, | ||
float * | fit_error | ||
) |
Fit a linear model from samples to target values.
[in] | targets | The target values |
[in] | samples | The samples / feature vectors |
[in] | D | The dimensionality of the samples |
[in] | count | The number of samples |
[out] | parameters* | Parameters of the linear fit |
[out] | fit_error* | Total error of the fit |
Definition at line 1438 of file optical_flow_landing.c.
References D, MAKE_MATRIX_PTR, MAT_MUL, MAT_SUB, n_samples, of_landing_ctrl, parameters, pprz_svd_float(), pprz_svd_solve_float(), and OpticalFlowLanding::use_bias.
Referenced by learn_from_file().
void guidance_h_module_enter | ( | void | ) |
Entering the horizontal module (user switched to module)
Entering the horizontal module (user switched to module)
Definition at line 85 of file ctrl_module_innerloop_demo.c.
void guidance_h_module_init | ( | void | ) |
Initialization of horizontal guidance module.
Definition at line 80 of file ctrl_module_innerloop_demo.c.
void guidance_h_module_read_rc | ( | void | ) |
Read the RC commands.
Definition at line 90 of file ctrl_module_innerloop_demo.c.
void guidance_h_module_run | ( | bool | in_flight | ) |
Main guidance loop.
[in] | in_flight | Whether we are in flight or not |
Definition at line 99 of file ctrl_module_innerloop_demo.c.
void guidance_v_module_enter | ( | void | ) |
Entering the vertical module (user switched to module)
Entering the vertical module (user switched to module)
Definition at line 111 of file ctrl_module_innerloop_demo.c.
void guidance_v_module_init | ( | void | ) |
Definition at line 105 of file ctrl_module_innerloop_demo.c.
void guidance_v_module_run | ( | bool | in_flight | ) |
Definition at line 656 of file optical_flow_hover.c.
void learn_from_file | ( | void | ) |
Definition at line 1274 of file optical_flow_landing.c.
References fit_linear_model_OF(), gains, load_texton_distribution(), n_read_samples, n_textons, RECURSIVE_LEARNING, recursive_least_squares_batch(), save_weights(), text_dists, and weights.
Referenced by vertical_ctrl_module_run().
void load_texton_distribution | ( | void | ) |
Definition at line 1236 of file optical_flow_landing.c.
References cov_divs_log, distribution_logger, gains, MAX_SAMPLES_LEARNING, n_read_samples, n_textons, sonar_OF, text_dists, and TEXTON_DISTRIBUTION_PATH.
Referenced by learn_from_file().
void load_weights | ( | void | ) |
Definition at line 1544 of file optical_flow_landing.c.
References n_textons, TEXTON_DISTRIBUTION_PATH, weights, and weights_file.
Referenced by vertical_ctrl_module_run().
float predict_gain | ( | float * | distribution | ) |
Definition at line 1509 of file optical_flow_landing.c.
References n_textons, of_landing_ctrl, OpticalFlowLanding::use_bias, and weights.
Referenced by recursive_least_squares(), recursive_least_squares_batch(), and vertical_ctrl_module_run().
void recursive_least_squares | ( | float | target, |
float * | sample, | ||
uint8_t | length_sample, | ||
float * | params | ||
) |
void recursive_least_squares_batch | ( | float * | targets, |
float ** | samples, | ||
uint8_t | D, | ||
uint16_t | count, | ||
float * | params, | ||
float * | fit_error | ||
) |
Recursively fit a linear model from samples to target values - batch mode, possibly for initialization.
[in] | targets | The target values |
[in] | samples | The samples / feature vectors |
[in] | D | The dimensionality of the samples |
[in] | count | The number of samples |
[out] | parameters* | Parameters of the linear fit |
[out] | fit_error* | Total error of the fit // TODO: relevant for RLS? |
Definition at line 1313 of file optical_flow_landing.c.
References D, n_textons, of_landing_ctrl, P_RLS, predict_gain(), recursive_least_squares(), OpticalFlowLanding::use_bias, and weights.
Referenced by learn_from_file().
void save_texton_distribution | ( | void | ) |
Definition at line 1209 of file optical_flow_landing.c.
References OpticalFlowLanding::agl, cov_div, distribution_logger, n_textons, of_landing_ctrl, pstate, texton_distribution, and TEXTON_DISTRIBUTION_PATH.
Referenced by vertical_ctrl_module_run().
void save_weights | ( | void | ) |
Definition at line 1526 of file optical_flow_landing.c.
References n_textons, TEXTON_DISTRIBUTION_PATH, weights, and weights_file.
Referenced by learn_from_file().
|
extern |
Definition at line 293 of file optical_flow_landing.c.
Referenced by final_landing_procedure(), fit_linear_model_OF(), guidance_v_module_enter(), PID_divergence_control(), predict_gain(), recursive_least_squares_batch(), reset_all_vars(), save_texton_distribution(), send_divergence(), set_cov_div(), update_errors(), vertical_ctrl_agl_cb(), vertical_ctrl_module_init(), and vertical_ctrl_module_run().