Implementation MSD19

From Control Systems Technology Group
Jump to navigation Jump to search

After a comprehensive understanding of the problem, it was divided into sub-categories to assess and work efficiently and to have a modular system as a deliverable. Following milestones were setup to keep the project on track and achieve the desired goals step by step.

  • Hardware Assembly and manual flight of the drone
  • Autonomous Flight of the drone
  • Localization of the drone
  • Path Planning of the drone
  • Setting up of visualization environment
  • Perception system for the drone
  • Integration of sub-systems
  • Testing

Getting started with Crazyflie 2.X

Crazyflie is the mini-drone which is used during this project. The positive aspect of Crazyflie is that a considerable amount of research is available online with guidelines to undergo autonomous flights. Considering the scope of this project and economical viability, Crazyflie was a good choice. Getting started section will cover the following main headings:

  • Manual Flight
  • Autonomous Flight
  • Modifications
  • Troubleshooting

A recommendation in the start would be to follow the procedure from manual to autonomous flights. Jumping directly into autonomous flight mode would is not advised as it would create a lot of ambiguity and lack of understanding of how the basic system works.

Manual Flight

This section explains in detail on how to setup a Crazyflie 2.X drone starting from hardware assembly to first manual flight. We used Windows to continue with initial setup of the software part for manual flight. However, Linux (Ubuntu 16.04) was preferred for the autonomous flight. Following additional hardware is required to setup a manual flight.

  • Bitcraze Crazyradio PA USB dongle
  • A remote control (PS4 Controller or Any USB Gaming Controller)

This link was used to get started with assembly and setting up the initial flight requirements.

NOTE: It must be noted that the Crazyflie is running on its latest firmware. The steps to flash the Crazyflie with latest firmware are discussed here.

The Crazyflie client is used for controlling the Crazyflie, flashing firmware, setting parameters and logging data. The main UI is built up of several tabs, where each tab is used for a specific functionality. We used this link to get started with first manual flight of the drone and to develop an understanding of Crazyflie Client. Assisted flight mode can be used to have a stable altitude and hovering as an initial manual testing mode.

Autonomous Flight

To get started with autonomous flight, the pre-requisites are as follows:

  • Loco Positioning System: Please refer to “Loco Positioning System Section” for more details. The link explains the whole setup of loco-positioning system.
  • Linux (Ubuntu 16.04)
  • Python Scripts: We used autnomousSequence.py file from crazyflie python library. We found this script to be the starting point of autonomous flight.

This link was also used develop a comprehensive understanding of autonomous flight. After setting up the LPS, modifications in the python script of autonomousSequence.py file were made to adjust it according to desired deliverables. The video below shows the Crazyflie hovering autonomously while changing yaw over time.


Autonomous Flight image.png

Modifications

The following modifications were made in the autonomousSequence.py file.

  • Ball Position Extraction from Simulator Data
  • Development of Path Planning Algorithm which takes the simulator data and converts it to drone position data
  • Drone Position data extraction and sending it to Simulator for visual representation

The modified_autonomousSequence.py file was developed in parallel with the project. The modifications were made as we were progressing through the project and new observations and suggestions were coming to light.

The original autonmousSequence.py file will develop the basic understanding of autonomous flight parameters and its comparison with the modified_autonomousSequence.py is advised to understand and follow through the changes and modifications.

Troubleshooting

The drone positioning parameters i.e. roll, pitch and yaw are not stable. The solution is to trim the parameters which can be seen in Firmware Configuration headline in the following link. For autonomous flights, issues like updating the anchor nodes and assigning them identification numbers may appear in Windows, but they can be solved by updating the nodes in Linux.

Localization

Mobile robots require a solution to determining a spatial relationship with its environment in order to fulfil any motion command. Localization is a backbone component of such robots as it covers the process of depicting a position (and orientation) estimation of the moving robot at a given time. Using sensors and onset information of the environment a robot will be able to create a spatial relationship with its surroundings. This relationship is in the kind of a transformation between the world coordinate system and the robot local reference. From a transformation a robot pose and attitude can computed. Additional challenge on localization comes from the fact that sensor data contains a level of error and need to be filtered/processed to account for accurate measurements.

Since a individual sensor would not be adequate for localising, the Crazyflie 2.0 offers numerous options apart from its standard hardware configuration. By default it is equipped with an IMU. For the project, several options were considered before moving on to implementation and testing their capabilities; (1) Camera and markers (Optitrack), (2) Ultra wideband positioning (LPS), (3) Optical motion detection (Flowdeck) or a combination of two. The intention of the project group was to avoid any platform specific dependencies that could lead to difficulties on integration. Thus, the focus was to either use LPS alone or combine with Flowdeck, considering limitations thereof.

The following sections describe each option and summarises the overall performance achieved.

Local Positioning System (Loco Deck)

The core of localization tool used for the drone is based on a purpose-built Loco Positioning system (LPS) that provides a complete hardware and software solution by Bitcraze. This system comes with a documentation regarding installation, configuration, and technical information in the online directory. Before starting, the following preparations are made:

  • 1 Loco positioning deck installed on Crazyflie
  • 8 Loco positioning nodes positioned within the room
  • Node fixed on stands 3D printed from makefile
  • USB power source for anchors
  • Ranging mode chosen as TWR

Overall hovering ability observed during hardware testing matches with what has been stated in company measurements as +/- 5 cm range.

Laser Range and Optical Sensor (Flow Deck 2.0)

An additional positioning component is a combination of VL53L0x Time of Flight sensor that measures ground distance and a PMW3901 optical flow sensor measures that ground planar movement. The expansion deck is plug and play. However, technical information is available.

Loco Deck + Flow Deck

Integration of sensor information occurs automatically as drone firmware can identify which types of sensors are available. Subsequently, the governing controller handles the data accordingly. To accept more sensor input, the Crazyflie utilizes an Extended Kalman Filter. The information flow from component to component at low level can be seen below;

Path Planning

Path planning refers to providing a set of poses (x,y,z and yaw) for the drone to go to at any time step. Due to the outbreak of COVID-19, our working environment changed drastically, and we cannot get access to the pitch at Tech United in the latter stage of our project. Therefore, we first design our path planning method in the pitch of the simulator, and then we transfer the planned path to fit the reference frame at the real pitch at Tech United. Two methods are designed by us and are described as below.

Algorithm 1 --- Drones moving along the soccer field long boundary

In this method, two drones are used so that the ball can be seen almost all the time. They are positioned at a specific height outside the pitch, respectively moving on the lines parallel to the sidelines of the pitch with yaw movements. The drones are supposed to move so that the ball is always around the centre of their camera views. Since the algorithm works the same for the two drones, we only show the computation of the planned path of one drone.

Assumptions

To facilitate our path planning, several assumptions are made as follows:

  • The ball is assumed to be on the ground all the time.
  • There is no obstacle to the drone on its height.
  • The ball velocity does not change in two consecutive sampling periods.

Below is the figure which depicts the method.

Refereance frame in the simulator
The illustration of the drones' positions and the pitch in the simulator

The dimension of the pitch is 18m x 12m, and the origin is at the centre of the pitch.

Algorithm

Since the drone moves on the lines parallel to the sidelines and on the same altitude, the x and z-coordinate references of the drone 20200323 SG barx^d r.png and 20200323 SG barz^d r.png are fixed. In this way, only y-coordinate and yaw angle change.In our algorithm, we set 20200323 SG barx^d r=7.png (the value is -7 for the other drone) and 20200323 SG barz^d r=2.png.

The following notations are made:

At sampling time k-1, the ball position is 20200323 SG Bk-1.png.

At sampling time k, the ball position is 20200323 SG Bk.png, and the drone position is 20200323 SG Dk.png.

Since the ball velocity does not change in two consecutive sampling periods, the predicted ball position at sampling time k+1 can be computed as

Equation 1

which we denote as 20200323 SG Bk+1 denotion.png.

The distance between 20200323 SG Bk+1 simple.png and 20200323 SG Dk simple.png is

Equation 2

The path planning method is then described as follows:

We assume there is a circle, with a radius of 20200323 SG R.png, whose centre is 20200323 SG Bk+1 simple.png.

Then we make the decision whether we rotate or move (or both) based on the relationship between dist and 20200323 SG R.png:

1. If dist < R:

  • If 20200323 SG pi4.png‎ (i.e. The ball is around the centre of the drone’s view), the drone does not have to move or rotate. Here 20200324 SG thetavalue.png.
Circle 1
In this way, 20200323 SG yk+1dr.png and 20200323 SG yawk+1dr=0.png.
  • If 20200323 SG thetaanother.png (the ball may be still in the view of the drone but not close to the centre of the view), the drone has to rotate a bit, but its position does not change. Then 20200323 SG yk+1dr.png and 20200323 SG yawk+1dr.png.
Circle 2

2. If dist > R:

  • If 20200323 SG theta04636.png (i.e. The ball far away from the drone but is around the centre of the drone’s view), the drone does not have to move or rotate. In this way, 20200323 SG yk+1dr.png and 20200323 SG yawk+1dr=0.png. The value 20200324 SG 04636arctan1over2.png is picked because we would like to keep the ball even closer to the centre of the drone's view when the ball is far away from the drone.
Circle 3
  • If 20200323 SG theta04636another.png (the ball is far away from the drone and is away from the centre of the drone’s view):
Under this circumstance, the drone can track the y-coordinate of the ball. We can set the drone reference at the sampling time k+1 as 20200323 SG bary^s k+1=ddd25.png. Ydmax.png is the maximum distance that the drone can travel per second. Since the sampling frequency in the simulator is between 16 Hz and 20 Hz, we set 20200325 SG ydmax25.png as the distance that a drone can go in a sampling period. In this scenario, the yaw reference changes as well 20200324 SG theta last.png.

Algorithm 2 -- Drone moving in a circle within soccer field

The objective of this path planning algorithm is to find the shortest path for the drone while keeping the ball in the field of view. This is to minimise the mobility of the drone under given performance constraints to ensure a stable view from the onboard camera. This is done by keeping the ball's position as reference and thereafter, using the ball velocity to predict the future ball position.

Assumptions

  • The altitude, zdrone of the drone is same throughout the duration of the game.
  • The ball is always on the ground, i.e., zball = 0.
  • The drone encounters no obstacles at altitude, zdrone.

Algorithm

Let R and D be the radius of the circle around the ball, and the distance of the drone from the ball respectively. The drone holds its current position for D < R and tracks the ball as D exceeds R as shown in Fig below.


Schematic representation of drone in a circle

Let Bk-1 = (xbk-1, ybk-1) be the position of the ball at sampling time (k - 1), Bk = (xbk; ybk) be the sampling position of the ball at sampling time k. The predicted position of the ball is therefore given by the following equation:

Equation 1
Equation 2

where t is the time taken by the drone to travel from position Bk-1 to Bk. Distance, D is then defined as follows:

Equation 3

Dref = (xdref ; ydref ) are the reference coordinates for the drone to achieve within time period, t and is denoted by the following equation:

Equation 4
Equation 5

The error between the reference position, Dref and the current position, Dk is estimated using the performance constraints of the Crazyflie and fed back to the algorithm to compute Dk+1. The error terms are estimated as follows:

Equation 6
Equation 7
Equation 8

Therefore, the reference position, Dref is computed as follows:

Equation 9
Equation 10

The drone reference position is then updated at every sampling frequency of the Visualiser that is also used to provide the information of ball position in the soccer field.

Transferring the coordinate in the simulator reference frame to that in the beacon reference frame

The dimension of the pitch in the simulator is 18mx12m, and the origin of the simulator frame is at the centre of the pitch. The dimension of the pitch at Tech United is 12m x 8m, and the origin of the beacon frame is at the centre of the beacon 0. Therefore, the planned path in the simulator cannot be applied in reality, unless we transfer the planned path in the simulator into that in the beacon frame. The transformation is depicted below.

Transfer Coordinate

Since we only consider this problem in the 2D frame, we do not transfer the z-coordinate.

Assume that the coordinate in the simulator reference frame is D=(x,y,z,yaw), and the coordinate in the beacon reference frame is Equation 4 .

Simulation

There are two goals for implementing a simulator, one is to verify and validate the concept of the path planning algorithm during the development process, and the other is to demonstrate the result of our system. To realize the goals, the scenario of using the simulator will be to replay the previous game of Tech United in the simulator and programmatically control the view angle and position, which will represent the camera view from the camera mounted on the physical drone. There will be drones flying synchronously in the virtual world and physical world, both are taking the setting points from the path planner. Considering the compatibility with previous game data, we decided to adopt the existed simulator of Tech United and modify the source code to meet our requirements. There were two functions implemented in the simulator which are described below.


Previous Game Data Extraction

To demonstrate the performance of our system, we planned to fly the drone both in the virtual world and the physical world. The virtual drone is for demonstrating that the output from our path planner is practical, and the physical drone is for demonstrating that our capability of flying the drone autonomously according to the output of the path planner.

To describe the whole scenario in detail. In the virtual world, a previous game of Tech United will be played, and the drone will fly around the field according to the output from the path planner. On the monitor, what the camera on the drone is filming will be shown. On the other hand, in the physical world, a drone will also be flying according to the command from the path planner, but there will be no game playing.

To realize the above-mentioned scenario, we need to extract the ball position from previous game data and transfer it to the path planner. In the beginning, we tried to retrieve the ball position from the data file loaded when replaying the previous game. After a few attempts, it turns out that it would be more straightforward to save the ball position into a matrix when it is used in the original program. A function is built to implement this.

Drone Camera View Control

To show what the camera is supposed to be filming in the virtual world, we need the position and yaw angle of the drone, and the pitch angle of the camera when it is mounted on the drone. With these defined inputs, we built a function which will read those data from a CSV format file, and change the displayed view in the simulator accordingly.

Perception

The fundamental goal is being able to referee the game of football. While testing for this project is a two vs. two game between two robots at the Tech United pitch facilities, the ultimate vision is that the system could be scaled and adapted for use in a real outdoor game between human beings. From the experience of previous projects, the client imposed the direction that the developed system uses drones with cameras. The main benefit of this being the relative convenience of setting up and the assumption that this dynamic capability would save cost by allowing for fewer cameras for comparable camera coverage.

The work activity relating to the camera system was divided into two workstreams to allow for concurrent work activity. Firstly, the selection and integration of the hardware, such that it would not adversely affect the flight dynamics of the drone. Owing to the small size of the Crazyflie 2 drone, and the additional safety issues with using the larger drone variant with extra load capacity, we were restricted to using a small FPS camera because of latency issues with other options, as discussed in Yussuf's section. While this option allows for a live camera feed to an external referee, this option seemingly does not allow for further processing of those images without significant delay. So this option would only ever allow for the most basic of systems, requiring humans firmly in the driving seat of all decision making. While this simplification was deemed an acceptable compromise for the scope of this project, it is assumed that the project will be continued by others, and so this is an essential factor to consider. In this project, we also assumed that the position of the ball and players is known, as this data has been taken from previous games that have been recorded from the server. In the real case, this would not be available, and mechanisms to determine this input must be a part of the system itself. Intending to address both of those points, the second workstream relating to the camera system focused on obtaining information from the data captured in the video streams. As the first step in that direction, the specific objective here was focused on getting the ball position so that this could input into Song and Ankita's control software and also update the simulator views. At this stage, the player positions have not been considered, as this information is required for more granular decisions.

Workstream 1

The initial step in developing this interpretation aspect of the camera system was to construct a scale model of the real pitch. Owing to logistical reasons, such as availability of the actual pitch and distance from our office, this proved to be a useful and convenient tool in early-stage prototyping of ideas. From this, we determined a first attempt strategy of a fixed camera, independent of the drone, to capture the ball and whole pitch. Then by taking the homography, we could determine the world co-ordinate of the ball with respect to the pitch from the image space. Several issues where identified from conducting scale model testing, such as bright spots, occlusions, and changes in lighting condition. In order to overcome these issues, the first step would be to apply a mask to the image; this would block out the background so that only the pitch area would be considered. At this early stage, occlusions would just be accepted, and in such cases, the system would not be able to provide a decisive location for the ball. This was deemed to be acceptable because the dynamics of the game mean that occlusions are usually temporary, and the drone would only move with a quite significant change in ball position anyway. From testing on the model and the actual pitch, it was found that occlusions are not as substantial as was expected. Of course, the use of extra cameras would also further reduce this impact. To address the issue of changing light conditions, using Hue, Saturation, Value (HSV) color rather than RGB color significantly reduced the effect of this. For this first proof of concept in the specific test environment, a bright orange ball was used, and so we could expect high contrast with all other actors on the pitch; also, the light conditions at the Tech United facilities are well controlled. To improve system quality, several filters are applied to the image, such as erosion and dilation, and the position of the ball in the image is identified by blob detection and taking the centroid.

image of the model pitch in the office here

It was found that the most convenient option would be to use the camera built into a mobile phone. From a practical point of view, this also has the benefit that everyone tends to carry a cellular phone around with them. In order to do this, the Epoccam app was used that allows the mobile phone to be used as a wireless webcam. It was found that if both the phone and laptop where connected to the Eduroam network, then there was no discernable latency issue. This was also tested using a mobile data hotspot, and this also proved to have excellent performance. In order to mount the mobile phone, a camera mount was constructed from a selfy-stick and a 3d printed base that allowed it to be secured to a scaffolding pole at the pitch site. On the pitch, testing showed that the delay and accuracy were both completely reasonable. At the extreme corners of the pitch from the camera, the system placed the ball within less than one meter of precision on a first attempt; this was deemed adequate just for informing the drone position. At the center of the pitch, the precision was within 20cm. Due to the coronavirus, further testing and refinement were not possible. There is, however, the scope for plenty of improvement if it should be needed. The camera was only placed 2m off the ground, and so this means that, at that height, at the extremes of the field, the pitch line angles are very acute. Also, the image quality was only set to low during that particular test. The testing that was done at the higher resolution showed no noticeable drop in latency.

Workstream 2

Drone's camera is a subsystem that fulfill the requirement to get the stream of the game from the drone. Additionally, the image streamed by drone's camera is also expected to obtain information from the actual field (e.g ball/player location). During the projects, three design options are considered and analyzed, see Table below.

Vision System Choices

The first option is analyzed by investigating the communication performance between raspberry pi and pc. The communication uses IP communication give ref link here. . Some setting change is observed to verify the communication performance, such as changing from TCP protocol to UDP and use different wifi router. The result shows high latency with the fastest would be 0.6s. This shows that wireless communication is not reliable to give field information (e.g ball location), hence we leave this option out.

For the second option, it has a risk of processing burden in raspberry pi to handle communication and image processing. In addition, more work is required on the drone system (e.g the drone needs to be expanded).

Given the consideration above, the requirement for the vision system is concluded as to only streaming video (option 3). Therefore, an FPV camera is chosen as camera on crazyflie drone.

Final vision system setup:

  • Wolfwhoop WT05FPV Camera
  • Skydroid 5.7 dual receiver

Operation:

  • Connect the power cable from FPV camera with crazyflie battery.
  • Connect skydroid receiver to pc run any webcam software/choose as second webcam for opencv.
  • Set radio frequency in FPV camera into skydroid receiver frequency .

Integration

This task integrates all the subsystem according to three deliveries defined. Those deliveries are defined as follows:

First Delivery: Reading from simulator data, translate it as drone setpoints sequence

The goal for this delivery is to verify the path planning algorithm in simulator. The plan is to extract the ball location data of previous game data (e.g to .csv file) and translate that into drone setpoint which will be equal to point of view location in simulator.

The expected result would be the point of view in simulator that play a recorded gameplay is equal to the setpoint generated by path planning algorithm

The integration task for this delivery involves simulator, and path planning algorithm. In general, the task identified are to answer these questions:

  1. How path planning is implemented and get ball location from a recorded techunited’s gameplay.
  2. How the setpoints produced by simulator is read by simulator as point of view.

These questions are solved by logging the ball location simultaneously while a gameplay is played. The path planning algorithm read this csv file as a matrix of ball location and return another .csv file that contains a matrix of camera/drone location. The simulator then is modified to read .csv file with camera location and insert that as simulator’s point of view.

<put image here>

Second Delivery: Implement the drone setpoint from previous gameplay on the crazyflie

The goal for this strategy is to verify the drone behavior based on setpoints given (e.g response time, maximum speed). Crazyflie read the .csv file of setpoint produced by previous delivery. This strategy shows the drone flies in the actual field based on the setpoint sequence generated, while the drone location is fed to simulator that playing a game data. The expected result would be the location of the camera in simulator is synchronized with the ground truth of drone location and hence the goals are achieved.

<image here>


This strategy will be the first delivery of the project where the drone flies in the actual field based on the setpoint sequence generated, while the drone location is fed to simulator that playing a game data. The expected result would be the location of the camera in simulator is synchronized with the ground truth of drone location and hence the goals are achieved.


This integration task involves drone, simulator, and path planning subsystem. The sub-tasks which are addressed should be:

  • Transform frame reference from simulator and drone

Since both use different frame reference, transforming the coordinate is required between loco positioning system(lps) and simulator. We decided to add this functionality in drone system: drone transform the setpoint to its lps coordinate and transform its ground truth position to simulator coordinate which is fed to simulator as camera position.

  • Communication between drone and simulator

The simulator is run in matlab enviroment while the drone is in python environment. To enable transferring the data from the drone to simulator, two options are considered:

  1. Drone write a file (e.g csv file) that contains drone actual location, and simulator read the file as simulator camera position
  2. Use socket communication, to communicate between the process

Both are feasible and tested with minimum code (<socket communication> <csv communication>). The second option has been tested with crazyflie running. Following are the pros and cons between the options:


<insert table here>

Final delivery: Get ball location from vision system, translate it as drone setpoints sequence

<insert image here>

Figure above shows how this strategy will look like. Based on the analysis for the camera on the drone, it is decided to use a static vision system to get the ball location. This static camera feeds ball location into path planning algorithm that later will generate the drone set point.

Since the ball localizing system can be developed in any system, integration between static vision system is considered based on different scenarios.

<insert table here>

Testing

The computed path planning algorithms are tested on the Visualiser and the hardware itself as explained as follows:

  • For a given recorded game, the Simulator is capable of providing the ball coordinates which are then fed into the path planning algorithm. The python script for both the algorithms generate a .CSV file with information on drone's reference position along with the yaw angle of the drone. This is sufficient information to set a frame of view within the game and visualise the activity of the active players and the ball. More information on the implementation of this method can be found here.
  • After the successful implementation of the algorithm on the simulator, the reference drone position points are plugged into AutonomousSequence.py python script. These points are fed into the drone to have the ball in the field of view of drone's camera.

In a real test case scenario, the drone gets the information of the ball's position using the onboard camera and objective of the drone would then be to keep the ball in its field of view.