About | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange

Onboard datafusion and position control on Ascending Technologies quadrotors

Description: This tutorial shows how to use the datafusion and position controller provided in the asctec_hl_firmware package running on the high level processor of Ascending Technologies quadrotors.

Keywords: quadrotor, position control

Tutorial Level: INTERMEDIATE

Overview

This tutorial shows how to work with the position controller and state estimation implemented on the High Level Processor (HLP) of the AscTec AutoPilot board. If you haven't done so already, please go through the safety instructions first. Acronyms used here are explained on the stack page. First of all, some details of how the position controller and datafusion work are explained. More details can be found in Onboard IMU and Monocular Vision Based Control for MAVs in Unknown In- and Outdoor Environments. At the end step by step instructions are given how to operate the helicopter.

State estimation and the position controller are implemented with fixed point arithmetics using a Simulink SDK (therefore "ssdk"). This sdk offers 50 parameter channels (~/ssdk/p1 - ~/ssdk/p50) and 40 debug channels (published as fcu/debug) to configure and debug the system.

Important Notes

When position control is enabled on the HLP (~/position_control = "HighLevel", ~state_estimation = "HighLevel_SSDK" or "Extern") you cannot switch the motors on/off with the RC when the serial interface switch is in the on-position! In any failure, switch the flight mode switch to "acc" and the serial interface switch "off" on your remote.

All parameters can be set with dynamic_reconfigure. reconfigure_gui might look a bit confusing at this stage. Have a look at launch/ssdk_parameters.yaml where the important parameters are set and commented. Changing the parameters ~/ssdk/p1 to ~/ssdk/p50 in the reconfigure_gui does not send them automatically to the HLP. To do so, check the send box. When it gets unchecked, the parameters arrived at the HLP.

State Estimation

The position controller heavily relies on a good and fast state estimate (position and velocity). To achieve this, there are two possibilities:

Onboard State Estimation

There is a Luenberger Observer running on the HLP which estimates position, velocity and acceleration sensor bias from an arbitrary position input and from the AutoPilot's acceleration sensors at a rate of 1 kHz. To use it, set ~state_estimation to "HighLevel_SSDK". Any position source can be used that publishes either a geometry_msgs/PoseWithCovarianceStamped relative to a fixed frame or a tf transform relative to a fixed frame according to the ROS coordinate frame conventions. Currently, only yaw is taken from the orientation. When using a tf transform, always the latest transform available is used. Parent and child frame can be set dynamically with the ~ssdk/tf_ref_frame_id and ~ssdk/tf_tracked_frame_id parameters.

Parameters

The behavior of the Luenberger Observer can be set with the error feedback matrices L for x/y (~ssdk/p41, ~ssdk/p42, ~ssdk/p43) and for z (~ssdk/p35, ~ssdk/p36, ~ssdk/p37) separately. Do not modify these values manually, this could cause an oscillating observer!. Instead, use Matlab's / Octave's place command. Given the desired poles, it computes the corresponding feedback matrices:

A = diag([1 1], 1);
C = [1 0 0];
poles =  [-15,-3,-0.01]; % position, velocity, acceleration bias
L = place(A', C', poles);

The above choice of poles yielded good results with Georg Klein's visual SLAM framework PTAM used as position input. The resulting values for L are 18.01, 45.18, 0.45 for x/y and z. Those values are set by default in the launch/ssdk_parameters.yaml file.

Note: this part is experimental and we would be happy to receive feedback how these values work for your setup, or which values you used.

Sanity Checks

Before flying, check that your position input is correct and that the Luenberger Observer works correctly. Note: Except for Position Input, you need to switch on the serial interface switch, see also RC Switches.

Position Input

rosrun asctec_hl_interface plot_position_input [namespace]
# e.g. when you pushed your node to the "pelican" namespace:
rosrun asctec_hl_interface plot_position_input pelican

This plots the current position input that the observer receives (fixed frame, in NED coordinates!).

• Moving the helicopter in positive x-direction (forward) in your reference coordinate system should increase the plotted value of x.
• Moving in positive y-direction (left) should decrease the plotted value of y.
• Moving in positive z-direction (up) should decrease the plotted value of z.

Decreasing values of y and z are correct because of the NED system they are plotted in. The explained behavior must not change when you rotate the helicopter around yaw. If it does, something with your transforms is wrong. Most likely, you are sending body fixed coordinates.

Estimated Position

rosrun asctec_hl_interface plot_filter_position [namespace]

This plots the current estimated position (fixed frame, in NED coordinates!).

It should behave and look similar to the position input check above, except that it should be smoother.

Estimated Velocity

rosrun asctec_hl_interface plot_filter_velocity [namespace]

This plots the current estimated velocity (fixed frame, in NED coordinates!). Moving in positive x-direction in your reference coordinate system should yield a positive velocity, moving in positive y and z coordinates should yield negative velocities. Again, this must not change when the yaw angle changes.

After moving the helicopter, hold it still. The velocities should immediately go back to 0 (+- 0.1m/s). If you pitch/roll the heli a bit, velocities should also stay within this margin. If not, something with the estimated acceleration sensor biases might go wrong, see next paragraph.

Estimated Acceleration Sensor Biases

rosrun asctec_hl_interface plot_filter_bias [namespace]

This plots the current estimated acceleration sensor biases (body frame, in NED coordinates!). Move the helicopter a bit in x/y/z direction, such that the biases can converge. Reasonable values should not exceed +- 0.1 m/s². Also, after having converged, the biases should not follow your motion anymore - they still follow, but that should be smaller than their steady state values. If this is not the case, the poles of the biases are too fast. If you observe that the biases converge too slow, the poles of the biases are too slow. See observer parameters to change that.

External State Estimation

In case you want to use your own state estimation, you can bypass the onboard estimator by setting ~state_estimation to "Extern" and providing your state estimate (position and velocity) with a asctec_hl_comm/mav_state message. As above, position and velocity have to be w.r.t. a fixed frame and according to the ROS coordinate frame conventions. Also do the sanity checks to ensure everything is working correctly and in the right directions.

Position Controller

Mode of Operation

A position controller based on nonlinear dynamic inversion running at 1 kHZ on the HLP which accepts both velocity commands and waypoints. The inversion permanently "inverts" the system dynamic such that we can apply linear control approaches on top of that. Therefore, the error controller is just a simple PID controller for each axis. Reference model based feed forward lets the helicopter quickly reach waypoints and reduces overshoot when arriving at the waypoint. To enable this position controller, set ~/position_control to "HighLevel".

Waypoints and velocity commands can be sent with an asctec_hl_comm/mav_ctrl message and setting asctec_hl_comm/mav_ctrl/type to "position" or "velocity". When sending a waypoint, you can additionally set a maximum velocity to fly to the waypoint. If this maximum velocity is greater than the value set in ~max_velocity_* it will be limited to ~max_velocity_*. Alternatively, you can use the ActionServer interface to send waypoints while getting feedback. For an example, have a look at src/waypoint_client.cpp.

Parameters for the position controller can be found in launch/ssdk_parameters.yaml. A detailed description will follow in a tutorial.

Behavior

RC Switches

Serial interface switch "off"

Normal operation according to the flight mode switch. Additionally, the Luenberger Observer is held in reset state, i.e. position equals current position input, velocity and bias are set to zero.

Serial interface switch "on"

When the serial interface is enabled, the motors cannot be switched on/off any more, although the heli seems to be in "acc mode", see also #Important_Notes.

• flight mode switch "acc": still normal operation, as in "acc mode".

• flight mode switch "height": height control with the position controller on the HLP. Setpoint is the height where the controller was activated.

• flight mode switch "gps": full position and yaw control with the position controller on the HLP. Setpoint is the position/heading where the controller was activated.

Stick Inputs

You can still steer the helicopter similarly to the GPS mode outdoors. I.e. excitation of a stick will integrate the setpoint of the position controller in body fixed coordinates. This can be interpreted as a velocity comand. As soon as you bring the sticks to the centered position (zero velocity), the helicopter will stay at its current position. Any stick input outside a deadband zone (~10%) around the centered position will abort any waypoint command, and the heli will fly according to the stick input. Stick inputs are always in body-fixed coordinates. Velocities sent with a asctec_hl_comm/mav_ctrl velocity message behave similarly. Integration of the setpoint is limited to a distance of 1 m from the current position.

Flying

Before Flight

Switch on the helicopter, and start hl_node with the provided launch file:

roslaunch asctec_hl_interface fcu.launch

In case you need to power-cycle the helicopter, restart hl_node (see asctec_hl_interface#Baudrate).

If you want to use the ActionServer interface, run

# make sure it's in the same namespace as hl_node
rosrun asctec_hl_interface waypoint_server

Before flying, check that you have external position input (first part of sanity checks) and that everything is in the correct direction. It can easily happen that some remappings went wrong or that e.g. the node providing position died for some reason... Also decide whether to take position input from tf or from geometry_msgs/PoseWithCovarianceStamped messages and set ~ssdk/listen_on_tf or remap accordingly.

Hovering

This is the recommended way when you fly the helicopter first with this framework or when you changed settings or your setup.

1. make sure you double-checked everything mentioned before .

2. put the flight mode switch to the "acc" position and the serial interface switch to "off".
3. hold the vehicle, switch on the motors and keep the thrust stick in the "idle" position.

4. serial interface switch "on", nothing should change. (Always switch on the serial interface first before enabling the position controller with the flight-mode switch!)

5. put the thrust stick to the centered position and switch the flight-mode switch to the "gps" position. The position controller keeps the current position as setpoint. This is your last chance of testing the whole system - move the helicopter a bit to produce an error and see if it behaves correctly.
6. leave out the helicopter. We set the error integrator conservatively by default, so it might happen that the helicopter will descend slighty. It will climb to the desired height within a few seconds.

7. Send waypoints, velocity commands or fly around with stick inputs. For sending waypoint/velocity commands, have a look at src/waypoint_client.cpp.

8. After flying, put the flight mode switch to "acc", then disable the serial interface and switch off the motors.

Take Off

1. make sure you double-checked everything mentioned before, and that your setup works .

2. put the flight mode switch to the "acc" position and the serial interface switch to "off".
3. switch on the motors and keep the thrust stick in the "idle" position.

4. serial interface switch "on", nothing should change. (Always switch on the serial interface first before enabling the position controller with the flight-mode switch!)

5. flight mode switch to "gps".
6. Move the thrust stick to the centered position. Thrust of the motors should not change. (remember, the desired height has integrated negatively while the thrust stick was in the idle position, so putting the thrust stick to the center is safe now)
7. There are two possibilities to make the helicopter take off now. In both cases it takes a few seconds until something happens, because the reference model has to "work" against the integrated setpoint from above first.
   1. Send a positive z-velocity command. All other velocities have to be zero!
   2. Send a waypoint with a z-value (height) greater than the current one. Make sure that the waypoint goal contains the current x/y position and yaw. Otherwise the controller tries to reach that goal while the helicopter is still on the ground.

Landing

1. Make sure you landing space is free.
2. The recommended way of landing is to send a negative z-velocity.

Happy Flying !