Extensive and realistic numerical simulations were conducted for a 6 DoFs ground manipulator (anthropomorphic arm and a spherical wrist) cooperating with one quadrotor UAV, see Fig. 5.1. The goal is to both show and validate the feasibility and effectiveness of MAGMaS and the control scheme presented in Chapter 4. The simulation has been performed in Matlab/Simulink environment with the SimMe- chanics modeling toolbox. With this method the plant dynamical model is derived by the toolbox based on the specified geometry and mass repartition. This approach guaranties that the model used in the controller and the one used for the dynam- ics simulation are derived independently. For the control allocation, see Sec. 4.1.2, the optimization problem is solved via Sequential Quadratic Programming (SQP) method. The simulation sample time is 1 ms and the control loop one is 10 ms. The simulated ground manipulator is a Universal Robot UR5, with links length
5.1. Underactuated Aerial Robot 71 -2 0 2 4 [r ad ] q1 q2 q3 q4 q5 q6 2 3 4 5 6 7 8 9 10 time -40 0 40 80 [N m ] um1 um2 um3 um4 um5 um6
Figure 5.3 – Ground manipulator side. On top, both desired and actual joint angles. The desired angles are from inverse kinematics of the task trajectory. On bottom, associated manipulator input torques.
of 1.0 m and 0.7 m, total arm’s mass of 18.4 kg, maximum rated payload is 5 kg and maximum joint torques are [150 150 150 28 28 28] N m, from base to EE. For the first simulation studies we decided to focus our interest on UR5 manipulator, mainly because its strength is such that the interest of cooperative manipulation can be exhibited with smaller/lighter object than for more powerful manipulators. Also the dynamics parameters of the UR5 have been publicly available in the liter- ature, which is not the case for all industrial manipulators. The model developed in Sec. 3.3 can be derived the same way for any chosen manipulator. The simulated AR is a quadrotor of 0.50 m circumference actuated by four motor-propeller sets, each one can generate up to 10 N, and the length of the arm holding the gripper is 40 cm. The spherical joints limit is considered to be described by a cone of π/4 half cone angle. All the motors are modeled as a second order linear system with a 10 ms rise time. The cooperative task studied corresponds to a trajectory tracking task with the load being a 5 kg bar of dimension 0.05 m × 0.2 m × 1 m. The loading of the bar on the back of a mobile platform on which the ground manipulator is mounted is explored, see Fig. 5.1. The UR5 grasps the bar from one end and the quadrotor from the other end. The task consists to follow an appropriate trajectory (generated through way-points and cubic-spline-based trajectory generator) to put the bar on the back of the mobile base. Such a scenario could be of interest in robotic search and rescue missions. The control system is implemented considering a highly uncertain model, some terms of the controller inverse dynamic are neglected, the Inertial matrices are assumed diagonal and the Coriolis/centripetal terms are omit- ted. Furthermore 10% uncertainty is considered for the contact points, in order to highlight the robustness of the control approach, as the model used in the controller is highly uncertain.
The results of the trajectory tracking task are depicted in Fig. 5.2, with the ground manipulator EE position measured with respect to its base. As it is evident from this figure the given trajectory is tracked sufficiently well, even though the
[r ad ] -0.4 -0.2 0 0.2 AR? AR3 ARA [N ] -20 0 20 40 60 h 1x h1y h1z time 2 4 6 8 10 [N m ] #10-3 -5 0 5 ur1 ur2 ur3 time 2 4 6 8 10 [N ] 20 30 40 ut
Figure 5.4 – AR side. Left, AR orientation and associated control torques, ur.
Right, output of AR force allocation ht and the AR thrust magnitude generated
along AR’s z-axis.
dynamics of the system is partially unknown, error in position are comprised in ±7 cm range. Note that the ground manipulator alone is not able to perform this task, because of the torque constraints. Indeed the object weight is at the limit of the ground manipulator payload and the weight-generated torque at its EE does not satisfy the joints torque limits. For this reason, the ground manipulator alone is not able to perform the task, without grasping the bar far from its CoM. The MAGMaS core idea is to mitigate this requirement thanks to the use of an AR acting as a
flying companion to reduce weight-generated torque on the ground manipulatorEE. The ground manipulator desired and actual joint angles are plotted in Fig. 5.3, with the desired joint angles obtained through inverse kinematics for the task tra- jectory. The weakness of the wrist joints generates larger errors in q(4, 5, 6), as the desired position can not be attained without violating the joint torque limits even with the help of the AR. However, thanks to the AR support the tracking task is performed sufficiently well. The control torques of the ground manipulator, shown in Fig. 5.3, remain within their actuation limits ([176 176 100 100 100 38 38] N m, respectively).
Fig. 5.4 shows the quadrotor states and control inputs in the simulation. Fig. 5.4 top left shows the orientation of the quadrotor which remains far away from the spherical joint limit and Fig. 5.4 bottom left shows the associated control torques. Fig. 5.4 top right illustrates the output of the optimal force allocation algorithm for the quadrotor, that is a desired force vector ht. This force vector is then generated
by urand utwhich are the moments and thrust of the AR, shown in Fig. 5.4 bottom
right and left, respectively, and again they all satisfy the system constraints. Note that in order to overcome the ground manipulator limits, from approximately t =5 s to t =7 s, the AR is pushed to its maximum total thrust by the control allocation algorithm.
5.1. Underactuated Aerial Robot 73
Figure 5.5 – Experimental setup: a KUKA LWR4 arm, a classical underactuated quadrotor and an in-house designed passive rotational joint. The two extreme configurations of the up-down trajectory with the bar hold horizontally are depicted.