Obstacle Avoidance for Autonomous Locomotion of a Quadrotor Using an HPF

— There is a concept called “kinodynamic motion planning” which can consider kinematic constraints and dynamic constraints simultaneously. In this paper, we test the proposed kinodynamic motion planning, which was confirmed in only simulations, by an actual experiment. The experiment assumes that the quadrotor moves in the static environment, and it is confirmed that the quadrotor can reach around the requested target point while avoiding the obstacles.


I. INTRODUCTION
Recently, there are many researches on autonomous locomotion for a quadrotor, which is the vertical takeoff and landing (VTOL) aerial robot with four rotors. For autonomous locomotion of a quadrotor, it needs to move and avoid obstacles while keeping its attitude. There is a concept called "kinodynamic motion planning" which can consider kinematic constraints and dynamic constraints simultaneously [1], and some control methods based on kinodynamic motion planning are proposed [2]- [5].
Therefore, we aimed to realize "kinodynamic motion planning" of the quadrotor for designing the control input which considers kinematic constraints and dynamic constraints, simultaneously. In this research, the kinodynamic motion planning for the quadrotor is achieved by combining control input based on the harmonic potential field (HPF) for considering the obstacle information on the environment with nonholonomic control input for considering the dynamics of the quadrotor. By using the proposed method, it is already confirmed by simulations that the quadrotor can move to the arbitrary target point while avoiding obstacles and keeping its attitudes [6].
In this paper, we test the proposed kinodynamic motion planning, which was confirmed in only the simulations, by an actual experiment. The experiment assumes that the quadrotor moves in the static environment, and it is confirmed that the quadrotor can reach around the requested target point while avoiding the obstacles. Moreover, a controller based on the HPF and the viscous damping force to save the speed is compared to a controller based on using only HPF, by checking the behavior of the quadrotor.

II. KINODYNAMIC MOTION PLANNING FOR A QUADROTOR
In the proposed method, kinodynamic motion planning is achieved by combining nonholonomic control input and the gradient information which is calculated from the HPF. The system input , which is constructed by nonholonomic control input c u and control input u  based on the gradient of the HPF, is as follows: Here, 1 U is a control input for acting on each translational motion, and 2 U , 3 U and 4 U are control inputs for acting on roll angle  , pitch angle  and yaw angle  , respectively. In the following subsections, we describe the dynamical model of an quadrotor, the control input based on nonholonomic control c u and the control input u  based on the gradient of an HPF.

A. Dynamical Model of a Quadrotor
A quadrotor controls its three directional positions (x, y, z), in which it moves back-and-forth, right-and-left and up-and-down, and three attitude angles (    , , ), in which it performs roll, pitch and yaw motion, by using mounted 4 rotors on the airframe. The coordinate (x, y, z) and the rotation angle (    , , ) constitute the righthanded system. Let define m [kg] as the mass of the quadrotor, l [m] as the length from the center of the airframe to the center of the rotor, g [m/s2] as the gravity (1) acceleration, x I , y I and z I [kg/m2] as the moment of inertia around each axis respectively, and r J [kg/m2] as the moment of inertia for a rotor. Here, 1 U is the control input for acting on each translational motion, and 2 U , 3 U and 4 U are the control inputs for acting on roll, pitch and yaw motions, respectively. The dynamical model of the quadrotor is:

B. Nonhlonomic Control Input
The control input is added for Zdirection and three attitude angle and given as follows [7]: In this equation, 5 1 ,..., k k are positive constant gains, and T z is an arbitrary altitude and ( ) are the desired angles.

C. Added Control Input
In this subsection, an added control input u  is described for the translational motion. In this paper, it is assumed that the quadrotor moves on X-Y plane while hovering in constant altitude. For the control in the X-Y plane, the X-and Ydirectional gradients of an HPF are added in the control input for  and  angles. When the position coordinate of the quadrotor is and the gradient of the HPF is , then, using the gradient of the HPF, an are the speed selection gain and the gradient selection gain of the HPF, respectively. The selection method of the selection gains depends on the movement characteristics and the form of the dynamic control law of the controlled object. The quadrotor can move its position by tilting the attitude. For example, in the X-Y plane, the quadrotor can move toward the X-axis by tilting the body to   angle, and move toward the Y-axis by tilting the body to  angle. Therefore, it is assumed that the quadrotor is hovering in constant altitude using a nonholonomic controller. At that time, the control toward the X-Y direction can be achieved by adding the X-and Y-directional gradients of the HPF to the pitch (  directional) controller 3 c u and the roll (  directional) controller 2 c u , which are based on nonholonomic control.
According to the above discussions, if the quadrotor only moves on the X-Y plane, then the selection gains c B and v K can be decided as below: . Using the extended coordinate vector 3 4   e x and the extended gradient vector 3 4 ) ( the control input based on the gradient of the HPF can be written by: Here,

III. THE SPCIFICATION OF THE AR.DRONE
In this experiment, the AR. Drone 2.0 developed by Parrot Co. is used as the controlled object. Figure 1 shows the overview of the AR. Drone. The size of the frame is 32 (length) × 28 (width) [cm], and its weight is 400 [gf]. In addition, small cameras are mounted on the front and under the frame. The AR. Drone can keep the hovering state by controlling the attitude angle and its angular velocity with the mounted ATMEGA8L 8bit micro controller. This controller can receive the velocity toward each axis direction, X V , Y V , and, Z V , a target altitude d z , and  angle speed  V , as the control input from outside [8].
In this experiment, it is assumed that this attitude controller is equivalent to a nonholonomic controller in our method, and the additional controllers based on an HPF, NADFs, and clamping control function are added. In other words, the kinodynamic motion planning is realized by giving the control inputs added on the  and  in our proposed method as the control inputs of Y V and X V for AR. Drone. Finally, the control inputs can be derived as follows:

IV. POSITION MEASUREMENT SYSTEM USING TWO CAMERAS
In the position measurement system used in this research, two cameras track the position of an infrared LED marker mounted on the AR. Drone, and the frame position is calculated [9]. Using the principle of triangulation based on the measured position, the 3D position of the marker ( z y x , , ) [cm] can be calculated on the coordinates, whose origin ) 0 , 0 , 0 ( O is defined at the intermediate point between two cameras. Figure 2 shows the overview of the position measurement system, which is based on using a stereo vision, for the horizontal and depth directional positions. It is assumed that the horizontal resolution of a camera image is defined by max u , the horizontal angle of view for the camera of each right and left is defined as R  Here, the average of the measured altitude from the right and left cameras is used as the marker position m y , because there are some errors between the right and left cameras in the actual measurement. Assume that the vertical angle of view of the right and left cameras as R  and L  , the vertical position of the marker on the right camera image as R v , the angle R  from the lower end of the vertical angle of view for the right camera to a marker, and the vertical position of the marker m y , can be given as follows: In this research, this system is used for measuring the position of the AR. Drone from outside. Experiments In this section, the actual moving experiments is conducted using the AR. Drone based on the proposed control input. As mentioned above, in this experiment, it is assumed that the mounted attitude controller on the AR. Drone is equal to the nonholonomic controller for keeping its attitude in our method, so that the added controllers based on an HPF, NADFs, and clamping control function are only added for guiding. Then, the target speed toward each axis, i.e., XR V , YR V , and ZR V , are given as the control inputs for AR. Drone: Note that, cx b , cy b , vx k , and vy k are the positive constant gains, and x f and y f are the gradients toward X-and Ydirection calculated from an HPF. Here, as mentioned later, VXR is set to a negative value for adjusting the difference between the robot axis system and the coordinate system of the position measurement system. In this experiment, the marker for measuring a position is mounted on a frame as in Fig. 4, and the position of the marker is measured by two cameras set in the environment for confirming that the proposed controller can guide the quadrotor to the target point. The gradient of an HPF is calculated by desktop PC, and the data is sent to the AR. Drone through the Wi-Fi. The HPF is the potential field which is calculated from the reaction force from the obstacles and the attractive force from the target point. In this experiment, the environment is assumed to be known, and the HPF that was created in advance is used. For checking the detail of the HPF, see the paper [6]. A. Conditions Figure 5 shows the picture of an actual environment, and Fig. 6 shows the position relation in an experimental environment as viewed from the top. As shown in Fig. 6, the initial position of the AR. Drone is set that the positive directions of the X-, Y-, and Z-axis of the position measurement system are matched with the positive directions of the R Y -and R Z -axis, and the negative direction of the R X -axis of the robot coordinate system, respectively. Moreover, as shown in Fig. 6, the distance between the cameras is set to 3. , respectively. The gain for x V is set to be larger than the gain for y V , because the inertial moments of AR. Drone around the X-and Y-axes are different, and there is a difference in the control effect [10].

B. Results
The results of the flight experiment are shown in Figs. 8 -12. Figure 8 shows the trajectory of the quadrotor on X-Z plane. Moreover, Figs. 9 -11 show the change of the positions of X, Y and Z directions, whereas Fig. 12 shows the error from the target position. In each graph, the blue solid lines show the results with viscous damping force, whereas the orange broken lines show the results without viscous damping force. The red solid lines in each graph show the target value.

C. Discussions
As shown in Figs. 8-12, it is confirmed that the quadrotor was able to reach the target position with both controllers while avoiding the obstacle. Moreover, as shown in Fig. 11, the both controllers were able to keep the altitude of the quadrotor within 10  [cm] from the target altitude. The controller with the viscous damping force was able to guide the quadrotor to the target point a little faster than the case using the controller without viscous damping force. This is attributed to the fact that the viscous damping force saved the overshoot and the controller was able to guide the quadrotor to the target point with less movement. There is a possibility that the viscous damping force works more effective by tuning the gains.

V. CONCLUSIONS
In this paper, actual experiments have been made to implement the kinodynamic motion planning based on an HPF to an actual machine. In particular, assuming that the attitude controller mounted on the AR. Drone developed by Parrot Co. is equivalent to the nonholonomic controller in our method, the controller based on the gradient of an HPF was added for guiding the AR. Drone. Then, the trajectory of the quadrotor was measured and recorded by using a cameras system mounted on the environmental side. From the actual experimental results, it was confirmed that the AR. Drone was able to move to an arbitrary target point while keeping its attitude angles and avoiding the obstacle. Moreover, it was shown that there exists a possibility to realize more effective control by adding the viscous damping force.