Module/rl obstacle avoidance

Modules

ArduIMU

Module/ADC generic

Module/Aeroprobe OTF

Module/Airspeed ETS

Module/AOA adc

Module/Bosch BMP085

Module/collective tracking control

Module/FlightBenchmark

Module/Freescale MPL3115A2

Module/GPS UBlox UCenter

Module/guidance vector field

Module/Honeywell HIH-4030

Module/Hygrosens TEMOD-I2C-R1

Module/infrared

Module/IST DigiPicco

Module/LM75

Module/Mag HMC58xx

Module/Meas Spec MS5611 I2C

Module/Melexis MLX90614

Module/rl obstacle avoidance

Module/Rotorcraft cam

Module/Sensirion SHT25

Module/Sensirion SHT75

Module/Servo switch

Module/System monitor

Module/TI TMP102

Module/TronSens HTM B71

Module/VTI SCP1000 I2C

Module/VTI SCP1000 SPI

Modules

Modules list

MultiUAV

Openlog

Pan Tilt Camera

SDLogger SPI Direct

Sensors/AMSYS

Introduction

This module is a proof-of-concept for a new method for obstacle detection and avoidance on quadrotors. One that does not require the addition of any sensors, but relies solely on measurements from the accelerometer and rotor controllers. The detection of obstacles is based on the principle that the airflow around a quadrotor changes when the quadrotor is flying near a surface. A well-known example of this is the ground effect, an increase in lift force close to a ground surface. Similarly, a change in dynamics occurs when a quadrotor is flying close to a wall or ceiling. The proposed method uses a reinforcement learning controller to detect obstacles based on these measurements, and take action to lead the quadrotor back to safety.

Note: This is an initial proof-of-concept, limited to the detection of obstacles underneath a quadrotor.

The module has been demonstrated on the Parrot Bebop 1 and Parrot Bebop 2 quadrotors to be able to successfully detect and avoid obstacles underneath. It is expected that the method can be extended to the detection of obstacles above a quadrotor as well. Extension to surfaces on the same level as the quadrotor, e.g. walls, is expected to require some significant improvements to the algorithm.

The work that this module is based on can be found here: http://resolver.tudelft.nl/uuid:1ce67e4b-1e63-4e9f-8644-360642a11be6

How does the algorithm work?

The detection of obstacles is based on a simplified quadrotor model and the estimation of an external force. In the current proof-of-concept, only the external force in the body z-axis is used for the detection of obstacles, but similar estimators can be derived for the external force in the body x and y-axis. This estimated external force in the body z-axis is used for the detection of obstacles underneath.

${\frac {F_{ext,z}}{m}}=\underbrace {{\hat {\dot {w}}}+pv-qu} _{\text{from IMU}}+\sum _{i}{\frac {k_{i}}{m}}\underbrace {\omega _{i}^{2}} _{\text{from motors}}-F_{drag,z}/m{\text{, with }}F_{{\text{drag,}}z}/m={\begin{cases}-k_{D,z}w^{2}&{\text{if }}w>0\\k_{D,z}w^{2}&{\text{otherwise}}\end{cases}}$

With:

• ${\textstyle {\frac {F_{ext,z}}{m}}}$, the estimated external force in z-direction (mass independent)
• ${\textstyle {\hat {\dot {w}}}}$, the acceleration in the body z-axis, as provided by the onboard accelerometer
• ${\textstyle p}$, the body roll rate
• ${\textstyle v}$, the speed in body y-axis
• ${\textstyle q}$, the body pitch rate
• ${\textstyle u}$, the speed in body x-axis
• ${\textstyle {\frac {k_{i}}{m}}}$, the propellor gains, estimated at the start of every flight
• ${\textstyle \omega _{i}}$, the rotor speed in rad/s
• ${\textstyle k_{D,z}}$, the drag coefficient iin body z-axis, estiamted during a system identification experiment
• ${\textstyle w}$, the speed in body z-axis

Setup

Airframe file

Add the following line to the airframe file to include the module.

<module name="rl_obstacle_avoidance"/>

To improve the performance it is recommended that the drag force is accounted for in the estimation of the external forces. In the module the drag force is assumed to be of the form:

$|F_{drag,z}|=k_{D,z}w^{2}$

<section name="RL_OBSTACLE_AVOID" prefix="RL_OBSTACLE_AVOID_">
    <define name="k_D_z" value="0.27106224"/>
</section>

The drag coefficients in body x-, y-, and z-axis can then be estimated in a system identification experiment. Some known coefficients can be found in the table below.

	k_D_x	k_D_y	k_D_z
Parrot Bebop 1 (with bumpers)	?	?	0.271
Parrot Bebop 2 (without bumpers)	?	?	0.373

Flight plan

For this module to work, the following additions to the flight plan need to be made.

Insert in the exceptions section of the flightplan

<exception cond="rl_obstacle_avoidance_hover()" deroute="Interrupt hover"/>
<exception cond="rl_obstacle_avoidance_save()" deroute="Interrupt save"/>

Insert before engines are started

<block name="Start Obstacle Avoidance initialization">
    <call_once fun="NavSetWaypointHere(WP_EPISODE_START)"/>
    <call_once fun="rl_obstacle_avoidance_turn_on()"/>
    <call_once fun="rl_obstacle_avoidance_request_new_policy()" />
    <attitude pitch="0" roll="0" throttle="0" until="set_estimated_accel_bias" vmode="throttle"/>
</block>

Insert after the takeoff procedure

<block name="Estimate prop gains">
    <call_once fun="NavSetWaypointHere(WP_STDBY)"/>
    <stay wp="STDBY" until="stage_time>1"/>
    <call_once fun="rl_obstacle_avoidance_flight_status(25)"/>
    <stay wp="STDBY" until="set_estimated_k_over_m"/>
</block>

Insert after the regular flight plan blocks

<block name="Interrupt hover">
    <call_once fun="NavSetWaypointHere(WP_EPISODE_TARGET)"/>
    <call_once fun="rl_obstacle_avoidance_flight_status(60)" />
    <stay wp="EPISODE_TARGET" vmode="climb" climb="0.0"  until="stage_time>0.5"/>
    <call_once fun="rl_obstacle_avoidance_hover_concluded()" />
    <deroute block="Run episode"/>
</block>
<block name="Interrupt save">
    <call_once fun="rl_obstacle_avoidance_flight_status(70)" />
    <attitude pitch="0" roll="0" throttle="0.8" until="stage_time>0.5" vmode="throttle"/>
    <call_once fun="rl_obstacle_avoidance_save_concluded()" />
    <deroute block="End episode"/>
</block>

Flight status

Flight status >= 50 and <60

Messages

The following messages must be included in your custom ./conf/messages.xml (if they are not present in your PaparazziLink version yet)

<message name="RL_OBSTACLE_AVOIDANCE" id="75">
    <field name="timestep" type="int32" unit="#"/>
    <field name="time" type="int32" unit="ms"/>
    <field name="body_acceleration_u" type="float" unit="m/s2"/>
    <field name="body_acceleration_v" type="float" unit="m/s2"/>
    <field name="body_acceleration_w" type="float" unit="m/s2"/>
    <field name="body_speed_u" type="float" unit="m/s"/>
    <field name="body_speed_v" type="float" unit="m/s"/>
    <field name="body_speed_w" type="float" unit="m/s"/>
    <field name="GPS_vertical_speed" type="float" unit="m/s"/>
    <field name="enu_position_x" type="float" unit="m"/>
    <field name="enu_position_y" type="float" unit="m"/>
    <field name="enu_position_z" type="float" unit="m"/>
    <field name="body_rate_p" type="float" unit="rad/s" alt_unit="deg/s"/>
    <field name="body_rate_q" type="float" unit="rad/s" alt_unit="deg/s"/>
    <field name="body_rate_r" type="float" unit="rad/s" alt_unit="deg/s"/>
    <field name="body_attitude_phi" type="float" unit="rad" alt_unit="deg"/>
    <field name="body_attitude_theta" type="float" unit="rad" alt_unit="deg"/>
    <field name="body_attitude_psi" type="float" unit="rad" alt_unit="deg"/>
    <field name="motor_speed_nw" type="uint16" unit="rpm"/>
    <field name="motor_speed_ne" type="uint16" unit="rpm"/>
    <field name="motor_speed_se" type="uint16" unit="rpm"/>
    <field name="motor_speed_sw" type="uint16" unit="rpm"/>
    <field name="F_ext_onboard_est" type="float" />
    <field name="F_ext_onboard_est_rolling" type="float" />
</message>
<message name="RL_TRAINING_DOWN" id="76">
    <field name="request" type="char[]" />
    <field name="parameters" type="char[]"/>
</message>

Todo

• Store policy onboard the quadrotor, or in the settings file
• Allow for running the module without the python application (using a previously determined policy)
• Turn off logging when not in training