- Email: [email protected]
Guidance, Navigation, and Control for Docking of Two Cubic Blimps
20th IFAC Symposium on Automatic Control in Aerospace August 21-25, 2016. Sherbrooke, Quebec, Canada 20th IFAC Symposium on Control in 20th IFAC Symposium on Automatic Automatic Control in Aerospace Aerospace August 21-25, 2016. Sherbrooke, Quebec, Canada Available online at www.sciencedirect.com August 21-25, 2016. Sherbrooke, Quebec, Canada August 21-25, 2016. Sherbrooke, Quebec, Canada
ScienceDirect IFAC-PapersOnLine 49-17 (2016) 260–265 Guidance, Navigation, and Control Guidance, Navigation, and Control Guidance, Navigation, and Control Guidance, Navigation, and Blimps Control Docking of Two Cubic Docking of Two Cubic Blimps Docking of Two Cubic Blimps Docking of Two Cubic Blimps ∗ ∗
for for for for
Patrick Abouzakhm ∗ Inna Sharf ∗ Patrick Abouzakhm ∗∗ Inna Sharf ∗∗ Patrick Patrick Abouzakhm Abouzakhm Inna Inna Sharf Sharf ∗ ∗ Mechanical Engineering Department, McGill University, Montreal, Engineering Department, McGill University, Montreal, ∗ Mechanical Canada (e-mail: [email protected], ∗ Mechanical Engineering Department, Mechanical Engineering Department, McGill McGill University, University, Montreal, Montreal, Canada (e-mail: [email protected], [email protected]) Canada [email protected], Canada (e-mail: (e-mail: [email protected], [email protected]) [email protected]) [email protected]) Abstract: This paper focuses on the development of guidance, navigation and control for the Abstract: This paper focuses on the development guidance, navigation and control for the docking of two blimps, named Tryphons. The of chaser and target Tryphons are assumed to Abstract: This paper focuses on development of guidance, navigation and control for Abstract: Thiscubic paper focuses on the the development of guidance, navigation and are control for the the docking of two cubic blimps, named Tryphons. The chaser and target Tryphons assumed to be brought sufficiently close together through a rendezvous stage, where the docking stage then docking of two cubic blimps, named Tryphons. The chaser and target Tryphons are assumed to docking of two cubic blimps, named Tryphons. chaser and target Tryphons are assumed to be brought through aa The rendezvous where the docking stage then begins and sufficiently the target isclose kepttogether stationary. An ARtag fiducialstage, marker system used for position be sufficiently close together through stage, where the docking stage then be brought brought sufficiently close together through a rendezvous rendezvous stage, where the is docking stage then begins and the target is kept stationary. An ARtag fiducial marker system is used for position based servoing a glideslope guidance algorithm forsystem soft docking. beginsvisual and the the targetcontrol is kept keptwith stationary. An ARtag ARtag fiducial marker system is used usedThe for physical position begins and target is stationary. An fiducial marker is for position based visual servoing control with a glideslope guidance algorithm for soft docking. The physical docking of the Tryphons is achieved through contact of the protruder and receiver ends of the based visual servoing control with a glideslope guidance algorithm for soft docking. The physical based visual servoing control with a glideslope guidance algorithm for soft docking. The physical docking of the Tryphons is achieved through contact of the protruder and receiver ends of the two electromagnetic docking mechanisms. Simulations of the docking process are carried out in docking of the Tryphons is achieved through contact of the protruder and receiver ends of docking of the Tryphons is achieved through contact ofofthe protruder and receiver ends out of the the two electromagnetic docking mechanisms. Simulations the docking process are carried in the open source simulation software Gazebo, using Robot Operating System (ROS) and virtual two electromagnetic docking mechanisms. Simulations of the docking process are carried out two open electromagnetic dockingsoftware mechanisms. Simulations of the dockingSystem process(ROS) are carried out in in the source simulation Gazebo, using Robot Operating and virtual sensors. the open open source source simulation simulation software software Gazebo, Gazebo, using using Robot Robot Operating Operating System System (ROS) (ROS) and and virtual virtual the sensors. sensors. sensors. © 2016, IFAC (International Federation of Automatic Control) Hosting by Elsevier Ltd. All rights reserved. Keywords: Guidance systems, Navigation, Output feedback, Feedback control, Autonomous Keywords: Guidance systems, systems, Navigation, Output Output feedback, Feedback Feedback control, Autonomous Autonomous vehicles Keywords: Guidance Keywords: Guidance systems, Navigation, Navigation, Output feedback, feedback, Feedback control, control, Autonomous vehicles vehicles vehicles 1. INTRODUCTION was developed at MIT for testing autonomous docking 1. INTRODUCTION INTRODUCTION was developed developed at MIT MIT for testing autonomous docking algorithms in both 1-g for andtesting microgravity environments. 1. was at autonomous docking 1. INTRODUCTION was developed at MIT for testing autonomous docking algorithms in both 1-g and microgravity environments. Singla et al.in (2006) design anmicrogravity adaptive output feedback both 1-g and environments. Autonomous blimps are one of many aerial vehicles which algorithms algorithms in both 1-g and microgravity environments. Singla et etlaw, al. where (2006) the design an of adaptive output feedback Autonomous blimps are one one of of many many aerial vehicles vehicles which control effects bounded output errors al. (2006) design an adaptive output feedback are effective for low-speed applications, because of Singla Autonomous blimps are aerial which Singla etlaw, al. where (2006) the design an of adaptive output feedback control effects bounded output errors Autonomous blimps are oneflight of many aerial vehicles which are effective for low-speed flight applications, because of are analysed. Tournes et al. (2011) present a higher order control law, where the effects of bounded output errors the flight time, payload and manoeuvrability they provide. are effective for low-speed flight applications, because of control law, where theeteffects of bounded output errors are analysed. analysed. Tournes al. (2011) present a higher order are effective forpayload low-speed flight applications, because of are the flight time, and manoeuvrability they provide. sliding mode controller which uses a camera and light Tournes et al. (2011) present aa higher order The indoor cubic airships presented in this paper, seen the flight time, payload and manoeuvrability they provide. are analysed. Tournes et al. (2011) present higher order sliding mode controller which uses a camera and light the flight time, payload and manoeuvrability they provide. The indoor cubic airships presented in this paper, seen based relative navigation system for docking. For AUVs, sliding mode controller which uses a camera and light in Fig. 1 and referred to as Tryphons, werepaper, developed The indoor cubic airships presented in seen sliding mode controller which uses adocking. cameraFor andAUVs, light based relative navigation system for The indoor cubic airships presented in this this paper, seen based in Fig. 1 and referred to as Tryphons, were developed Lee etrelative al. (2002) and Park et al. (2009) use a AUVs, visual navigation system for docking. For with the vision of producing artistic performances and conin Fig. 1 and referred to as Tryphons, were developed based relative navigation system for docking. For AUVs, Lee et et al. al. (2002) (2002)approach and Park Park et et docking al. (2009) (2009) use visual in Fig. and of referred to as Tryphons, were developed with the1vision vision producing artistic performances and con- Lee servoing to use a stationary and al. aa structing floating structures artistic using the basic building block, with the of performances and conLee et al.control (2002)approach and Parkfor et docking al. (2009) a visual visual servoing control for to use a stationary stationary with the vision of producing producing artistic performances and con- servoing structing floating structures using the basic building block, target. For UAVs, Wu et al. (2013) use visual sensors to control approach for docking to a the brick.floating The Tryphon project is the a collaboration between structing structures using basic building block, servoing control approach for docking to a stationary target. For UAVs, Wu et al. (2013) use visual sensors to structing floating structures using the basic building block, the brick. The Tryphon project is a collaboration between obtain full-state feedback and dock to a probe and drogue target. For UAVs, Wu et al. (2013) use visual sensors to engineering and arts, and thus is addresses a between number target. the brick. Tryphon project aa collaboration For UAVs, Wu et and al. (2013) use visualand sensors to obtain full-state feedback dock to a probe drogue the brick. The The Tryphon project isalso collaboration between engineering and arts, and thus also addresses a number using afull-state LQR controller. Wilson etto al. (2015)and present a obtain feedback and dock aa probe drogue of engineering research topics, such as human robot inengineering and arts, and thus also addresses a number obtain full-state feedback and dock to probe and drogue using a LQR controller. Wilson et al. (2015) present a engineering andresearch arts, and thus such also as addresses arobot number of engineering engineering topics, human in- using complete solution to airborne docking using a vision-aided a LQR controller. Wilson et al. (2015) present a teraction, state estimation for unmanned aerial vehicles of research topics, such as human robot inusing a LQR controller. Wilson et al. (2015) present a complete solution to airborne docking using a vision-aided of engineering research topics, such as human robot interaction,aerobot state estimation estimation for unmanned and aerialswarming vehicles complete unscented Kalmanto for relative resulting solution airborne docking using (UAVs), behaviour for development, teraction, state unmanned aerial vehicles complete solution tofilter airborne docking navigation using aa vision-aided vision-aided unscented Kalman filter for relative navigation resulting teraction, state estimation for unmanned and aerialswarming vehicles unscented (UAVs), aerobot behaviour development, in successful experimental demonstrations. Typically, the Kalman filter for relative navigation resulting and assembly of multiple (St-Ongeand et al. 2015). unscented (UAVs), aerobot behaviour development, swarming Kalman filter for relative navigation resulting in successful experimental demonstrations. Typically, the (UAVs), aerobot behaviourblimps development, and swarming and assembly of multiple blimps (St-Onge et al. 2015). docking problem is solved using a mechanism which has in successful experimental demonstrations. Typically, the The focus of this paper is the assembly of Tryphons, i.e., and assembly of blimps (St-Onge et in successful experimental demonstrations. Typically, the docking problem is solved using a mechanism which has and assembly of multiple multiple blimps (St-Onge et al. al. 2015). 2015). The focus of this paper is the assembly of Tryphons, i.e., one physical contact point. problem is using the docking of two Tryphons with eachof other using i.e., two docking The focus of this paper is the assembly Tryphons, docking problem is solved solved using aa mechanism mechanism which which has has one physical contact point. The focus of this paper is the assembly of Tryphons, i.e., the docking docking of of two Tryphons with with each each other other using two two one physical contact point. electromagnetic protruder-receiver mechanisms. the Tryphons using one physical contact point. marker systems, such as ARtag are an effective vithe docking of two two Tryphons with docking each other using two Fiducial electromagnetic protruder-receiver docking mechanisms. Fiducial marker systems, such as as ARtag are are an effective viAn output feedback control law which uses relative navigaelectromagnetic protruder-receiver docking mechanisms. sion based navigation system. According to an Fiala (2005b), Fiducial marker systems, such ARtag effective vielectromagnetic protruder-receiver docking mechanisms. An output feedback control law which uses relative navigaFiducial marker systems, such as ARtag are an effective vision based navigation system. According to Fiala (2005b), tion, or more specifically, a position based visual servoing An output feedback control which uses navigaARtag hasnavigation reduced sensitivity to lighting variation and based system. to (2005b), An feedback controla law law whichbased uses relative relative naviga- sion tion,output or more more specifically, position visual servoing sion based navigation system. According According to Fiala Fiala (2005b), ARtag has reduced sensitivity to lighting variation and controller utilizing a fiducial marker system is designed. tion, or specifically, a position based visual servoing partial occlusion, andsensitivity uses quadrilateral outlines withand an has to variation tion, or more specifically, a position based visual servoing ARtag controller utilizing fiducial marker system is designed. designed. ARtag has reduced reduced to lighting lighting variation and partial occlusion, occlusion, andsensitivity usesthe quadrilateral outlines with an The controller mustaaa track themarker generated trajectories of a partial controller utilizing fiducial system is edge-based approach for unique feature stage of the and uses quadrilateral outlines with an controller utilizing fiducial marker system is designed. The controller controller must algorithm track the the under generated trajectories of of aa partial occlusion, and for usesthe quadrilateral outlines with an edge-based approach unique feature stage of the glideslope guidance the assumption The must track generated trajectories detection process, and a robust digital encoding method approach for the unique feature stage of the The controller must algorithm track the under generated trajectories of aa edge-based glideslope guidance the assumption of edge-based approach for the unique feature stage of the detection process, and a robust digital encoding method stationary, cooperative target. glideslope guidance algorithm under the assumption of a for the verification/identification stage.encoding Fiducial method marker process, glideslope algorithm stationary,guidance cooperative target. under the assumption of a detection detection process, and and aa robust robust digital digital for the the verification/identification verification/identification stage.encoding Fiducial method marker stationary, cooperative target. for stage. Fiducial marker stationary, cooperative target. Docking of two or more vehicles is a difficult task which re- for the verification/identification stage. Fiducial marker Docking of two or more vehicles is a difficult task which requires precision and robustness docking mechanism Docking of more vehicles is difficult task which Docking of two two or or more vehicles in is aathe difficult task which rerequires precision and robustness in the docking mechanism and guidance, navigation and control strategies used. The quires precision and robustness in the docking mechanism quires precision and robustness in the docking mechanism and guidance, guidance, navigationarises and control control strategies used. The docking task commonly in space satelliteused. missions, and navigation and strategies The and guidance, navigationarises and control strategies used. The docking task commonly in space satellite missions, autonomous underwater vehicle (AUV) missions, and undocking commonly in space missions, docking task task underwater commonly arises arises in(AUV) space satellite satellite missions, autonomous vehicle missions, and unmanned surface and aerial vehicle missions. In spacecraft autonomous underwater vehicle (AUV) missions, and unautonomous underwater vehicle (AUV) missions, and unmanned surface and aerial vehicle missions. In spacecraft missions, the strategies for vehicle dockingmissions. to a non-cooperative manned surface and In manned surface and aerial aerial vehicle missions. In spacecraft spacecraft missions, the strategies for docking to aa non-cooperative non-cooperative target are different and more challenging than to a comissions, the strategies for docking to missions, the strategies for docking to a non-cooperative target are different and more challenging than to where cooperative one. Nolet et al. (2004) treat the case target are different and more challenging than to aaa cotarget are one. different and more challenging than to where cooperative Nolet et al. (2004) treat the case the target one. is fully cooperative, and use guidoperative Nolet et treat the operative one. Nolet et al. al. (2004) (2004) treataa glideslope the case case where where the target is fully cooperative, and use glideslope guidance algorithm forcooperative, trajectory generation. The SPHERES the target is and guidthe is fully fully and use use aa glideslope glideslope guid- Fig. 1. Tryphons in artistic performance ancetarget algorithm forcooperative, trajectory generation. The SPHERES SPHERES testbed used to evaluate the glideslope guidance algorithm ance algorithm for trajectory generation. The ance algorithm for trajectory generation. The SPHERES Fig. 1. Tryphons in artistic performance testbed used to evaluate the glideslope guidance algorithm Fig. 1. Tryphons in artistic performance testbed testbed used used to to evaluate evaluate the the glideslope glideslope guidance guidance algorithm algorithm Fig. 1. Tryphons in artistic performance Copyright © 2016 IFAC 260 2405-8963 Copyright©©2016, 2016IFAC IFAC (International Federation of Automatic Control) 260Hosting by Elsevier Ltd. All rights reserved. Copyright © 2016 IFAC 260 Copyright 2016responsibility IFAC 260Control. Peer review ©under of International Federation of Automatic 10.1016/j.ifacol.2016.09.045
IFAC ACA 2016 August 21-25, 2016. Quebec, Canada Patrick Abouzakhm et al. / IFAC-PapersOnLine 49-17 (2016) 260–265
261
systems have been used as the navigation solution in several applications, including docking. Tsai (2012) studied the docking of an axel rover to the central module of the DuAxel rover, a platform developed by NASA’s Jet Propulsion Laboratory and the California Institute of Technology, where a fiducial marker system was used with successfully carried out experiments. Fiala (2005a) applies an ARtag system to accurately determine the relative pose for vision based spacecraft docking. An ARtag system was chosen for Tryphon docking due to the advantages stated above, considering occlusion and lighting variations are concerns in the indoor environments that Tryphons are flown in.
a cone-shaped plastic part with a metallic plate at the center. This cone shape helps guide the electromagnet towards the metallic plate, and has an outer diameter of 8.5 cm. Each Tryphon has one receiver and one protruder mounted on the same face of the cube, at the center of the bordering vertical trusses. The chaser Tryphon has two mounted Firefly MV USB cameras located within each of the docking mechanism trusses. The target Tryphon has counterpart ARtag markers located near each of its docking mechanism components. The receiver end located on the target Tryphon with its ARtag marker, and the protruder end of the chaser Tryphon with the mounted camera are shown in Fig. 2.
Noteworthy aspects of this research come from the necessity of docking the two Tryphons at two contact points, and thus using two cameras to generate two relative positions which are then used by an output feedback controller which tracks two different, but dependant, glideslope trajectories. This paper first describes the setup of the unique Tryphon blimps used for docking, followed by an overview of their dynamics. The guidance and navigation system, and controller used for docking are then developed, and finally simulation results demonstrating docking of two blimps under realistic modelling of sensor noise are presented and discussed.
Fig. 2. Docking mechanism with: receiver end with ARtag marker mounted on target Tryphon and protruder end with camera mounted on chaser Tryphon
2. TRYPHON BLIMP AND EQUIPMENT
3. TRYPHON DYNAMICS
Tryphons have been designed with an easily assembled structure for facilitated transport to artistic performances around the world. The blimp is composed of a cubic bladder with side length of 2.15 m which is inflated with helium gas, and an external truss structure made of carbon fiber rods and 3D printed parts. The structure maintains the cubic shape and supports all additional equipment such as the embedded micro-computer, docking mechanism, sensors, batteries, and actuators. 2.1 General Purpose Hardware
In order to first analyse the docking control problem for the two cubes, the case where a target is fixed under perfect closed-loop control is considered, as simulations show under 1 cm in error during regulation at a fixed position. The regulation of a single Tryphon under closed loop control has been achieved in Br`eches (2015), and the assumption of a stationary target is a significant step, albeit somewhat restrictive, towards solving the docking problem to a moving target. 3.1 Kinematics
The mounted electronics used on Tryphons must be lightweight and energy efficient to fit within the payload of the blimp and lengthen flight times. Each Tryphon blimp has an onboard Overo Firestorm, which is a Linuxbased microprocessor produced by Gumstix. The boards communicate with a ground-station laptop over a 5GHz network, and use Robot Operating System (ROS) software, allowing computationally intensive algorithms to be executed off-board. The Overo is connected to a Robovero expansion board, which adds USB ports and includes an IMU. Tryphons are actuated using eight propellers, which are located at the center of each of the four lower horizontal trusses, and four vertical trusses.
z B L
C
x
p BC
p c2 y
r2
Z
p t2
rc
p c1 r1
L Cameras
p t1
O X
Y L
ARtag sets
Chaser L
L
Target L
2.2 Docking Hardware For docking, additional equipment is needed on both the chaser and target Tryphons. The complete docking mechanism consists of four components: two receiver ends and two protruder ends. The components are designed using carbon fiber rods and 3D printed plastic parts to maintain the aesthetics of Tryphons, with the protruder end holding the electromagnet and the receiver end holding 261
Fig. 3. Chaser and target cube system Consider Fig. 3, where the chaser’s body-fixed frame origin is located at its center of mass, and the inertial frame origin is placed at the center of mass of the (stationary) target. The orientation of the chaser relative to the target can therefore be expressed using Tait-Bryan angles (Z-YX) with ψ, θ, and φ as yaw, pitch and roll respectively.
IFAC ACA 2016 262 August 21-25, 2016. Quebec, Canada Patrick Abouzakhm et al. / IFAC-PapersOnLine 49-17 (2016) 260–265
The rotation matrix R is defined as that transforming vector components from the inertial frame into the bodyfixed frame. The body frame angular velocity vector ω = [p, q, r]T of the chaser cube is related to Euler rates ϕ˙ = ˙ θ, ˙ ψ] ˙ T by ω = Sϕ, ˙ where: [φ, 1 0 − sin θ S = 0 cos φ cos θ sin φ (1) 0 − sin φ cos θ cos φ leading to the inverse relationship: φ˙ = p + q sin φ tan θ + r cos φ tan θ (2) θ˙ = q cos φ − r sin φ ˙ ψ= q sin φ sec θ + r cos φ sec θ The position vectors r1 and r2 are measured between the target and chaser cubes through the use of camera-ARtag pairs as described in section 2, and are thus obtained B in the body-fixed frame as rB 1 and r2 . More specifically, these vectors measure the relative position between the corresponding protruder and receiver ends of the docking mechanism. By vector addition (see Fig. 3), these vectors can be expressed in terms of chaser/target position rB C and constant component position vectors (in respective frames) of the cameras and ARtags: B B rB 1 = −rC − pc1 + Rpt1 (3) B B rB 2 = −rC − pc2 + Rpt2 = [x, y, z]T , pt1 = [ L2 , − L2 , 0]T , pt2 = where rB C L L L L L L T T B T [− 2 , − 2 , 0] , pB c1 = [ 2 , 2 , 0] , and pc2 = [− 2 , 2 , 0] . Here, pt1 and pt2 denote the constant vector components in the inertial frame and must be transformed to the chaser B body-fixed frame. Furthermore, rB 1 and r2 can be differentiated with respect to time to give the relative velocities r˙ B 1 and r˙ B 2 , where equation 2 is used to substitute the Euler rate terms that appear with the angular velocities p, q, and r. These equations will be used to model the output feedback of the system as described in section 3.3. 3.2 Equations of Motion The equations of motion are written, with the rotational equations taken about the center of mass and both translational and rotational equations expressed in the body-fixed frame, as: B B (mI3×3 + Am )(v˙ C + ω × vC ) = RFD + αRFG + FC × (J + AJ )ω ˙ = MD + p× BC RFD + pBC RFB + MC − τ I
(4)
B where vC = [u, v, w] is the velocity of the center of mass of the chaser Tryphon resolved in the body-fixed frame, with m, diagonal matrices J, Am and AJ representing the mass, matrix of inertia, added mass and added inertia respecB tively, and τ I = ω × (J + AJ )ω and ω × vC as the Coriolis and centripetal effects from expressing the dynamics in the body-fixed frame. Furthermore, FD = − 21 ρair Cd A|vC |vC 1 and MD = − 32 ρair Cd L5 |ω|ω (Br`eches 2015), are the drag force and drag moment respectively, where Cd is the drag coefficient, ρair is the density of air and A is the projected area of Tryphon normal to vC . FC and MC are the control forces and torques, α is the fractional difference between the buoyancy force FB and gravity force FG , with α = 0 implying neutral buoyancy, and p× BC is the cross product matrix associated with the position vector pointing from the centroid to the center of mass of the Tryphon. In the
262
ideal model, the center of mass is located directly under T the center of buoyancy, i.e., pB BC = [0, 0, lz ] , although minor variations are expected in the real system. All forces and moments above are expressed in the body-fixed frame, with the exception of FD , FG and FB . 3.3 State-Space Model The state-space model is now formulated and linearized to design a linear output feedback controller using the poleplacement technique. The measured variables include the measurements obtained from the camera-ARtag pairs and orientation sensing, and the output feedback consists of the estimated values from the state estimator. In first order form, the equations of motion are written as: x˙ = f (x, u) (5) B
B
where the state vector x = [(rC )T , ϕT , (vC )T , ω T ]T and input vector u = [FTC , MTC ]T , with f (x, u) as: B × B f (x, u) =
vC − ω rC S−1 ω
(6) 1 B (RFD + αRFG + FC ) − ω × vC m + Am × × −1 JJ (MD + pBC RFD + pBC RFB + MC − τ I )
and with JJ = J + AJ . The subset Sd of the kernel of f , i.e., satisfying f (x0 , u0 ) = 0, is chosen as the operating point for the linearization using Taylor series expansion: Sx = {[0, −L, 01×10 ]T } Sd = Sx × Su where (7) Su = {[0, 0, −αmg, 0, 0, 0]T }
representing the chaser cube in its docked position with the target. In order to make the linear model independent of u0 , α is taken as 0 meaning a neutrally buoyant cube. The linear state-space equation is then obtained: (8) x˙ δ = A0 xδ + B0 uδ with 03×3 03×3 I3×3 AL 0 0 −L 03×3 03×3 03×3 I3×3 (9) , AL = 0 0 0 A0 = 03×3 03×3 03×3 03×3 L 0 0 03×3 Aϕ 03×3 03×3 −mglz 0 0 2 Jxx + AJ + Am lz −mglz Aϕ = (10) 0 0 Jyy + AJ + Am lz2 0 0 0 and 03×3 03×3 03×3 03×3 B0 = I3×3 (11) 03×3 m + Am 03×3 J−1 J The measurement and output equations are obtained as: z = C0 xδ (12) ˆ 0 xδ y=C B
B
Here, xδ=x − x0 , uδ =u − u0 , z= [(r1 )T , (r2 )T , ϕT , ω T ]T , B B B B and y = [(r1 )T , (r2 )T , ϕT , (˙r1 )T , (˙r2 )T , ω T ]T represents the output feedback with state estimation. Here, matrix C0 is the Jacobian matrix of the output measurement, ˆ 0 accounts for estimation of velocities: while C
IFAC ACA 2016 August 21-25, 2016. Quebec, Canada Patrick Abouzakhm et al. / IFAC-PapersOnLine 49-17 (2016) 260–265
−I3×3 CA12 03×3 03×3 C 0 0 −I C0 = 3×3 A22 3×3 3×3 03×3 I3×3 03×3 03×3 03×3 03×3 03×3 I3×3 −I3×3 CA12 03×3 03×3 −I3×3 CA22 03×3 03×3 0 I 0 0 ˆ0 = C 03×3 03×3 −I3×3 C3×3 3×3 3×3 3×3 B12 03×3 03×3 −I3×3 CB 22 03×3 03×3 03×3 I3×3 0 0 −L 1 0 0 (−1)i L , i = 1, 2 CAi2 = 2 L (−1)i+1 L 0 0 0 L 1 0 0 (−1)i L , i = 1, 2 CBi2 = 2 −L (−1)i+1 L 0
(13)
(14)
(15)
(16)
These equations represent the linear dynamics model for the chaser cube with a fixed target, near the docked position, with the modelled output feedback. It can be shown that the model is both controllable and observable, and meets the sufficient condition for generic pole assignability of a minimal realization through static output feedback control (Syrmos et al.,1997): m+p≥n+1 (17) ˆ 0) where m (# of columns in B0 ) and p (# rows in C represent the size of the outputs and inputs respectively. This model can therefore be used to design a closedloop output feedback controller for docking using the pole placement technique, to be presented in section 5. 4. GUIDANCE AND NAVIGATION The full docking problem of Tryphon blimps has been broken down into two stages: rendezvous and docking. The rendezvous stage brings the chaser and target Tryphon close together to a predefined distance and orientation for the docking setup, i.e., where the dimensions of the ARtag markers are within the visible range of the cameras. In this stage, each Tryphon acts independently and uses the global localization and computed torque controller with waypoint guidance as described in Br`eches (2015) to manoeuvre to the desired pre-dock position. Once this stage is complete, the docking stage begins and the guidance, navigation and control systems of the chaser switch to the more well-suited methods, which are described in the following sections. The target Tryphon is assumed to remain in perfect regulation at the desired location throughout the docking process. 4.1 Sensing and State Estimation During the docking stage, the chaser Tryphon relies on various sensors and a state estimation algorithm to determine its pose and velocity relative to the target. As noted earlier, sensing includes the use of the two on-board cameras, which measure the relative position of the corresponding B ARtag markers to give vectors rB 1 and r2 as defined in section 3.1, the accelerometer and gyroscope measurements of the onboard IMU, and the compass bearing angle. The orientation of the target Tryphon is known through 263
263
its independent sensing; therefore, a relative orientation between chaser and target can be determined. The ROS package ”ar track alvar”, based on the open source ARtag tracking libraries, is used to extract the relative pose of the camera-ARtag pairs. A fiducial marker with dimensions 4×4 cm2 is chosen, with a visible range of approximately 6 cm − 100 cm with the equipped cameras and lenses. The package outputs a relative position and orientation, although the orientation measurements are not sufficiently accurate for docking purposes, and therefore the use of aforementioned independent orientation sensing (with the IMU and compass) is preferred. A standard implementation of a Kalman filter is used to obtain the velocity measurements r˙ B ˙B 1 and r 2 , and to filter all measurements for the output feedback used by the controller. 4.2 Glideslope Guidance Considering the target Tryphon is regulating at a desired position, and that the docking receiver’s cone is designed to allow for some misalignments by guiding the electromagnet, it is desirable to have low speeds during contact and little overshoot so as to not cause a rebound effect. Therefore, a trajectory tracking method which controls the final approach velocity for soft docking is ideal, and it is for this reason that a glideslope guidance algorithm has been chosen. Another advantage of the glideslope guidance algorithm is its reduction in control effort near the end of the docking stage, which for spacecraft, reduces the plume impingement on the target according to Hablani et al. (2002), and for the case of Tryphons, reduces the effects of propwash. According to the glideslope guidance formulation in Nolet et al. (2004), the algorithm consists of generating a straight line path from the initial location of the chaser to the target. Let ρ(t) be the distance-to-go and ρ(t) ˙ be the approach velocity of the chaser which decreases linearly with ρ(t). Specified parameters for the algorithm include the initial commanded velocity ρ˙ 0 < 0, and the final commanded arrival velocity ρ˙ T < 0, which combined with the initial distance-to-go, ρ0 , result in a glideslope a (< 0) and manoeuvre period T given as: ρ˙ 0 − ρ˙ T (18) a= ρ0 1 ρ˙ T (19) T = ln a ρ˙ 0 The glideslope guidance algorithm is now defined by the differential equation: (20) ρ(t) ˙ = aρ(t) + ρ˙ T whose solution is: ρ˙ T at ρ(t) = ρ0 eat + (e − 1) (21) a Following Hablani et al. (2002), the vectors corresponding to the trajectory to be tracked are defined as: ρ(t) ˙ = ρ(t)ˆ ˙ u (22) ρ(t) = ρ(t)ˆ u where u ˆ = [cos β1 cos β2 cos β3 ]T , and cos β1 , cos β2 and cos β3 are the direction cosines of ρ0 , with ρ0 = ||ρ0 ||. For the case of Tryphon docking, a novel approach is implemented where two glideslope trajectories are generated,
IFAC ACA 2016 264 August 21-25, 2016. Quebec, Canada Patrick Abouzakhm et al. / IFAC-PapersOnLine 49-17 (2016) 260–265
one between each of the receiver and protruder for each of the two docking mechanisms. These two desired trajectories are tracked by the control simultaneously, which reduces the relative distance and orientation between the chaser and target. Ideally, parameters should be chosen so that the manoeuvre periods for each glideslope trajectory are identical. To do this, the final commanded arrival velocities at the two docks are set equal (ρ˙ T 1 = ρ˙ T 2 ) to the value desired for soft docking, and the glideslope for the trajectory with the largest initial distance-to-go (ρ01 ), call this a1 , is chosen to give an appropriate T1 and ρ˙ 01 . Finally, the unknown value of a2 is determined for which the two manoeuvre periods are equal, by solving for the roots of the following equation, prior to the trajectory generation: T1 − T2 (a2 ) = 0 (23) where T2 (a2 ) is found by combining equations 18 and 19: 1 ρ˙ T ln (24) T2 (a2 ) = a2 a2 ρ02 + ρ˙ T This solution is determined once, at the beginning of the manoeuvre and ideally, ensures the same manoeuvre time for the two glideslope trajectories. 5. CONTROL r(t)
xδ
K
z
algorithm developed in Br`eches (2015) is implemented which determines the individual propeller commands to produce the desired control wrench. 6. SIMULATION RESULTS Experiments with Tryphons are performed during field trials, which are organized only a few times per year in a large enough space to support two or more of these flying cubes. For this reason, a detailed simulator, using the non-linear model, has been developed using Gazebo software, for testing and evaluation of guidance and control strategies, as well as state estimation. Advantages of Gazebo are its 3D rendering abilities, and its compatibility with ROS, which allows all algorithms used in simulation to be directly ported to the physical Tryphons. Virtual sensors can also be added in Gazebo with noise profiles similar to those of their real counterparts. The parameters used in the simulation environment for the docking of two Tryphons are given in table 1. Table 1. Tryphon system parameters m (kg)
L (m)
lz (m)
a1
ρ˙ T ) (m s
Tmax (N)
Cd
11.7
2.25
-0.17
-0.05
-0.02
0.63
1.74
J (kg · m2 )
Jx = 15.2 Jy = 15.3 Jz = 16
y
Fig. 4. Block diagram of output feedback control Using the state-space model derived in section 3.3, a closed loop, output feedback controller, u = −Ky, is designed using the pole placement technique; the block diagram of the controller is included in Fig. 4. For docking purposes, a controller with gain matrix K which docks the chaser Tryphon with the target with as little overshoot as possible is desired. For this reason, all closed loop poles of the system are chosen to be critically damped. Furthermore, the Tryphon’s propellers have a relatively low maximum thrust (Tmax ), and therefore gains which do not demand control inputs exceeding these values while tracking the glideslope trajectory are ideal. Using the pole placement technique on the state-space model, leads to an intermediate gain matrix K , which corresponds to the state feedback gain matrix. The output feedback gain matrix K is found using the generalized matrix inverse following the approach in G¨ uney and Atasoy (2011), where: †
ˆ0 K = K C
(25)
ˆ† C 0
represents the Moore-Penrose pseudoinverse of the and ˆ 0. state estimation matrix C The closed-loop, output feedback controller formulated above has been designed with the assumption of neutral buoyancy, since the actual difference between the buoyancy and gravity forces is expected to be small, however, unknown and time-varying. The system, state and output feedback gain matrix are augmented by adding an integral action term in z to compensate for the difference between the weight and buoyancy forces. The force distribution 264
Am (kg) Amx = 11.3 Amy = 11.3 Amz = 11.3
AJ (kg · m2 )
AJx = 0.2 AJy = 0.2 AJz = 0.2
For docking simulations, the target Tryphon is stationary, neutral buoyancy is assumed and the Kalman filter described in section 4.1 is used. The chaser Tryphon starts by regulating under output feedback control at a desired output of y = [.2, 1, .1, .2, 1, .1, 01×12 ]T , simulating the completion of the rendezvous stage at a 1 m distance, with 20% and 10% misalignments in lateral and vertical directions, respectively. Furthermore a small, random disturbance in orientation is applied on the chaser just before docking begins to observe the robustness in the control and glideslope guidance algorithm. The docking stage then begins, where the chaser tracks the glideslope trajectories. It is important to note that the simulation does not emulate the actual docking, i.e., electromagnet contacting the metallic plate. Fig. 5 shows the results of a simulated docking, where the vector components of B the estimated, desired and error values of rB 1 and r2 are given, as well as the relative orientation of the chaser. The manoeuvre period for the glideslope trajectory was 26.4 s, and the results clearly show that for both trajectories the manoeuvre times are indeed the same. Furthermore, all position error terms near the end of the trajectory are less than 4.25 cm, which falls within the area of the coneshaped receiver, thereby indicating that a successful dock is feasible. The initial orientation disturbances are also attenuated to near zero. 7. CONCLUSION The Tryphons are unique aerial vehicles with many applications in art and engineering. The topics that have been
IFAC ACA 2016 Patrick Abouzakhm et al. / IFAC-PapersOnLine 49-17 (2016) 260–265 August 21-25, 2016. Quebec, Canada
0.25
0.15
Coordinate x2 (m)
Coordinate x1 (m)
0.25
estimated desired error
0.2
0.1 0.05
0.15 0.1
REFERENCES
0
−0.05
−0.05 0
10 20 Time (s)
30
1
0
10 20 Time (s)
30
0
10 20 Time (s)
30
0
10 20 Time (s)
30
1 Coordinate y2 (m)
Coordinate y1 (m)
the Fonds Qu´eb´ecois de Recherche sur la Nature et les Technologies(FQRNT).
0.05
0
0.8 0.6 0.4 0.2 0
0.8 0.6 0.4 0.2 0
0
10 20 Time (s)
30
0.1
Coordinate z2 (m)
Coordinate z1 (m)
estimated desired error
0.2
0.05
0
−0.05
0.1
0.05
0
−0.05 0
10 20 Time (s)
30
(a)
(b)
Angle (degrees)
1 0 −1 −2
Roll Pitch Yaw
−3 0
5
10
15 Time (s)
265
20
25
30
(c)
Fig. 5. Simulation results showing estimated, desired and B error vector components of (a) rB 1 and (b) r2 , and the (c) relative orientation of the chaser covered in this paper are the guidance, navigation and control for the docking of two Tryphons. A dynamics model, output feedback visual servoing controller and glideslope guidance algorithm have been presented. The docking mechanism presented consists of a unique electromagnetic system, which requires the contact of two protruder ends with their corresponding receivers. The navigation system described utilizes a fiduciary marker system to obtain the relative positions between the protruders and receivers, i.e., the chaser and target. A previously developed glideslope guidance algorithm was successfully extended to the scenario where simultaneous docking at two physical locations is required. Simulations are carried out with a stationary target and demonstrate feasibility of docking. Future work will focus on experimental demonstrations and generalization of the methodology to docking to a moving target. ACKNOWLEDGEMENTS This work was supported by the National Sciences and Engineering research Council of Canada(NSERC) and 265
Br`eches, P.Y. (2015). Dynamics Modeling and State Feedback Control of a Lighter-than-air Cubic Blimp. Master’s thesis, McGill University, Canada. Fiala, M. (2005a). ARTag Fiducial Marker System Applied to Vision Based Spacecraft Docking. Shaw Conference Centre, Edmonton, Alberta, Canada. 35–40. Fiala, M. (2005b). Comparing ARTag and ARToolkit Plus fiducial marker systems. In IEEE International Workshop on Haptic Audio Visual Environments and their Applications, 2005, 148–153. G¨ uney, S. and Atasoy, A. (2011). An approach to pole placement method with output feedback. Hablani, H.B., Tapper, M.L., and Dana-Bashian, D.J. (2002). Guidance and Relative Navigation for Autonomous Rendezvous in a Circular Orbit. Journal of Guidance, Control, and Dynamics, 25(3), 553–562. Lee, P.M., Jeon, B.H., and Lee, C.M. (2002). A docking and control system for an autonomous underwater vehicle. 3, 1609–1614. Nolet, S., Kong, E., and Miller, D.W. (2004). Autonomous docking algorithm development and experimentation using the SPHERES testbed. 5419, 1–15. Park, J.Y., Jun, B.h., Lee, P.m., and Oh, J. (2009). Experiments on vision guided docking of an autonomous underwater vehicle using one camera. Ocean Engineering, 36(1), 48–61. Singla, P., Subbarao, K., and Junkins, J.L. (2006). Adaptive Output Feedback Control for Spacecraft Rendezvous and Docking Under Measurement Uncertainty. Journal of Guidance, Control, and Dynamics, 29(4), 892–902. St-Onge, D., Gosselin, C., and Reeves, N. (2015). Dynamic modelling and control of a cubic flying blimp using external motion capture. Proceedings of the Institution of Mechanical Engineers, Part I: Journal of Systems and Control Engineering, 229(10), 970–982. Syrmos, V.L., Abdallah, C.T., Dorato, P., and Grigoriadis, K. (1997). Static output feedbackA survey. Automatica, 33(2), 125–137. Tournes, C., Shtessel, Y., and Forem, D. (2011). Automatic Space Rendezvous and Docking using Second Order Sliding Mode Control. In A. Bartoszewicz (ed.), Sliding Mode Control, 307–330. InTech. Tsai, D. (2012). Autonomous Vision-Based Docking of the Tethered Axel Rover for Planetary Exploration. Master’s thesis, Lule University of Technology. Wilson, D.B., Gktogan, A.H., and Sukkarieh, S. (2015). Guidance and Navigation for UAV Airborne Docking. In Robotics: Science and Systems. Wu, S., Zhang, L., Xu, W., Zhou, T., and Luo, D. (2013). Docking control of autonomous aerial refueling for UAV based on LQR. In 2013 10th IEEE International Conference on Control and Automation (ICCA), 1819– 1823.
ScienceDirect IFAC-PapersOnLine 49-17 (2016) 260–265 Guidance, Navigation, and Control Guidance, Navigation, and Control Guidance, Navigation, and Control Guidance, Navigation, and Blimps Control Docking of Two Cubic Docking of Two Cubic Blimps Docking of Two Cubic Blimps Docking of Two Cubic Blimps ∗ ∗
for for for for
Patrick Abouzakhm ∗ Inna Sharf ∗ Patrick Abouzakhm ∗∗ Inna Sharf ∗∗ Patrick Patrick Abouzakhm Abouzakhm Inna Inna Sharf Sharf ∗ ∗ Mechanical Engineering Department, McGill University, Montreal, Engineering Department, McGill University, Montreal, ∗ Mechanical Canada (e-mail: [email protected], ∗ Mechanical Engineering Department, Mechanical Engineering Department, McGill McGill University, University, Montreal, Montreal, Canada (e-mail: [email protected], [email protected]) Canada [email protected], Canada (e-mail: (e-mail: [email protected], [email protected]) [email protected]) [email protected]) Abstract: This paper focuses on the development of guidance, navigation and control for the Abstract: This paper focuses on the development guidance, navigation and control for the docking of two blimps, named Tryphons. The of chaser and target Tryphons are assumed to Abstract: This paper focuses on development of guidance, navigation and control for Abstract: Thiscubic paper focuses on the the development of guidance, navigation and are control for the the docking of two cubic blimps, named Tryphons. The chaser and target Tryphons assumed to be brought sufficiently close together through a rendezvous stage, where the docking stage then docking of two cubic blimps, named Tryphons. The chaser and target Tryphons are assumed to docking of two cubic blimps, named Tryphons. chaser and target Tryphons are assumed to be brought through aa The rendezvous where the docking stage then begins and sufficiently the target isclose kepttogether stationary. An ARtag fiducialstage, marker system used for position be sufficiently close together through stage, where the docking stage then be brought brought sufficiently close together through a rendezvous rendezvous stage, where the is docking stage then begins and the target is kept stationary. An ARtag fiducial marker system is used for position based servoing a glideslope guidance algorithm forsystem soft docking. beginsvisual and the the targetcontrol is kept keptwith stationary. An ARtag ARtag fiducial marker system is used usedThe for physical position begins and target is stationary. An fiducial marker is for position based visual servoing control with a glideslope guidance algorithm for soft docking. The physical docking of the Tryphons is achieved through contact of the protruder and receiver ends of the based visual servoing control with a glideslope guidance algorithm for soft docking. The physical based visual servoing control with a glideslope guidance algorithm for soft docking. The physical docking of the Tryphons is achieved through contact of the protruder and receiver ends of the two electromagnetic docking mechanisms. Simulations of the docking process are carried out in docking of the Tryphons is achieved through contact of the protruder and receiver ends of docking of the Tryphons is achieved through contact ofofthe protruder and receiver ends out of the the two electromagnetic docking mechanisms. Simulations the docking process are carried in the open source simulation software Gazebo, using Robot Operating System (ROS) and virtual two electromagnetic docking mechanisms. Simulations of the docking process are carried out two open electromagnetic dockingsoftware mechanisms. Simulations of the dockingSystem process(ROS) are carried out in in the source simulation Gazebo, using Robot Operating and virtual sensors. the open open source source simulation simulation software software Gazebo, Gazebo, using using Robot Robot Operating Operating System System (ROS) (ROS) and and virtual virtual the sensors. sensors. sensors. © 2016, IFAC (International Federation of Automatic Control) Hosting by Elsevier Ltd. All rights reserved. Keywords: Guidance systems, Navigation, Output feedback, Feedback control, Autonomous Keywords: Guidance systems, systems, Navigation, Output Output feedback, Feedback Feedback control, Autonomous Autonomous vehicles Keywords: Guidance Keywords: Guidance systems, Navigation, Navigation, Output feedback, feedback, Feedback control, control, Autonomous vehicles vehicles vehicles 1. INTRODUCTION was developed at MIT for testing autonomous docking 1. INTRODUCTION INTRODUCTION was developed developed at MIT MIT for testing autonomous docking algorithms in both 1-g for andtesting microgravity environments. 1. was at autonomous docking 1. INTRODUCTION was developed at MIT for testing autonomous docking algorithms in both 1-g and microgravity environments. Singla et al.in (2006) design anmicrogravity adaptive output feedback both 1-g and environments. Autonomous blimps are one of many aerial vehicles which algorithms algorithms in both 1-g and microgravity environments. Singla et etlaw, al. where (2006) the design an of adaptive output feedback Autonomous blimps are one one of of many many aerial vehicles vehicles which control effects bounded output errors al. (2006) design an adaptive output feedback are effective for low-speed applications, because of Singla Autonomous blimps are aerial which Singla etlaw, al. where (2006) the design an of adaptive output feedback control effects bounded output errors Autonomous blimps are oneflight of many aerial vehicles which are effective for low-speed flight applications, because of are analysed. Tournes et al. (2011) present a higher order control law, where the effects of bounded output errors the flight time, payload and manoeuvrability they provide. are effective for low-speed flight applications, because of control law, where theeteffects of bounded output errors are analysed. analysed. Tournes al. (2011) present a higher order are effective forpayload low-speed flight applications, because of are the flight time, and manoeuvrability they provide. sliding mode controller which uses a camera and light Tournes et al. (2011) present aa higher order The indoor cubic airships presented in this paper, seen the flight time, payload and manoeuvrability they provide. are analysed. Tournes et al. (2011) present higher order sliding mode controller which uses a camera and light the flight time, payload and manoeuvrability they provide. The indoor cubic airships presented in this paper, seen based relative navigation system for docking. For AUVs, sliding mode controller which uses a camera and light in Fig. 1 and referred to as Tryphons, werepaper, developed The indoor cubic airships presented in seen sliding mode controller which uses adocking. cameraFor andAUVs, light based relative navigation system for The indoor cubic airships presented in this this paper, seen based in Fig. 1 and referred to as Tryphons, were developed Lee etrelative al. (2002) and Park et al. (2009) use a AUVs, visual navigation system for docking. For with the vision of producing artistic performances and conin Fig. 1 and referred to as Tryphons, were developed based relative navigation system for docking. For AUVs, Lee et et al. al. (2002) (2002)approach and Park Park et et docking al. (2009) (2009) use visual in Fig. and of referred to as Tryphons, were developed with the1vision vision producing artistic performances and con- Lee servoing to use a stationary and al. aa structing floating structures artistic using the basic building block, with the of performances and conLee et al.control (2002)approach and Parkfor et docking al. (2009) a visual visual servoing control for to use a stationary stationary with the vision of producing producing artistic performances and con- servoing structing floating structures using the basic building block, target. For UAVs, Wu et al. (2013) use visual sensors to control approach for docking to a the brick.floating The Tryphon project is the a collaboration between structing structures using basic building block, servoing control approach for docking to a stationary target. For UAVs, Wu et al. (2013) use visual sensors to structing floating structures using the basic building block, the brick. The Tryphon project is a collaboration between obtain full-state feedback and dock to a probe and drogue target. For UAVs, Wu et al. (2013) use visual sensors to engineering and arts, and thus is addresses a between number target. the brick. Tryphon project aa collaboration For UAVs, Wu et and al. (2013) use visualand sensors to obtain full-state feedback dock to a probe drogue the brick. The The Tryphon project isalso collaboration between engineering and arts, and thus also addresses a number using afull-state LQR controller. Wilson etto al. (2015)and present a obtain feedback and dock aa probe drogue of engineering research topics, such as human robot inengineering and arts, and thus also addresses a number obtain full-state feedback and dock to probe and drogue using a LQR controller. Wilson et al. (2015) present a engineering andresearch arts, and thus such also as addresses arobot number of engineering engineering topics, human in- using complete solution to airborne docking using a vision-aided a LQR controller. Wilson et al. (2015) present a teraction, state estimation for unmanned aerial vehicles of research topics, such as human robot inusing a LQR controller. Wilson et al. (2015) present a complete solution to airborne docking using a vision-aided of engineering research topics, such as human robot interaction,aerobot state estimation estimation for unmanned and aerialswarming vehicles complete unscented Kalmanto for relative resulting solution airborne docking using (UAVs), behaviour for development, teraction, state unmanned aerial vehicles complete solution tofilter airborne docking navigation using aa vision-aided vision-aided unscented Kalman filter for relative navigation resulting teraction, state estimation for unmanned and aerialswarming vehicles unscented (UAVs), aerobot behaviour development, in successful experimental demonstrations. Typically, the Kalman filter for relative navigation resulting and assembly of multiple (St-Ongeand et al. 2015). unscented (UAVs), aerobot behaviour development, swarming Kalman filter for relative navigation resulting in successful experimental demonstrations. Typically, the (UAVs), aerobot behaviourblimps development, and swarming and assembly of multiple blimps (St-Onge et al. 2015). docking problem is solved using a mechanism which has in successful experimental demonstrations. Typically, the The focus of this paper is the assembly of Tryphons, i.e., and assembly of blimps (St-Onge et in successful experimental demonstrations. Typically, the docking problem is solved using a mechanism which has and assembly of multiple multiple blimps (St-Onge et al. al. 2015). 2015). The focus of this paper is the assembly of Tryphons, i.e., one physical contact point. problem is using the docking of two Tryphons with eachof other using i.e., two docking The focus of this paper is the assembly Tryphons, docking problem is solved solved using aa mechanism mechanism which which has has one physical contact point. The focus of this paper is the assembly of Tryphons, i.e., the docking docking of of two Tryphons with with each each other other using two two one physical contact point. electromagnetic protruder-receiver mechanisms. the Tryphons using one physical contact point. marker systems, such as ARtag are an effective vithe docking of two two Tryphons with docking each other using two Fiducial electromagnetic protruder-receiver docking mechanisms. Fiducial marker systems, such as as ARtag are are an effective viAn output feedback control law which uses relative navigaelectromagnetic protruder-receiver docking mechanisms. sion based navigation system. According to an Fiala (2005b), Fiducial marker systems, such ARtag effective vielectromagnetic protruder-receiver docking mechanisms. An output feedback control law which uses relative navigaFiducial marker systems, such as ARtag are an effective vision based navigation system. According to Fiala (2005b), tion, or more specifically, a position based visual servoing An output feedback control which uses navigaARtag hasnavigation reduced sensitivity to lighting variation and based system. to (2005b), An feedback controla law law whichbased uses relative relative naviga- sion tion,output or more more specifically, position visual servoing sion based navigation system. According According to Fiala Fiala (2005b), ARtag has reduced sensitivity to lighting variation and controller utilizing a fiducial marker system is designed. tion, or specifically, a position based visual servoing partial occlusion, andsensitivity uses quadrilateral outlines withand an has to variation tion, or more specifically, a position based visual servoing ARtag controller utilizing fiducial marker system is designed. designed. ARtag has reduced reduced to lighting lighting variation and partial occlusion, occlusion, andsensitivity usesthe quadrilateral outlines with an The controller mustaaa track themarker generated trajectories of a partial controller utilizing fiducial system is edge-based approach for unique feature stage of the and uses quadrilateral outlines with an controller utilizing fiducial marker system is designed. The controller controller must algorithm track the the under generated trajectories of of aa partial occlusion, and for usesthe quadrilateral outlines with an edge-based approach unique feature stage of the glideslope guidance the assumption The must track generated trajectories detection process, and a robust digital encoding method approach for the unique feature stage of the The controller must algorithm track the under generated trajectories of aa edge-based glideslope guidance the assumption of edge-based approach for the unique feature stage of the detection process, and a robust digital encoding method stationary, cooperative target. glideslope guidance algorithm under the assumption of a for the verification/identification stage.encoding Fiducial method marker process, glideslope algorithm stationary,guidance cooperative target. under the assumption of a detection detection process, and and aa robust robust digital digital for the the verification/identification verification/identification stage.encoding Fiducial method marker stationary, cooperative target. for stage. Fiducial marker stationary, cooperative target. Docking of two or more vehicles is a difficult task which re- for the verification/identification stage. Fiducial marker Docking of two or more vehicles is a difficult task which requires precision and robustness docking mechanism Docking of more vehicles is difficult task which Docking of two two or or more vehicles in is aathe difficult task which rerequires precision and robustness in the docking mechanism and guidance, navigation and control strategies used. The quires precision and robustness in the docking mechanism quires precision and robustness in the docking mechanism and guidance, guidance, navigationarises and control control strategies used. The docking task commonly in space satelliteused. missions, and navigation and strategies The and guidance, navigationarises and control strategies used. The docking task commonly in space satellite missions, autonomous underwater vehicle (AUV) missions, and undocking commonly in space missions, docking task task underwater commonly arises arises in(AUV) space satellite satellite missions, autonomous vehicle missions, and unmanned surface and aerial vehicle missions. In spacecraft autonomous underwater vehicle (AUV) missions, and unautonomous underwater vehicle (AUV) missions, and unmanned surface and aerial vehicle missions. In spacecraft missions, the strategies for vehicle dockingmissions. to a non-cooperative manned surface and In manned surface and aerial aerial vehicle missions. In spacecraft spacecraft missions, the strategies for docking to aa non-cooperative non-cooperative target are different and more challenging than to a comissions, the strategies for docking to missions, the strategies for docking to a non-cooperative target are different and more challenging than to where cooperative one. Nolet et al. (2004) treat the case target are different and more challenging than to aaa cotarget are one. different and more challenging than to where cooperative Nolet et al. (2004) treat the case the target one. is fully cooperative, and use guidoperative Nolet et treat the operative one. Nolet et al. al. (2004) (2004) treataa glideslope the case case where where the target is fully cooperative, and use glideslope guidance algorithm forcooperative, trajectory generation. The SPHERES the target is and guidthe is fully fully and use use aa glideslope glideslope guid- Fig. 1. Tryphons in artistic performance ancetarget algorithm forcooperative, trajectory generation. The SPHERES SPHERES testbed used to evaluate the glideslope guidance algorithm ance algorithm for trajectory generation. The ance algorithm for trajectory generation. The SPHERES Fig. 1. Tryphons in artistic performance testbed used to evaluate the glideslope guidance algorithm Fig. 1. Tryphons in artistic performance testbed testbed used used to to evaluate evaluate the the glideslope glideslope guidance guidance algorithm algorithm Fig. 1. Tryphons in artistic performance Copyright © 2016 IFAC 260 2405-8963 Copyright©©2016, 2016IFAC IFAC (International Federation of Automatic Control) 260Hosting by Elsevier Ltd. All rights reserved. Copyright © 2016 IFAC 260 Copyright 2016responsibility IFAC 260Control. Peer review ©under of International Federation of Automatic 10.1016/j.ifacol.2016.09.045
IFAC ACA 2016 August 21-25, 2016. Quebec, Canada Patrick Abouzakhm et al. / IFAC-PapersOnLine 49-17 (2016) 260–265
261
systems have been used as the navigation solution in several applications, including docking. Tsai (2012) studied the docking of an axel rover to the central module of the DuAxel rover, a platform developed by NASA’s Jet Propulsion Laboratory and the California Institute of Technology, where a fiducial marker system was used with successfully carried out experiments. Fiala (2005a) applies an ARtag system to accurately determine the relative pose for vision based spacecraft docking. An ARtag system was chosen for Tryphon docking due to the advantages stated above, considering occlusion and lighting variations are concerns in the indoor environments that Tryphons are flown in.
a cone-shaped plastic part with a metallic plate at the center. This cone shape helps guide the electromagnet towards the metallic plate, and has an outer diameter of 8.5 cm. Each Tryphon has one receiver and one protruder mounted on the same face of the cube, at the center of the bordering vertical trusses. The chaser Tryphon has two mounted Firefly MV USB cameras located within each of the docking mechanism trusses. The target Tryphon has counterpart ARtag markers located near each of its docking mechanism components. The receiver end located on the target Tryphon with its ARtag marker, and the protruder end of the chaser Tryphon with the mounted camera are shown in Fig. 2.
Noteworthy aspects of this research come from the necessity of docking the two Tryphons at two contact points, and thus using two cameras to generate two relative positions which are then used by an output feedback controller which tracks two different, but dependant, glideslope trajectories. This paper first describes the setup of the unique Tryphon blimps used for docking, followed by an overview of their dynamics. The guidance and navigation system, and controller used for docking are then developed, and finally simulation results demonstrating docking of two blimps under realistic modelling of sensor noise are presented and discussed.
Fig. 2. Docking mechanism with: receiver end with ARtag marker mounted on target Tryphon and protruder end with camera mounted on chaser Tryphon
2. TRYPHON BLIMP AND EQUIPMENT
3. TRYPHON DYNAMICS
Tryphons have been designed with an easily assembled structure for facilitated transport to artistic performances around the world. The blimp is composed of a cubic bladder with side length of 2.15 m which is inflated with helium gas, and an external truss structure made of carbon fiber rods and 3D printed parts. The structure maintains the cubic shape and supports all additional equipment such as the embedded micro-computer, docking mechanism, sensors, batteries, and actuators. 2.1 General Purpose Hardware
In order to first analyse the docking control problem for the two cubes, the case where a target is fixed under perfect closed-loop control is considered, as simulations show under 1 cm in error during regulation at a fixed position. The regulation of a single Tryphon under closed loop control has been achieved in Br`eches (2015), and the assumption of a stationary target is a significant step, albeit somewhat restrictive, towards solving the docking problem to a moving target. 3.1 Kinematics
The mounted electronics used on Tryphons must be lightweight and energy efficient to fit within the payload of the blimp and lengthen flight times. Each Tryphon blimp has an onboard Overo Firestorm, which is a Linuxbased microprocessor produced by Gumstix. The boards communicate with a ground-station laptop over a 5GHz network, and use Robot Operating System (ROS) software, allowing computationally intensive algorithms to be executed off-board. The Overo is connected to a Robovero expansion board, which adds USB ports and includes an IMU. Tryphons are actuated using eight propellers, which are located at the center of each of the four lower horizontal trusses, and four vertical trusses.
z B L
C
x
p BC
p c2 y
r2
Z
p t2
rc
p c1 r1
L Cameras
p t1
O X
Y L
ARtag sets
Chaser L
L
Target L
2.2 Docking Hardware For docking, additional equipment is needed on both the chaser and target Tryphons. The complete docking mechanism consists of four components: two receiver ends and two protruder ends. The components are designed using carbon fiber rods and 3D printed plastic parts to maintain the aesthetics of Tryphons, with the protruder end holding the electromagnet and the receiver end holding 261
Fig. 3. Chaser and target cube system Consider Fig. 3, where the chaser’s body-fixed frame origin is located at its center of mass, and the inertial frame origin is placed at the center of mass of the (stationary) target. The orientation of the chaser relative to the target can therefore be expressed using Tait-Bryan angles (Z-YX) with ψ, θ, and φ as yaw, pitch and roll respectively.
IFAC ACA 2016 262 August 21-25, 2016. Quebec, Canada Patrick Abouzakhm et al. / IFAC-PapersOnLine 49-17 (2016) 260–265
The rotation matrix R is defined as that transforming vector components from the inertial frame into the bodyfixed frame. The body frame angular velocity vector ω = [p, q, r]T of the chaser cube is related to Euler rates ϕ˙ = ˙ θ, ˙ ψ] ˙ T by ω = Sϕ, ˙ where: [φ, 1 0 − sin θ S = 0 cos φ cos θ sin φ (1) 0 − sin φ cos θ cos φ leading to the inverse relationship: φ˙ = p + q sin φ tan θ + r cos φ tan θ (2) θ˙ = q cos φ − r sin φ ˙ ψ= q sin φ sec θ + r cos φ sec θ The position vectors r1 and r2 are measured between the target and chaser cubes through the use of camera-ARtag pairs as described in section 2, and are thus obtained B in the body-fixed frame as rB 1 and r2 . More specifically, these vectors measure the relative position between the corresponding protruder and receiver ends of the docking mechanism. By vector addition (see Fig. 3), these vectors can be expressed in terms of chaser/target position rB C and constant component position vectors (in respective frames) of the cameras and ARtags: B B rB 1 = −rC − pc1 + Rpt1 (3) B B rB 2 = −rC − pc2 + Rpt2 = [x, y, z]T , pt1 = [ L2 , − L2 , 0]T , pt2 = where rB C L L L L L L T T B T [− 2 , − 2 , 0] , pB c1 = [ 2 , 2 , 0] , and pc2 = [− 2 , 2 , 0] . Here, pt1 and pt2 denote the constant vector components in the inertial frame and must be transformed to the chaser B body-fixed frame. Furthermore, rB 1 and r2 can be differentiated with respect to time to give the relative velocities r˙ B 1 and r˙ B 2 , where equation 2 is used to substitute the Euler rate terms that appear with the angular velocities p, q, and r. These equations will be used to model the output feedback of the system as described in section 3.3. 3.2 Equations of Motion The equations of motion are written, with the rotational equations taken about the center of mass and both translational and rotational equations expressed in the body-fixed frame, as: B B (mI3×3 + Am )(v˙ C + ω × vC ) = RFD + αRFG + FC × (J + AJ )ω ˙ = MD + p× BC RFD + pBC RFB + MC − τ I
(4)
B where vC = [u, v, w] is the velocity of the center of mass of the chaser Tryphon resolved in the body-fixed frame, with m, diagonal matrices J, Am and AJ representing the mass, matrix of inertia, added mass and added inertia respecB tively, and τ I = ω × (J + AJ )ω and ω × vC as the Coriolis and centripetal effects from expressing the dynamics in the body-fixed frame. Furthermore, FD = − 21 ρair Cd A|vC |vC 1 and MD = − 32 ρair Cd L5 |ω|ω (Br`eches 2015), are the drag force and drag moment respectively, where Cd is the drag coefficient, ρair is the density of air and A is the projected area of Tryphon normal to vC . FC and MC are the control forces and torques, α is the fractional difference between the buoyancy force FB and gravity force FG , with α = 0 implying neutral buoyancy, and p× BC is the cross product matrix associated with the position vector pointing from the centroid to the center of mass of the Tryphon. In the
262
ideal model, the center of mass is located directly under T the center of buoyancy, i.e., pB BC = [0, 0, lz ] , although minor variations are expected in the real system. All forces and moments above are expressed in the body-fixed frame, with the exception of FD , FG and FB . 3.3 State-Space Model The state-space model is now formulated and linearized to design a linear output feedback controller using the poleplacement technique. The measured variables include the measurements obtained from the camera-ARtag pairs and orientation sensing, and the output feedback consists of the estimated values from the state estimator. In first order form, the equations of motion are written as: x˙ = f (x, u) (5) B
B
where the state vector x = [(rC )T , ϕT , (vC )T , ω T ]T and input vector u = [FTC , MTC ]T , with f (x, u) as: B × B f (x, u) =
vC − ω rC S−1 ω
(6) 1 B (RFD + αRFG + FC ) − ω × vC m + Am × × −1 JJ (MD + pBC RFD + pBC RFB + MC − τ I )
and with JJ = J + AJ . The subset Sd of the kernel of f , i.e., satisfying f (x0 , u0 ) = 0, is chosen as the operating point for the linearization using Taylor series expansion: Sx = {[0, −L, 01×10 ]T } Sd = Sx × Su where (7) Su = {[0, 0, −αmg, 0, 0, 0]T }
representing the chaser cube in its docked position with the target. In order to make the linear model independent of u0 , α is taken as 0 meaning a neutrally buoyant cube. The linear state-space equation is then obtained: (8) x˙ δ = A0 xδ + B0 uδ with 03×3 03×3 I3×3 AL 0 0 −L 03×3 03×3 03×3 I3×3 (9) , AL = 0 0 0 A0 = 03×3 03×3 03×3 03×3 L 0 0 03×3 Aϕ 03×3 03×3 −mglz 0 0 2 Jxx + AJ + Am lz −mglz Aϕ = (10) 0 0 Jyy + AJ + Am lz2 0 0 0 and 03×3 03×3 03×3 03×3 B0 = I3×3 (11) 03×3 m + Am 03×3 J−1 J The measurement and output equations are obtained as: z = C0 xδ (12) ˆ 0 xδ y=C B
B
Here, xδ=x − x0 , uδ =u − u0 , z= [(r1 )T , (r2 )T , ϕT , ω T ]T , B B B B and y = [(r1 )T , (r2 )T , ϕT , (˙r1 )T , (˙r2 )T , ω T ]T represents the output feedback with state estimation. Here, matrix C0 is the Jacobian matrix of the output measurement, ˆ 0 accounts for estimation of velocities: while C
IFAC ACA 2016 August 21-25, 2016. Quebec, Canada Patrick Abouzakhm et al. / IFAC-PapersOnLine 49-17 (2016) 260–265
−I3×3 CA12 03×3 03×3 C 0 0 −I C0 = 3×3 A22 3×3 3×3 03×3 I3×3 03×3 03×3 03×3 03×3 03×3 I3×3 −I3×3 CA12 03×3 03×3 −I3×3 CA22 03×3 03×3 0 I 0 0 ˆ0 = C 03×3 03×3 −I3×3 C3×3 3×3 3×3 3×3 B12 03×3 03×3 −I3×3 CB 22 03×3 03×3 03×3 I3×3 0 0 −L 1 0 0 (−1)i L , i = 1, 2 CAi2 = 2 L (−1)i+1 L 0 0 0 L 1 0 0 (−1)i L , i = 1, 2 CBi2 = 2 −L (−1)i+1 L 0
(13)
(14)
(15)
(16)
These equations represent the linear dynamics model for the chaser cube with a fixed target, near the docked position, with the modelled output feedback. It can be shown that the model is both controllable and observable, and meets the sufficient condition for generic pole assignability of a minimal realization through static output feedback control (Syrmos et al.,1997): m+p≥n+1 (17) ˆ 0) where m (# of columns in B0 ) and p (# rows in C represent the size of the outputs and inputs respectively. This model can therefore be used to design a closedloop output feedback controller for docking using the pole placement technique, to be presented in section 5. 4. GUIDANCE AND NAVIGATION The full docking problem of Tryphon blimps has been broken down into two stages: rendezvous and docking. The rendezvous stage brings the chaser and target Tryphon close together to a predefined distance and orientation for the docking setup, i.e., where the dimensions of the ARtag markers are within the visible range of the cameras. In this stage, each Tryphon acts independently and uses the global localization and computed torque controller with waypoint guidance as described in Br`eches (2015) to manoeuvre to the desired pre-dock position. Once this stage is complete, the docking stage begins and the guidance, navigation and control systems of the chaser switch to the more well-suited methods, which are described in the following sections. The target Tryphon is assumed to remain in perfect regulation at the desired location throughout the docking process. 4.1 Sensing and State Estimation During the docking stage, the chaser Tryphon relies on various sensors and a state estimation algorithm to determine its pose and velocity relative to the target. As noted earlier, sensing includes the use of the two on-board cameras, which measure the relative position of the corresponding B ARtag markers to give vectors rB 1 and r2 as defined in section 3.1, the accelerometer and gyroscope measurements of the onboard IMU, and the compass bearing angle. The orientation of the target Tryphon is known through 263
263
its independent sensing; therefore, a relative orientation between chaser and target can be determined. The ROS package ”ar track alvar”, based on the open source ARtag tracking libraries, is used to extract the relative pose of the camera-ARtag pairs. A fiducial marker with dimensions 4×4 cm2 is chosen, with a visible range of approximately 6 cm − 100 cm with the equipped cameras and lenses. The package outputs a relative position and orientation, although the orientation measurements are not sufficiently accurate for docking purposes, and therefore the use of aforementioned independent orientation sensing (with the IMU and compass) is preferred. A standard implementation of a Kalman filter is used to obtain the velocity measurements r˙ B ˙B 1 and r 2 , and to filter all measurements for the output feedback used by the controller. 4.2 Glideslope Guidance Considering the target Tryphon is regulating at a desired position, and that the docking receiver’s cone is designed to allow for some misalignments by guiding the electromagnet, it is desirable to have low speeds during contact and little overshoot so as to not cause a rebound effect. Therefore, a trajectory tracking method which controls the final approach velocity for soft docking is ideal, and it is for this reason that a glideslope guidance algorithm has been chosen. Another advantage of the glideslope guidance algorithm is its reduction in control effort near the end of the docking stage, which for spacecraft, reduces the plume impingement on the target according to Hablani et al. (2002), and for the case of Tryphons, reduces the effects of propwash. According to the glideslope guidance formulation in Nolet et al. (2004), the algorithm consists of generating a straight line path from the initial location of the chaser to the target. Let ρ(t) be the distance-to-go and ρ(t) ˙ be the approach velocity of the chaser which decreases linearly with ρ(t). Specified parameters for the algorithm include the initial commanded velocity ρ˙ 0 < 0, and the final commanded arrival velocity ρ˙ T < 0, which combined with the initial distance-to-go, ρ0 , result in a glideslope a (< 0) and manoeuvre period T given as: ρ˙ 0 − ρ˙ T (18) a= ρ0 1 ρ˙ T (19) T = ln a ρ˙ 0 The glideslope guidance algorithm is now defined by the differential equation: (20) ρ(t) ˙ = aρ(t) + ρ˙ T whose solution is: ρ˙ T at ρ(t) = ρ0 eat + (e − 1) (21) a Following Hablani et al. (2002), the vectors corresponding to the trajectory to be tracked are defined as: ρ(t) ˙ = ρ(t)ˆ ˙ u (22) ρ(t) = ρ(t)ˆ u where u ˆ = [cos β1 cos β2 cos β3 ]T , and cos β1 , cos β2 and cos β3 are the direction cosines of ρ0 , with ρ0 = ||ρ0 ||. For the case of Tryphon docking, a novel approach is implemented where two glideslope trajectories are generated,
IFAC ACA 2016 264 August 21-25, 2016. Quebec, Canada Patrick Abouzakhm et al. / IFAC-PapersOnLine 49-17 (2016) 260–265
one between each of the receiver and protruder for each of the two docking mechanisms. These two desired trajectories are tracked by the control simultaneously, which reduces the relative distance and orientation between the chaser and target. Ideally, parameters should be chosen so that the manoeuvre periods for each glideslope trajectory are identical. To do this, the final commanded arrival velocities at the two docks are set equal (ρ˙ T 1 = ρ˙ T 2 ) to the value desired for soft docking, and the glideslope for the trajectory with the largest initial distance-to-go (ρ01 ), call this a1 , is chosen to give an appropriate T1 and ρ˙ 01 . Finally, the unknown value of a2 is determined for which the two manoeuvre periods are equal, by solving for the roots of the following equation, prior to the trajectory generation: T1 − T2 (a2 ) = 0 (23) where T2 (a2 ) is found by combining equations 18 and 19: 1 ρ˙ T ln (24) T2 (a2 ) = a2 a2 ρ02 + ρ˙ T This solution is determined once, at the beginning of the manoeuvre and ideally, ensures the same manoeuvre time for the two glideslope trajectories. 5. CONTROL r(t)
xδ
K
z
algorithm developed in Br`eches (2015) is implemented which determines the individual propeller commands to produce the desired control wrench. 6. SIMULATION RESULTS Experiments with Tryphons are performed during field trials, which are organized only a few times per year in a large enough space to support two or more of these flying cubes. For this reason, a detailed simulator, using the non-linear model, has been developed using Gazebo software, for testing and evaluation of guidance and control strategies, as well as state estimation. Advantages of Gazebo are its 3D rendering abilities, and its compatibility with ROS, which allows all algorithms used in simulation to be directly ported to the physical Tryphons. Virtual sensors can also be added in Gazebo with noise profiles similar to those of their real counterparts. The parameters used in the simulation environment for the docking of two Tryphons are given in table 1. Table 1. Tryphon system parameters m (kg)
L (m)
lz (m)
a1
ρ˙ T ) (m s
Tmax (N)
Cd
11.7
2.25
-0.17
-0.05
-0.02
0.63
1.74
J (kg · m2 )
Jx = 15.2 Jy = 15.3 Jz = 16
y
Fig. 4. Block diagram of output feedback control Using the state-space model derived in section 3.3, a closed loop, output feedback controller, u = −Ky, is designed using the pole placement technique; the block diagram of the controller is included in Fig. 4. For docking purposes, a controller with gain matrix K which docks the chaser Tryphon with the target with as little overshoot as possible is desired. For this reason, all closed loop poles of the system are chosen to be critically damped. Furthermore, the Tryphon’s propellers have a relatively low maximum thrust (Tmax ), and therefore gains which do not demand control inputs exceeding these values while tracking the glideslope trajectory are ideal. Using the pole placement technique on the state-space model, leads to an intermediate gain matrix K , which corresponds to the state feedback gain matrix. The output feedback gain matrix K is found using the generalized matrix inverse following the approach in G¨ uney and Atasoy (2011), where: †
ˆ0 K = K C
(25)
ˆ† C 0
represents the Moore-Penrose pseudoinverse of the and ˆ 0. state estimation matrix C The closed-loop, output feedback controller formulated above has been designed with the assumption of neutral buoyancy, since the actual difference between the buoyancy and gravity forces is expected to be small, however, unknown and time-varying. The system, state and output feedback gain matrix are augmented by adding an integral action term in z to compensate for the difference between the weight and buoyancy forces. The force distribution 264
Am (kg) Amx = 11.3 Amy = 11.3 Amz = 11.3
AJ (kg · m2 )
AJx = 0.2 AJy = 0.2 AJz = 0.2
For docking simulations, the target Tryphon is stationary, neutral buoyancy is assumed and the Kalman filter described in section 4.1 is used. The chaser Tryphon starts by regulating under output feedback control at a desired output of y = [.2, 1, .1, .2, 1, .1, 01×12 ]T , simulating the completion of the rendezvous stage at a 1 m distance, with 20% and 10% misalignments in lateral and vertical directions, respectively. Furthermore a small, random disturbance in orientation is applied on the chaser just before docking begins to observe the robustness in the control and glideslope guidance algorithm. The docking stage then begins, where the chaser tracks the glideslope trajectories. It is important to note that the simulation does not emulate the actual docking, i.e., electromagnet contacting the metallic plate. Fig. 5 shows the results of a simulated docking, where the vector components of B the estimated, desired and error values of rB 1 and r2 are given, as well as the relative orientation of the chaser. The manoeuvre period for the glideslope trajectory was 26.4 s, and the results clearly show that for both trajectories the manoeuvre times are indeed the same. Furthermore, all position error terms near the end of the trajectory are less than 4.25 cm, which falls within the area of the coneshaped receiver, thereby indicating that a successful dock is feasible. The initial orientation disturbances are also attenuated to near zero. 7. CONCLUSION The Tryphons are unique aerial vehicles with many applications in art and engineering. The topics that have been
IFAC ACA 2016 Patrick Abouzakhm et al. / IFAC-PapersOnLine 49-17 (2016) 260–265 August 21-25, 2016. Quebec, Canada
0.25
0.15
Coordinate x2 (m)
Coordinate x1 (m)
0.25
estimated desired error
0.2
0.1 0.05
0.15 0.1
REFERENCES
0
−0.05
−0.05 0
10 20 Time (s)
30
1
0
10 20 Time (s)
30
0
10 20 Time (s)
30
0
10 20 Time (s)
30
1 Coordinate y2 (m)
Coordinate y1 (m)
the Fonds Qu´eb´ecois de Recherche sur la Nature et les Technologies(FQRNT).
0.05
0
0.8 0.6 0.4 0.2 0
0.8 0.6 0.4 0.2 0
0
10 20 Time (s)
30
0.1
Coordinate z2 (m)
Coordinate z1 (m)
estimated desired error
0.2
0.05
0
−0.05
0.1
0.05
0
−0.05 0
10 20 Time (s)
30
(a)
(b)
Angle (degrees)
1 0 −1 −2
Roll Pitch Yaw
−3 0
5
10
15 Time (s)
265
20
25
30
(c)
Fig. 5. Simulation results showing estimated, desired and B error vector components of (a) rB 1 and (b) r2 , and the (c) relative orientation of the chaser covered in this paper are the guidance, navigation and control for the docking of two Tryphons. A dynamics model, output feedback visual servoing controller and glideslope guidance algorithm have been presented. The docking mechanism presented consists of a unique electromagnetic system, which requires the contact of two protruder ends with their corresponding receivers. The navigation system described utilizes a fiduciary marker system to obtain the relative positions between the protruders and receivers, i.e., the chaser and target. A previously developed glideslope guidance algorithm was successfully extended to the scenario where simultaneous docking at two physical locations is required. Simulations are carried out with a stationary target and demonstrate feasibility of docking. Future work will focus on experimental demonstrations and generalization of the methodology to docking to a moving target. ACKNOWLEDGEMENTS This work was supported by the National Sciences and Engineering research Council of Canada(NSERC) and 265
Br`eches, P.Y. (2015). Dynamics Modeling and State Feedback Control of a Lighter-than-air Cubic Blimp. Master’s thesis, McGill University, Canada. Fiala, M. (2005a). ARTag Fiducial Marker System Applied to Vision Based Spacecraft Docking. Shaw Conference Centre, Edmonton, Alberta, Canada. 35–40. Fiala, M. (2005b). Comparing ARTag and ARToolkit Plus fiducial marker systems. In IEEE International Workshop on Haptic Audio Visual Environments and their Applications, 2005, 148–153. G¨ uney, S. and Atasoy, A. (2011). An approach to pole placement method with output feedback. Hablani, H.B., Tapper, M.L., and Dana-Bashian, D.J. (2002). Guidance and Relative Navigation for Autonomous Rendezvous in a Circular Orbit. Journal of Guidance, Control, and Dynamics, 25(3), 553–562. Lee, P.M., Jeon, B.H., and Lee, C.M. (2002). A docking and control system for an autonomous underwater vehicle. 3, 1609–1614. Nolet, S., Kong, E., and Miller, D.W. (2004). Autonomous docking algorithm development and experimentation using the SPHERES testbed. 5419, 1–15. Park, J.Y., Jun, B.h., Lee, P.m., and Oh, J. (2009). Experiments on vision guided docking of an autonomous underwater vehicle using one camera. Ocean Engineering, 36(1), 48–61. Singla, P., Subbarao, K., and Junkins, J.L. (2006). Adaptive Output Feedback Control for Spacecraft Rendezvous and Docking Under Measurement Uncertainty. Journal of Guidance, Control, and Dynamics, 29(4), 892–902. St-Onge, D., Gosselin, C., and Reeves, N. (2015). Dynamic modelling and control of a cubic flying blimp using external motion capture. Proceedings of the Institution of Mechanical Engineers, Part I: Journal of Systems and Control Engineering, 229(10), 970–982. Syrmos, V.L., Abdallah, C.T., Dorato, P., and Grigoriadis, K. (1997). Static output feedbackA survey. Automatica, 33(2), 125–137. Tournes, C., Shtessel, Y., and Forem, D. (2011). Automatic Space Rendezvous and Docking using Second Order Sliding Mode Control. In A. Bartoszewicz (ed.), Sliding Mode Control, 307–330. InTech. Tsai, D. (2012). Autonomous Vision-Based Docking of the Tethered Axel Rover for Planetary Exploration. Master’s thesis, Lule University of Technology. Wilson, D.B., Gktogan, A.H., and Sukkarieh, S. (2015). Guidance and Navigation for UAV Airborne Docking. In Robotics: Science and Systems. Wu, S., Zhang, L., Xu, W., Zhou, T., and Luo, D. (2013). Docking control of autonomous aerial refueling for UAV based on LQR. In 2013 10th IEEE International Conference on Control and Automation (ICCA), 1819– 1823.