11.1 Experimental Setup

Fixing an experimental prototype is a time-consuming and expensive task because of the high cost and fragility of the sensors and other electronic components embedded on an UAV. In addition, evaluating new algorithms results is a risky mission where the experimental platform is continuously in danger of falling and crashing due to bugs in the implementation or to a bad parameter tuning. Henceforth, an inexpensive navigation test-bed based on the Robot Operative System (ROS) was built to reduce the implementation time and economic cost. It is conformed by an inexpensive commercial quadrotor type AR.Drone, wirelessly connected to a ground computer where all the extra computations are executed using the middleware ROS, as illustrated in Fig. 11.1. Such a test-bed follows and extends the ideas presented in [7], and offers an excellent alternative for fast and safe validation of new proposed control strategies, state observers, and computer vision algorithms.

11.1.2 Monocular Vision Localization

The aerial vehicle position is obtained using computer vision and inertial data fused with an EKF algorithm. The vision algorithm, based on Parallel Tracking and Mapping (PTAM), estimates camera pose in an unstructured scene [7–9]. The algorithm executes in parallel the vision information for the tracking and mapping. It also constructs a sparse depth map (see Fig. 11.2) which is used in this work to estimate distance to frontal objects.

Even if the PTAM algorithm is a good solution for pose estimation, it was conceived for mostly static and small scenes, and an absolute scale for the map is not provided. This could be considered as a drawback for MAV (Micro Aerial Vehicle) applications. Nevertheless, in [7] and [10] the authors proposed a nice solution fusing data measurements coming from an IMU, a camera, and ultrasound sensors, and using a scale estimator and an EKF. One advantage of this solution is that the vision approach can be obtained as open-source for ROS (Robot Operating System).

For this work the control algorithm code in [7] was replaced with a new one in order to easily implement and validate different control strategies and to help tuning the required gains. Finally, the localization algorithm was modified to recover the point-cloud of the depth map generated by the PTAM algorithm, and send it to another node to estimate the distance to potential collisions, as explained in Sect. 11.2. This way the operator can select online between the different programmed trajectories, control laws, and operation modes, as well as modify in real time any parameter for tuning.

11.1.3 Prototype

The AR.Drone 2.0 is a well-known commercial quadrotor (price ≈ 300 USD) which can be safely used close to people and is robust to crashes. It measures 53 × 52 cm and weighs 0.42 kg. It is equipped with three-axis gyroscopes and accelerometers, an ultrasound altimeter, an air pressure sensor, and a magnetic compass. It also provides video streams from two cameras, the first is looking downwards with a resolution of 320 × 240 pixels at a rate of 60 fps and is used to estimate the horizontal velocities with an optic flow algorithm while the second camera is looking forward with a resolution of 1080 × 720 at 30 fps and is used by the monocular vision algorithm. Fig. 11.3 introduces the main technical characteristics of the AR.Drone 2.0.

However, neither the software nor the hardware from the AR.Drone can be modified easily. It includes an internal onboard autopilot to control roll, pitch, altitude velocity, and yaw rotational speed (ϕ, θ, $\dot{z}$, and $\dot{\psi }$), according to external references. These references are considered as control inputs and are computed and sent at a frequency of 100 Hz. All sensor measurements are sent to a ground station at a frequency of 200 Hz, where the vision localization and the state estimation algorithms are calculated in real-time on ROS.

11.1.4 Haptic Interface

The low-cost Novint Falcon haptic device is used to remotely control the UAV while providing the pilot with force feedback. It is composed of a three-degree-of-freedom robot arm in delta configuration, with a touch workspace of about $10\times 10\times 10\text{ cm}$ and 400 dpi of resolution (see Fig. 11.4). This haptic device is capable of exerting forces up to 8.9 N in the three axes. The Novint Falcon is connected to the ground station via USB and is controlled with the help of ROS. The position of the final effector and information from the bottoms are recovered and used to control the drone's behavior, while information from the computer vision algorithm is used to determine the exerted forces, improving the flying experience and awareness of the pilot. More details about the control of the UAV from the arm position and the generation of the forces applied by the haptic device are given in Sect. 11.3.

11.2 Collision Avoidance

The continuous change in the operation conditions produced by the environmental factors and the interactions with other unknown agents pose a mayor challenge in the pursuit of autonomous and semi-autonomous navigation of mobile robots, with the added difficulty of the payload limitation for UAVs. This drives in the need of real-time perception of the unknown environment and an appropriate reaction, both being a major concern in the design of autonomous UAVs. In the present work, we are interested in the development of an effective strategy for detection and avoidance of collisions in UAV navigation. Particularly, we are interested in the use of the already available information from the embedded sensors in the inexpensive commercial quadrotor AR.Drone 2.0.

We intend to employ computer vision algorithms using information from the frontal camera to detect feature points on the image and use them to estimate the distance to possible obstacles. This is accomplished taking advantage of the sparse depth map generated by the PTAM localization algorithm. Since only a sparse map is available, some possible obstacles can be ignored if not enough visible characteristic points are present. In order to ensure collision avoidance and safe operation of the system, only the horizontal projection of the point-cloud is considered, as depicted in Fig. 11.2. This means that the height of the obstacles is ignored, and obstacle evasion is only possible in the horizontal plane. This approach, although conservative, is still useful for several missions where obstacles such as walls or columns are present. However, it still results in a noisy depth-map, and special attention should be taken for obstacles presenting low-texture surfaces (flat one-color walls).

As stated, distance to possible obstacles is estimated from the horizontal projection of the point-cloud. It is composed of a set P of n characteristic points on the image $p_i(x_i,y_i,z_i)$, $i=1,\dots ,n$, computed by the PTAM algorithm. We define the estimated distance to frontal obstacles $d_y$ as the average depth, w.r.t. the position of the quadrotor along y, of the η points inside certain lateral range ε from the lateral position of the quadrotor x, namely

$\begin{array}{c}\hfill d_y=y-\frac{1}{{\eta }_y}\sum _{i\in {\mathrm{\Omega }}_y}{y}_i,\hfill \\ \hfill {\mathrm{\Omega }}_y=\{p_i(x_i,y_i,z_i)\in P|x_i\in [x-\epsilon ,x+\epsilon ]\}.\hfill \end{array}$

We can define the estimated distance to lateral obstacles $d_x$ in the same way.

In order to avoid collisions, a potential field is applied such that if distance $d_i$ ($i=x,y$) falls bellow certain safe distance $d_s$, then a repulsive force $F_{{\mathit{rep}}_i}$ will be exerted as follows (see Fig. 11.5):

$F_{{\mathit{rep}}_i}=\{\begin{array}{cc}\hfill 0\hfill & \hfill \text{if}{d}_i>d_s,\hfill \\ \hfill -k_{{\mathit{rep}}_i}(\frac{1}{d_i}-\frac{1}{d_s})\left(\frac{1}{d_i^2}\right)\hfill & \hfill \text{if}{d}_i⩽d_s.\hfill \end{array}$

Note that lateral obstacle detection becomes much more challenging since only a frontal camera is used for this purpose.

11.3 Haptic Teleoperation

In this work we successfully applied the Novint Falcon haptic device to assist in teleoperation and to prevent the user from crashing the quadrotor against an obstacle. To do so, the position of the final effector of the haptic device ($x_h,y_h,z_h$) provides a linear relation between the desired roll and pitch angles (${\varphi }_d,{\theta }_d$) and the desired altitude velocity (${\dot{z}}_d$). It is given as

$\begin{array}{c}\hfill {\varphi }_d=k_{hy}(y_h-o_y),\hfill \\ \hfill {\theta }_d=k_{hx}(x_h-o_x),\hfill \\ \hfill {\dot{z}}_h=k_{hz}(z_h-o_z),\hfill \end{array}$

where $k_{hx},k_{hy},k_{hz},o_x,o_y$, and $o_z$ are suitable gains and offsets. This strategy is useful for quadrotors with an internal orientation and altitude controllers. Observe from (11.3) that to keep the quadrotor hovering in a desired position, the user should keep the haptic device at the center of its workspace. In order to assist the user in this task and simplify the manual control of the UAV, a proportional derivative controller is applied to regulate the haptic's final effector position at the origin if no force from the operator is exerted. Similarly, a repulsive force is applied to the haptic device once the quadrotor approaches an obstacle. Then the feedback forces applied to the haptic device are defined as

$\begin{array}{c}{F}_x=\{\begin{array}{cc}-k_{phx}x-k_{dhx}\dot{x}\hfill & \hfill \text{if}{d}_x>d_s,\hfill \\ -k_{h{\mathit{rep}}_x}(\frac{1}{d_x}-\frac{1}{d_s})\left(\frac{1}{d_x^2}\right)\hfill & \hfill \text{if}{d}_x⩽d_s,\hfill \end{array}\hfill \\ F_y=\{\begin{array}{cc}-k_{phy}y-k_{dhy}\dot{y}\hfill & \hfill \text{if}{d}_y>d_s,\hfill \\ -k_{h{\mathit{rep}}_y}(\frac{1}{d_y}-\frac{1}{d_s})\left(\frac{1}{d_y^2}\right)\hfill & \hfill \text{if}{d}_y⩽d_s,\hfill \end{array}\hfill \\ F_z=-k_{phz}z-k_{dhz}\dot{z},\hfill \end{array}$

with the control gains $k_{phx},k_{phy},k_{phz},k_{dhx},k_{dhy},k_{dhz}$, $k_{h{\mathit{rep}}_i}\in {\mathbb{R}}^{+}$.

11.4 Real-Time Experiments

Extensive experiments were executed to validate the proposed algorithms. The parameters used were tuned by trial-and-error and are presented in Table 11.1.

Table 11.1

Parameters

ε[m]	k rep	ds[m]	$k_{h{\mathit{rep}}_{x,y}}$
0.5	4	5	2

k hx	k hy	k hz	k phx,y,z	k dhx,y,z
12.7	14.3	18.2	100	500

Some experiments can be found on video at

https://www.youtube.com/watch?v=fr0dTSm6Go8

For the teleoperation scenario a human pilot controls the position of the quadrotor through a haptic device. The practical goal here is to use the haptic device to feed back information from the vehicle to the pilot and prevent him from colliding, via opposite forces in the haptic device when the quadrotor goes out of a safety zone. This is useful, for example, in wall-inspection missions where the operator's visibility is limited.

Collision-free haptic teleoperation is studied through Figs. 11.6–11.9. In these flight tests the user flew the UAV in semi-autonomous mode using a haptic device. Here only the orientation is in autonomous mode. The user attempts to deliberately crash the vehicle against a frontal wall a few times. The first tries were realized slowly, but the last was done at high speed, see Fig. 11.6.

The position responses in the x and y states of the UAV through the experiments are depicted in Fig. 11.7. Observe in this figure (for y axis) at times 6, 9, and 13.5 s, how the reactive collision avoidance algorithm prevented the user from driving the quadrotor to a dangerous area too close to the wall. Furthermore, at the time of 16 s the pilot deliberately tried to crash the UAV in a fast maneuver toward the wall, and even though it got to touch the wall it never crashed thanks to the good performance of the proposed algorithm even in this extreme case. For the x position, it remains quasi-constant since no lateral obstacles were present.

Finally, observe the feedback force applied to the haptic device and the control inputs send to the UAV, respectively shown in Figs. 11.8 and 11.9. When approaching the wall, the haptic device exerts a repulsive force alerting the human operator of the danger and even forbidding him from crashing the quadrotor, see the 16th second in Figs. 11.8 and 11.9.

It is important to point out that the obtained results are quite satisfactory, taking into account that only a monocular camera is used to locate the quadrotor and to detect collisions, instead of using an expensive motion capture system and/or extra range sensors as did other teams. Henceforth, the obtained results can be easily reproduced in outdoor flight tests.

11.5 Discussion

In this chapter we have studied the use of a haptic device for collision-free UAV teleoperation, by using the force feedback to drive the human user off from potential obstacles. The use of remote operation of UAVs by a human pilot remains a good alternative to the not always feasible completely autonomous navigation. The use of force feedback through a haptic device to assist the human operator allows improving the flight experience for the pilot, as well as increasing the safety of the mission. In this case we have proposed using reactive collision avoidance by means of potential fields applied to the haptic device. This way the human user is able to feel forces when he is trying to drive the drone into a dangerous area.

Force feedback is also applied to ease the piloting task by automatically directing the final effector of the haptic device to its center, which also implies the attitude stabilization of the UAV.

We also presented an obstacle detection strategy based on computer vision and a frontal camera. Such methodology allows us to detect possible collisions from only visual information. However, as only a sparse depth-map projected to the horizontal plane is used, the height of the obstacles remains unknown. Still this is useful for safe operation in several scenarios, for example, to avoid crashes against walls or columns in indoor missions where pilot's visibility is limited.