Module/collective tracking control

Introduction

This module/algorithm (paper with the details at https://arxiv.org/abs/1810.00182) allows a team of aircraft (they can fly at different speeds) to track a (possibly moving) target. In particular, the centroid of the team converges to the target as it is shown in the following screenshots.

At t=90 secs, the aircraft are just orbiting around a waypoint before the algorithm starts. At t=420 secs the centroid almost converge to the target's position. The other screenshots show the centroid of the formation tracking the moving target. The velocities of the aircraft were between 10 and 16 m/s. The target was moving between 2 and 4m/s.

Custom messages

The module needs all-to-all communication (including the target). The following messages need to be included in your messages.xml.

These messages go in the telemetry section of messages.xml. The CTC_CONTROL message is just for telemetry porpuses, if you want it, do not forget to add the corresponding entry in the telemtry file.

    <message name="CTC" id="37">
      <field name="table" type="int16[]"/>
    </message>

    <message name="CTC_INFO_FROM_TARGET" id="40">
      <field name="px" type="float"/>
      <field name="py" type="float"/>
      <field name="vx" type="float"/>
      <field name="vy" type="float"/>
    </message>

    <message name="CTC_CONTROL" id="211">
        <field name="v_centroid_x" type="float" unit="m"/>
	<field name="v_centroid_y" type="float" unit="m"/>
	<field name="target_vx" type="float" unit="m/s"/>
	<field name="target_vy" type="float" unit="m/s"/>
	<field name="target_px" type="float" unit="m"/>
	<field name="target_py" type="float" unit="m"/>
	<field name="ref_px" type="float" unit="m"/>
	<field name="ref_py" type="float" unit="m"/>
    </message>

    <message name="CTC_INFO_TO_NEI" id="255">
        <field name="vx" type="float"/>
        <field name="vy" type="float"/>
        <field name="px" type="float"/>
        <field name="py" type="float"/>
    </message>

These messages go in the datalink section of messages.xml. They are necessary to init the algorithm from the ground control station.

    <message name="CTC_REG_TABLE" id="160" link="forwarded">
        <field name="ac_id" type="uint8"/>
        <field name="nei_id" type="uint8"/>
    </message>

    <message name="CTC_CLEAN_TABLE" id="161" link="forwarded">
        <field name="ac_id" type="uint8"/>
    </message>

Usage

Add this module in the target's flightplan

  <modules>
    <module name="collective_tracking_control_target"/>
  </modules>

Add this module in the flightplan of each aircraft (no the target)

  <modules>
    <module name="collective_tracking_control"/>
  </modules>

You need to define the neighboring relations of the aircraft. To do so, after you power up all the autopilots, you need to set an internal table by sending a series of commands with the help of the Python script ./PAPARAZZI_HOME/sw/ground_segment/python/collective_tracking_control/ctc_init.py . You must pass two arguments, the first one is an integer with the ID of the target, the second one a .txt file with the IDs of the rest of the aircraft.

The target will be broadcasting its position and velocity all the time. However, the rest of the aircraft will run the algorithm only if we tell them to do so. Hence, you must call the algorithm from the corresponding navigation block, i.e.,

<block name="Collective tracking control">
  <call fun="collective_tracking_control()"/>
</block>

Tuning the gains, Python simulation

The algorithm needs some parameters and gains to be set. The following Python simulation considers realistic orders of magnitude for distances and ground speeds. So you can try the gains and see the expected behavior of the algorithm before trying it in Paparazzi.

You can find the Python simulation at ./PAPARAZZI_HOME/sw/ground_segment/python/collective_tracking_control/ctc_simulation.py

WARNING

This algorithm commands roll angles for the aircraft and NO waypoints. If one aircraft stops receiving information from the other aircraft, then it will not update its roll angle and it might continue flying straight. Be sure to set safety measurements in your flight plan, for example:

<exceptions>
  <exception cond="dist2_to_home > (max_dist_from_home*max_dist_from_home)" deroute="STAND_BY"/>
</exceptions>