Towards the Realization of Mobile Manipulating Unmanned Aerial Vehicles (MM-UAV): Peg-in-Hole Insertion Tasks

Transcription

1 Towards the Realization of Mobile Manipulating Unmanned Aerial Vehicles (MM-UAV): Peg-in-Hole Insertion Tasks Christopher Korpela Matko Orsag PaulOh and Pareshkumar Brahmbhatt Drexel Autonomous Systems Lab Drexel University Philadelphia, PA USA Faculty of Electrical Engineering and Computing University of Zagreb Zagreb, Croatia Drexel Autonomous Systems Lab Drexel University Philadelphia, PA USA Abstract-This paper proposes a solution to the peg-in-hole problem from an aerial vehicle. While the ground robotics community has mostly solved insertion-style tasks, an air vehicle attempting the same task faces significantly greater challenges. Thus, aerial manipulation mandates a strongly coupled aircraftarm control scheme to tackle the peg-in-hole problem. Manipulator compliance along with fast inverse kinematics calculations facilitate task completion during hover. We present recent results showing the ability of a simulated dexterous aerial manipulator performing a hose into pump insertion. Kinematic and dynamic modeling of the aircraft-manipulator system are developed. Simulation results validate our system model by generating a kinematic solution and arm trajectory. Lastly, a hardware-in-theloop test rig is proposed to bridge the gap between basic aerial manipulation research and the ability of flying robots to perform tasks such as door opening, bridge repair, agriculture care, and other applications requiring interaction with the environment. In particular, the usefulness of these capabilities is highlighted in areas where ground robots cannot reach or terrains they are unable to navigate. I. INTRODUCTION Peg-in-hole or insertion tasks have been thoroughly studied and largely solved by the ground robotics colmnunity. These tasks require both position and force control and typical implementations involve force/torque sensing, compliance/impedance control, vision systems, excellent position control, or a combination of these methods. Some common applications include inserting a power plug into a socket, changing a light bulb, or placing a bolt into a structure. Ground-based mobile manipulators have solved these problems with sub-millimeter accuracy on an assembly line or even in cluttered environments such as in a home or office [1]. Expanding the workspace from 2-dimensions to 3- dimensions achievable only by air vehicles would greatly enhance the utility of flying robots with arms. A simple task such as replacing a light bulb requires a hydraulic lift if the socket is 2 stories or higher. A ground robot would have great difficulty performing this bulb replacement. However, a flying robot with a dexterous arm could easily approach the ceiling, hover, remove the old bulb, and insert the new one. Advantages of aerial manipulators have been explained in recent publications [2], [3], [4], [5]. While still largely Manuscript received March IS, This project was supported in part by a US NSF CRI II-New, Award # CNS , DoD Advanced Civil Schooling, and a U.S. Fullbright Scholarship. Fig. 1: Aerial Manipulator CAD Concept underdeveloped, aerial manipulation has seen advances in quadrotors and small co-axial helicopters. These vehicles are inexpensive and easy to operate in motion capture volumes and outdoor environments. However, manipulation capabilities are limited to grasping lightweight objects primarily due to the poor payloads of small UAVs (unmanned aerial vehicles). This paper presents a solution to the peg-in-hole problem from an aerial vehicle using a dexterous 7 degree-offreedom (DOF) manipulator. A control scheme to coordinate the coupled manipulator-aircraft system (Fig. 1) allows for the insertion of a hose into a pump. Section II briefly describes the theory behind insertion tasks. Sections III and IV describe the dynamic and kinematic model for the aircraft and manipulator. Section V details the proposed control architecture for the coupled system. Simulations and Results are shown in Secs. VI and VIII. II. THE PEG-IN-HOLE PROBLEM Peg-in-hole insertion tasks represent a classic control problem that has been widely studied. Industrial robots, mobile manipulators, and personal assistance robots are typically required to perform an assembly or insertion task. Various techniques such as compliant motion, learning, passive compliance, hybrid position and force control, and impedance control have been implemented. All of these solutions deal with challenges in position error and the dynamic interaction of the manipulator with its environment [6] /13/$ IEEE

2 Pump-fixed Point of insertion ---+ Lx End-effector r Oirect',.,I m m""1 Hose - moveable Fig. 2: Peg-in-hole concept: Hose into pump insertion Fig. 3: Yamaha RMAX Unmanned Helicopter While a ground robot can relatively easily perform the pegin-hole function, a flying robot with a manipulator faces far more difficulties. Even with strong hover control, vibrations and other aerodynamic effects caused by the rotating blades will introduce drift and aircraft attitude changes that will propagate down to the end-effector. The aerial manipulator must constantly adjust to compensate for the vehicle movement and further have adequate compliance to prevent a crash, particularly during the loose manipulator-environment coupling after insertion. Inserting a hose into a replacement pump can be seen as fairly simple peg-in-hole problem. However, with the coupled aircraft-manipulator system, this task requires careful planning. In this scenario, a hose is represented by a PVC cylinder (Fig. 2) and the grasp depends on both the arm and its end-effector. The end-effector consists of two fingers and an opposable thumb that can provide a stable grasp of the cylinder. Peg-tilting is a common methodology where the cylinder is first titled to facilitate the initial insertion, aligning with respect to the holes and then finally pushed through [7]. An initial orientation and tilting angle is derived in order to guarantee a good insertion. Peg-tilting has proven successful in peg-in-hole insertions having a clearance of tenths of millimeters [8]. III. HOVERING STABILITY IN ROTORCRAFT UAVs Due to their hovering and maneuvering capabilities, rotorcraft UAVs are the optimal choice for flying robots. Two types of rotorcrafts stand out: namely standard helicopters (containing a main and tail rotor) and widely used quadrotors. Given the weight and size of the proposed manipulator (rv 7kg), an unmanned, high endurance helicopter is the only aircraft capable of carrying such a payload. The Yamaha RMAX helicopter (Fig. 3) represents such an aerial vehicle that is commercially available. The analysis presented within this paper is applied to, but not limited to, the standard helicopter dynamic model. It can be easily applied to other rotorcraft like quadrotors, for example. Helicopter dynamics, as well as a simplified helicopter dynamic model in hover, neglecting flapping angles, H-force of the main rotor, fuselage drag, etc., are well understood [9]-[11]. Combining helicopter dynamics with manipulator dynamics increases the complexity and having one or more arms further adds to the complexity of the overall mathematical model. Therefore, a simplified mathematical model is reduced down to two realistic scenarios: Manipulation stage - where the mobile manipulator performs a certain task while the vehicle remains hovering in place; and Flying stage - where the robot flies to the desired position, while the manipulator remains stowed [2]. The peg-in-hole task, which is the focus of this paper, is performed in the Manipulation stage. Aircraft and manipulator dynamics are cross-coupled, but in the Manipulation stage, when the helicopter is hovering, its dynamics (i.e. linear and angular speed and acceleration) can be neglected, and manipulator mass, moments of inertia, and movement are regarded as disturbances to the aircraft hovering model. Although the aircraft needs to fly to the point of contact, Flying stage, the peg-in-hole task is conducted at hover assuming the aircraft has already reached an appropriate pose for task completion. In this paper, the aircraft is considered to be stable. The authors in [5] derived the necessary stability condition for a helicopter under classical PID control and with varying payloads and moments of inertia. The same reasoning was applied in the stability analysis of an aerial manipulator dubbed MM-UAV (Mobile Manipulating Unmanned Aerial Vehicle) in [12]. In this paper, a quadrotor was equipped with two 4-DOF manipulators. The stability was analyzed for different manipulator poses. Authors in [13] proposed a dynamic reduction to avoid complex aerodynamic effects like air-resistance and blade flapping. Their control design was able to stabilize a helicopter under known load. By applying the same criteria to a helicopter model equipped with one to two 7-DOF dexterous manipulator arms, a fully stable aerial robot can be achieved. When the aircraft is stable, the only dynamics of concern is the disturbance reaction dynamics produced from the movement of the manipulator(s). The disturbances cause reactions in the vehicle attitude which in turn cause position and orientation errors of the vehicle that propagate down to the end effector(s) of the arm(s). Building on the results in [14], we write the generalized disturbance response transfer function: 8 8 _ K(l + jj(t)8))w(8) ( ) ((t)W(t)8 + w(t) 2 (1)

3 Base - Shoulder pitch / Wrist yaw Shoulder yaw / Wrist pitch form: i-iti = [CO ' -SOi C "'i SOiS"'i a C so; co,c",., -co,s",., a iso, 0 s",., C"" di The position vector of the end-effector can easily be obtained through forward kinematics. The complete transformation from base to end-effector can be simplified as: o H - [R T l ] l ' (3) n (4) where R represents the 3 x 3 rotated orientation and T is the 3 x 1 position of the tool with respect to the base frame. B. Inverse Kinematics Fig. 4: CAD drawing of manipulator (showing each joint) where ((t) and w(t) represent the time varying damping ratio and natural frequency of a standard 2nd order system [15]; and (3(t) is an offset zero, which causes the bias disturbance response not to decay to zero [14]. When limt-tcx), angle offset and correspondingly the position offset decay to: K! 8(s) = w(t) 2! w( s) (2) By taking account the worst case scenario for parameters (( t), w(t), and (3(t), one can derive a manipulator control strategy that can cope with both static and dynamic disturbances. In reality, during peg-in-hole operations, contact forces are exerted on the aircraft body. To complete a peg-in-hole mission successfully, one would ultimately need to implement hybrid pose/force control, similar to those proposed in [16], [17]. IV. A. Manipulator Kinematics MANIPULATOR MODEL As shown in Fig. 4, the redundant 7-DOF manipulator consists of serially connected revolute, rigid, and modular joints and links. Frame 0 is located at the base of the torso while Frame E is the end-effector frame. Joint rotation axes are also indicated. The Denavit-Hartenburg parameters are shown in Table I. The assembly consists of a torso with the option for two manipulators. The current configuration consists of the left arm only containing the following joints: should pitch, shoulder roll, shoulder yaw, elbow pitch, wrist yaw, wrist pitch, and wrist roll. The 4-DOF left arm end-effector has joints: thumb roll, thumb pitch, ring finger pitch, and index finger pitch. The distal joints of the fingers are under-actuated and conform to the object being grasped. The transformation matrix relating the torso to the endeffector frame is obtained by chain-multiplying the homogeneous transformations together where joint ' i ' is in the standard Given a desired task space trajectory (x(t),x(t)), the goal is to find a suitable joint space trajectory (q( t), q( t)) to generate the given trajectory. The literature has numerous examples of geometric, numerical, and analytical methods to solve the inverse kinematic (lk) problem. Due to the inherent drift in the aircraft, the system will continually require fast IK computations. Therefore, we have chosen an analytical IK approach. The IK solver (called ilifast and further described in Sec. VI) is a closed-form process the can generate solutions on the order of 4 microseconds which is significantly faster than numerical methods [18]. Thus, it is possible to investigate the null space of the solution set. In a kinematically redundant manipulator, a nonempty null space exists because of the excess of input space relative to the manipulable space ( n > m). The null space is a set of task space velocities that yield null joint space velocities at the current robot configuration and these task velocities belong to the orthogonal complement of the feasible task space velocities [19]. A key feature of the IK solver includes the generation of all possible 6D transform solutions. Further, it can handle arbitrary joint complexity and generates an optimized C++ database. C. Manipulator Dynamics Using a Recursive Newton-Euler algorithm [20], [21] and neglecting friction forces, one can derive generalized force/torque equations produced from each joint movement: n n n 2:)Dij(q)!}j] + L L[Ckj(q)qkqj] + hi(q) Ti, = j=o k=oj=o Frame Link 0 d a '" 0-1 I pi/ pil pi/ pi/ pi/ pil pil2 7-E E :s; i :s; n TABLE I: Denavit-Hartenberg parameters for manipulator (5)

4 - IF"Tt Control I Wl'W2 Motor 0"- Dynamics - W3,U Helicopter y R,XR,ZR Model - - " q,q,ij -... Recursiv Newton. uler Mo W rob,uh' VB,au 1 Fig. 5: Dynamic model of the aerial robot: Wi, U - - torque and thrust inputs; F, T - Forces and torques produced within the propulsion system; W - disturbance torques from the manipulator; q, q, ij - joint angles, speeds and acceleration; red line marks the decoupling in Manipulation stage. with Dij as a generalized inertia tensor, Ck j is the generalized Coriolis and Centrifugal force matrix and hi is a generalized gravity force. Given that TO calculates forces produced on the aircraft body (i.e. W = TO), Newton-Euler analysis provides the necessary tools to calculate static and dynamic disturbances acting on the helicopter. In a complete model, Newton-Euler equations for manipulator motion need to be provided with initial angular and linear speeds and accelerations. To simplify the overall problem, in Sec. III, we make a reasonable assumption that the aircraft is in hover during Manipulation stage. This assumption enables us to regard the initial linear and angular dynamics of the aircraft body as zero, thus effectively decouple the two dynamics. Fig. 5 shows the dynamic model of aerial robot. V. IMPEDANCE CONTROL An impedance control strategy is proposed to control the dynamic interaction between the manipulator and its environment. Impedance control enables contact between the manipulator and its environment while maintaining stability during the transition from free motion to interaction [22]. In a simplified manner, the manipulator can be seen as massspring-damper system behaving like an impedance towards the environment. Even with excellent vehicle position control, relative motions between the UAV and work piece highlight the need for compliant manipulation approaches. To address the difficulties of using a rigid, redundant manipulator, a desired end-effector impedance can be expressed as: interactions has the form: = T(XYZ) Kp(XYZd-XYZ) +Kd(XYZ -XYZ) (7) where T(XY Z) is torque commanded to joints to provide XYZ movement, Kp and Kd are the proportional and derivative. I control coefficients, XY Zd and XY Z d are the desired endeffector position and velocity trajectories, and XY Z and XY Z are the current end-effector position and velocity. To specify actual torques (T) to send to the individual joints (q) and considering inertia (M), gravity (G), Coriolis and centripetal torque (C), and viscous and Coulomb friction (F), the equation has the form: T(q) = JT T(XY Z) + M(q)ij + C(q, q)ij + F(q) + G(q) (8) The Jacobian (J) is used to linearly map the joint space and task space velocities. Therefore, the differential kinematics showing this mapping has the form: :i; = J(q)q (9) and the Jacobian transpose relates the torque to the generalized forces expressed as: (10) The manipulator does not have force/torque (FIT) sensing at the end-effector but each actuator has FIT sensing in the joint. Position and torque telemetry data is provided in addition to active compliance through an impedance loop closure. VI. SIMULATION The simulation environment consists of a full scale replica of the Systems Integrated Sensors Test Rig (SISTR) [24]. The SISTR test environment is well established and allows for controllable and repeatable experiments. A two-story overhead gantry provides an x, y, z emulated position of the aircraft model. Using Model Reference Adaptive Control (MRAC), a linearized model of the rotorcraft serves as the reference model. The gains are updated according to the error between the gantry state and the math model of the rotorcraft. A 3- DOF gimbal provides the roll, pitch, and yaw angles of the 3-0OF Gimbal ----:. (Emulated UAV) where Md is the inertia matrix, Bd is the damping matrix, and Kd is the stiffness matrix. Vectors x and Xd represent the actual and desired end-effector positions, and Ie represents the generalized force the environment exerts upon the endeffector [23]. A proposed Cartesian PD controller to move the 7-DOF manipulator through space without regard to environmental Fig. 6: Simulation Scene

5 (a) Approach ;;IIS:I;'.!. _... (b) Arm Movement Fig. 8: SISTR test and evaluation environment (c) Grasp (d) Inserti on Fig. 7: Images from Peg-in-hole experiment vehicle. The gantry contains an upper carriage to provide x, y translation and a linear actuator performs z-axis movement. The OpenRAVE [18] robotics virtual environment is used to simulate the aircraft-manipulator model for peg-in-hole insertion. In this scenario, the manipulator grasps onto a hose (represented by a simple cylinder with similar mass properties) and inserts the hose into a replacement pump (represented by another cylinder). There is 5mrn of clearance for the insertion. The manipulator is exported from CAD into a COLLADA format which is an XML schema that OpenRAVE supports. The ikfast solver generates the inverse kinematics solution based on the COLLADA file and IK type (Transform6D). A Python script is run to execute the necessary tasks to perform the peg-in-hole insertion. First, the gantry joints are set as active to move the manipulator into position. Next, the endeffector grasps the hose after generating a IK solution. Finally, the hose is inserted into the pump after another IK solution is generated and executed. When a trajectory is computed, the planner must check for mechanical constrains that do not prevent arm movement, there are no environmental collisions, and finally there are no self-collisions. VII. HARDWARE-SOFTWARE ARCHITECTURE The manipulator used in this research (under construction at the time of this writing) is the HDT MK2 7-DOF robotic arm with a 4-DOF end-effector. This arm has a load capacity of over 50 pounds (23 kg) but only weighs 16 pounds (7.3kg) making it easily transportable by an autonomous helicopter such as the Yamaha RMAX. Each actuator has local velocity, position, force, and impedance loop closure. Communications are provided over Ethernet and CAN bus. As described in Sec. VI, the SISTR test environment (Fig. 9) provides a mechanism to emulate aircraft flight dynamics in a controllable and repeatable manner. This overhead gantry eliminates the need for significant UAV setup time, suitable testing locations, and the possibility of crashes. It enables the capture of forces and torques transmitted to the aircraft during both flight and manipulation. Considerable of effort has been made to implement the control system using the Robot Operating System (ROS). Using the provided node based and message exchange system, it is easy to build the control system and establish communication between the manipulator and gantry test rig which also incorporates a motion capture (MOCAP) system. The motion capture system is based on 18 VlOO:R2 OptiTrack cameras connected to a PC running Arena Software. The PC sends the data via the NatNet protocol. The controller PC implements a C++ class to read in the data being streamed from the motion capture computer. Fast Ethernet speed allows for a fast connection with practically no lag between sending and receiving data. VIII. SIMULATION RESULTS The results indicate accurate end-effector control capability while performing the insertion task within the defined performance metric (hose to pump insertion). At present, our preliminary testing includes a purely kinematic solution without force feedback. Fig. 7 shows a progression of the manipulator actions such as approach, grasping, and peg-insertion. The aircraft-manipulator begins at a designated starting point and traverses across the simulated environment (Fig. 7a) with the arm in a stow configuration. The emulated aircraft maintains a simulated hover after the system moves into a position where is can interact with the hose. Upon finger closure, the hose is moved in an upward motion for insertion into the pump. An inverse kinematic (ilifast) calculation is done to move the end-effector in a collision-free trajectory in order to grasp the hose. Having knowledge of the center point of the pump, the ikfast solver calculates a trajectory to move the hose within the corner of

6 Fig. 9: CAD concept of aerial manipulation test bed the hole. Fig. 7b shows the movement of the manipulator, Fig. 7c demonstrates grasping of the hose, and finally Fig. 7d shows the insertion into the pump. The gantry and 3-DOF gimbal emulate the aircraft and the manipulator is attached to the vehicle's "underbelly". The limitations posed by rotating blades and any ground effect are ignored. IX. FUTURE WORK AND CONCLUSIONS The next phase is to construct the aircraft-arm assembly as part the SISTR test environment as shown in Fig. 9. The aircraft model will be implemented in SISTR and the proposed force feedback and impedance control scheme will also be utilized on the actual manipulator. In addition to peg-in-hole style tasks, we are also exploring other practical applications: Door opening: The vehicle approaches a door, turns the door knob, and opens the door coordinating aircraft-arm movement.. Long cylinder transport: The vehicle uses two manipulators to grasp a long cylinder or pipe that could cause instability if only one manipulator was used. After grasping, the vehicle transports the cylinder to another location. Tool rotation or valve twisting: The vehicle grasps a tool to perform an assembly task or twists a valve. We have proposed a force compliance methodology for a mobile, airborne platform containing a dexterous manipulator used for peg-in-hold tasks. A kinematic and dynamic model was built for this system and simulation results validate our kinematic model. In addition to hose insertion, other practical tasks such as door opening and valve turning can be investigated using this system. Our next phase is a hardware implementation of the aerial manipulator in an emulated test environment. REFERENCES [I] B. Hamner, S. C. Koterba, J. Shi, R. Simmons, and S. Singh, "An autonomous mobile manipulator for assembly tasks," Autonomous Robots, vol. 28, no. 1, pp , January [2] M. Orsag, C. Korpela, and P. Oh, "Modeling and control of MM-UAV: Mobile manipulating unmanned aerial vehicle," in Proc.lnternational Coriference on Unmanned Aircraft Systems, ICUAS, [3] A. Keemink, M. Fumagalli, S. Stramigioli, and R. Carloni, "Mechanical design of a manipulation system for unmanned aerial vehicles," in Robotics and Automation (ICRA), 2012 IEEE International Conference on, may 2012, pp [4] D. Mellinger, Q. Lindsey, M. Shomin, and Y. Kumar, "Design, modeling, estimation and control for aerial grasping and manipulation," in Proc. IEEEIRSJ Int Intelligent Robots and Systems (IROS) Conf, 2011, pp [5] P. E. 1. Pounds, D. R. Bersak, and A. M. Dollar, "Grasping from the air: Hovering capture and load stability," in Proc. IEEE Int Robotics and Automation (ICRA) Conf, 2011, pp [6] S. kook Yun, "Complaint manipulation for peg-in-hole: Is passive compliance a key to the contact motion?" in Proc. IEEE Int Robotics and Automation (ICRA) Cont [7] J. F. Broenink and M. L. J. Tiernego, "Peg-in-hole assembly using impedance control with a 6 dof robot," in Proceedings 8th European Simulation Symposium, [8] D. Strip, "Insertions using geometric analysis and hybrid force-position control: method and analysis," in Robotics and Automation, Proceedings., 1988 IEEE International Conference on, apr 1988, pp vol.3. [9] A. Bramwell, G. Done, and D. Balmford, Bramwell's helicopter dynamics. American Institute of Aeronautics and Astronautics, [10] J. Wu, Y. Fan, T. Feng, and Y. Chao, "Simplified model building of mini unmanned helicopter in hover," in Advanced Computer Control (ICACC), nd International Conference on, vol. 5, march 2010, pp [II] P. Pounds, R. Mahony, and P. Corke, "Modelling and control of a large quadrotor robot," Control Engineering Practice, vol. 18, no. 7, pp ,2010. [12] C. Korpela, M. Orsag, M. Pekala, and P. Oh, "Dynamic stability of a mobile manipulating unmanned aerial vehicle," in Proc. IEEE Int Robotics and Automation (ICRA) Cont. 2013, To Appear. [13] R. Mahony and R. Lozano, "(almost) exact path tracking control for an autonomous helicopter in hover manoeuvres," in Robotics and Automation, Proceedings. ICRA '00. IEEE International Coriference on, vol. 2, 2000, pp vol.2. [14] P. Pounds, D. Bersak, and A. Dollar, "Stability of small-scale uav helicopters and quadrotors with added payload mass under pid control," Autonomous Robots, vol. 33, pp , [15] S. Ghosh, Control Systems: Theory and Applications. Pearson Education Canada, [16] S. Bellens, J. De Schutter, and H. Bruyninckx, "A hybrid pose / wrench control framework for quadrotor helicopters," in Robotics and Automation (ICRA), 2012 IEEE International Conference on, may 2012, pp [17] L. Marconi, R. Naldi, and L. Gentili, "Modelling and control of a flying robot interacting with the environment," Automatica, vol. 47, no. 12, pp , Dec [18] R. Diankov and 1. Kuffner, "Openrave: A planning architecture for autonomous robotics," Robotics Institute, Pittsburgh, PA, Tech. Rep. CMU-RI-TR-08-34, July [19] B. Siciliano, L. Sciavicco, L. Villani, and G. Oriolo, Robotics: Modelling, Planning and Control, 1st ed. Springer Publishing Company, Incorporated, [20] P. Corke, "A robotics toolbox for MATLAB," IEEE Robotics and Automation Magazine, vol. 3, no. 1, pp , Mar [21] R. Jazar, Theory of Applied Robotics: Kinematics, Dynamics, and Control (2nd Edition). Springer, [22] N. Hogan, "Impedance control: An approach to manipulation," in Proc. American Control Cont. 1984, pp [23] F. Almeida, A. Lopes, and P. Abreu, "Force-impedance control: a new control strategy of robotic manipulators," Recent advances in Mechatronics, pp , [24] Y. Narli and P. Y. Oh, "Hardware-in-the-loop test rig to capture aerial robot and sensor suite performance metrics," in Proc. IEEEIRSJ Int Intelligent Robots and Systems Cont. 2006, pp