4.2 Simulation
4.2.3 Vector field for Obstacle Avoidance
Artificial potential field is widely applied in obstacle avoidance, and collision avoid-ance, for example in (Khatib, 1986, Chang et al., 2003). In our case, the vector field is also suitable for obstacle avoidance. In this simulation, the quadrotor flies to a
−0.5
(a) The 2D vector field is the same with Figure 4·1(a) but translated to [0.9, 1.5, −0.5]T, with a constant altitude 0.5 meter. The trajectory con-verges to the circle (x − 0.9)2+ (y − 1.5)2 = 1
(b) The quadrotor starts from [0.9, 0, −0.5]T to follow the vector field, and spire to the center [0.9, 1.5, −0.5]T, to which the vector field is trans-lated. The quadrotor achieves a trajectory the same with the logarithmic spiral as in Figure 4·2.
Figure 4·3: Simulation of a single quadrotor flying in two vector fields, limit cycle (left) and logarithmic spiral (right).
destination point, while trying to avoid collision with an obstacle on its path. The destination generates an attracting field to pull the quadrotor to it, while the obstacle generates a repelling field to push the quadrotor away from colliding.
Remark 1. The destination center point [xa, ya]T generates an attracting vector field as the gradient of a 2D Gaussian function
fa(x, y) = Aaexp−(x − xa)2− (y − ya)2
The obstacle center point [xr, yr]T generates a rejecting vector field with its mag-nitude to be a Gaussian function
fr(x, y) = Arexp
−(x − xr)2− (y − yr)2 2σr2
, and the direction is departing the center of the obstacle, formulated as
Vr(x, y) = fr(x, y)
while [x, y]T 6= [xr, yr]T, and Aa and Ar are constant coefficients.
Both formulations are infinitely differentiable, so they allow for the computation of the necessary spatial derivatives for the endogenous map. The attracting vector field in equation (4.5) guarantees the attracting vector magnitude is small if the robot is far away from its destination, and the magnitude converges to 0 when approaching to its destination. So that in the control, the robot will not move too fast when it is far away from the destination and when it is almost arrived at the destination. The repelling vector field in equation (4.6) guarantees that the magnitude of repelling vector is larger as closer to the obstacle, preventing the robot from colliding to the obstacle.
0 0.5 1 1.5 2 2.5 3
0 0.5 1 1.5
2 Start Destination P ath
Figure 4·4: A composite 2D vector field of an attracting field from the destination at [2.5, 1.5]T and a repelling field from the obstacle at [1.2, 0.6]T. The path shows the expected trajectory of a robot starts from the point [0, 0.2]T, and then follow the vector field to the destina-tion, where the vector vanishes to zero.
A vector field composed from an attracting and a repelling vector field is shown in Figure 4·4. The two vector fields are formulated from Remark 1. Figure 4·5 shows the simulation results by applying the vector field in obstacle avoidance.
−0.5 the quadrotor passes it from left.
−0.5 the quadrotor passes it from right.
Figure 4·5: The quadrotor avoids the obstacle with different trajecto-ries when the (virtual) obstacles (black circle) are at different positions.
4.3 Experiments
In this section, we carry out the experiments applying the vector field method. The experiments were conducted with a K500 quadrotor, form KMel Robotics1. The po-sition and orientation of the quadrotor are observed by an OptiTrack motion capture system2 at a default update rate 120Hz and obtained by MATLAB via Java scripts reading UDP packets from the OptiTrack. The velocity and angular velocity are computed by numerical differentiation from the position and Euler angles, rendered by the Extended Kalman Filter (EKF) in MATLAB.
The UDP packet parsing, the EKF algorithm, and the SE(3) controller are imple-mented in C language and are called by MATLAB via the MEX interface to speed up the calculation, making it possible to run the algorithm on-line without downgrading the OptiTrack update rate, even when flying multiple quadrotors. Meanwhile, the K500 receives the desired thrust, Euler angles and Euler angles’ rate as the inputs, and executes its on-board SO(3) controller based on the on-board IMU data at a
1http://kmelrobotics.com/, (KMel Robotics was acquired by Qualcomm Technologies, Inc.
on February 2, 2015.)
2http://www.optitrack.com/
500Hz loop rate.
All experiments were conducted corresponding to the same setup as in the simu-lations in Section 4.2. The same as in the simusimu-lations, we let the quadrotor take off and fly into the vector field by following a pre-defined trajectory, which is generated from B´ezier curves (Farouki, 2012, Murray, 2009). The final state of this pre-defined trajectory is designed to match the vector field so that the quadrotor can transition smoothly from the trajectory following control to the vector field following control.
4.3.1 Limit Cycle
The quadrotor first flies to [0.5, 0.5, −0.5]T, which is outside of the limit cycle (x − 0.9)2 + (y − 1.5)2 = 1 on a constant altitude (−z = 0.5 meter), with a pre-defined trajectory, then in the vector field following, it converges to the limit cycle as expected, with a small position error due to modeling error, control error and even aerodynamic influences. Along the cycle, the quadrotor flies at a constant speed 1.4m/s, as in Figure 4·6(a).
(a) Composite image of a single quadrotor flying a limit cycle, http://tinyurl.com/qbus6c7.
(b) Composite image of a single quadrotor fly-ing a logarithmic spiral, http://tinyurl.com/
kx2meq4.
Figure 4·6: A single quadrotor flying a limit cycle and a logarithmic spiral path at a constant altitude 0.5 meter.
4.3.2 Logarithmic Spiral
The quadrotor first flies to [0.9, 0, −0.5]T where it enters the vector field, as shown in Figure 4·6(b) at a constant altitude −z = 0.5 meter. The maximum speed in this case is 1.4m/s, and the quadrotor quickly slows down when approaching the center of the logarithmic spiral.