Nov 2, 2007 - the USSV, and the actual hardware and software components used for control of ... the control design probl
Proceedings of the 2007 IEEE/RSJ International Conference on Intelligent Robots and Systems San Diego, CA, USA, Oct 29 - Nov 2, 2007
ThC6.3
Control Design for Unmanned Sea Surface Vehicles: Hardware-In-The-Loop Simulator and Experimental Results P. Krishnamurthy, F. Khorrami, T. L. Ng Abstract— We address the control design problem for stabilization and tracking of Unmanned Sea Surface Vehicles (USSVs). To this end, we describe the design and implementation of a high-accuracy real-time Six Degree-of-Freedom (DOF) Hardware-In-The-Loop (HITL) simulation platform for use in development and evaluation of controllers for USSVs. The HITL platform incorporates a nonlinear dynamic model of the USSV, emulation of sensors and instrumentation onboard the USSV, and the actual hardware and software components used for control of the USSV in the experimental testbed. Detailed models of hydrodynamic effects, actuators including thrusters/propellers and control surfaces, and disturbances including ocean currents, waves, and wind are included in the dynamic simulation. The fidelity of the developed HITL simulator is demonstrated through comparisons with experimental data collected from a USSV. We also propose a nonlinear backstepping-based controller for stabilization and tracking for USSVs and present closed-loop results from HITL simulation and experimental testing.
I. I NTRODUCTION Control design for stabilization and tracking of USSVs is well-recognized to be an important problem [1–9] with several civilian and military applications. In particular, design of controllers capable of operating in high sea states and able to attenuate roll and pitch oscillations due to wave and wind disturbances is an open problem of great practical relevance. To achieve this ultimate goal, a 6-DOF modeling framework with sufficient detail and cast in a way amenable to address the control design problem was developed in our previous paper [10]. In this paper, we describe the development of a high-fidelity USSV HITL simulation platform. We also present closed-loop results with a nonlinear backsteppingbased controller for both HITL and experimental tests. The HITL simulation platform incorporates a detailed dynamic model of the USSV, emulation of the hardware components onboard the USSV, and the actual hardware and software components used in experimental USSV control. The developed HITL simulator incorporates emulation of the instrumentation onboard the USSV including the sensors and actuators and the interface to these hardware components through a Controller Area Network (CAN) bus. The HITL The authors are with IntelliTech Microsystems, Inc., 4931 Telsa Drive, Suite B, Bowie, MD 20715. The second author is also with Control/Robotics Research Laboratory (CRRL), Department of Electrical and Computer Engineering, Polytechnic University, Brooklyn, NY, 11201. This work was supported in part by the Office of Naval Research (ONR) under SBIR contract Nos. N00014-04-M-0181 and N0001406-C-0051. Emails: pkrishnamurthy@imicro.biz,
khorrami@smart.poly.edu, tlng@imicro.biz
1-4244-0912-8/07/$25.00 ©2007 IEEE.
simulation platform provides the computer which runs the controls software with the exact environment which it sees when operating in the experimental USSV testbed. The HITL simulator includes the detailed 6-DOF nonlinear USSV dynamic model developed in our previous paper [10]. The dynamic model incorporates models of the significant hydrodynamic effects including radiation-induced and damping forces and moments. Disturbance models of forces and moments caused by ocean currents, waves, and wind are also included. Furthermore, detailed models of the actuators including thrusters/propellers and control surfaces are provided. While the models of each effect have long been available in the hydrodynamic literature [11,12,1], all the relevant effects were brought together in a synthesized setting in [10] in a form that is tractable for use in a control design. Previous control designs have so far been essentially based on a planar model [2–9] incorporating at most a highly simplified linearized model of the roll and pitch dynamics. The disturbances have also been so far treated as essentially exogenous noise inputs. However, since the actual forces and moments especially due to waves are highly coupled with the states of the USSV dynamic model, it can be expected that controllers designed by explicitly addressing the disturbance models can achieve significantly better performance. Utilizing a 6-DOF model of the USSV dynamics is particularly crucial in the design of controllers for roll and pitch oscillation attenuation in the presence of wave and wind disturbance inputs. The paper is organized as follows. The architectures of the experimental setup and the HITL platform are described in Section II. The utilized dynamic model of the USSV is briefly summarized in Section III. Experimental validation of the developed HITL simulator through comparison with experimental data is presented in Section IV. In Section V, both HITL and experimental testing results using a backsteppingbased controller [10] are presented. II. E XPERIMENTAL S ETUP AND HITL P LATFORM The HITL platform is designed to exactly mimic the operating conditions seen by the controls hardware and software in operation onboard the USSV. The hardware architecture of the experimental USSV testbed (also see Section IV) is illustrated in Figure 1. The control algorithms are implemented on a notebook computer. The sensors and actuators on the USSV are all connected to a common highspeed CAN bus. A Kvaser USB-to-CAN adapter is utilized
3660
to access the CAN bus from the notebook computer. The currently available sensors on the USSV include a compass, a GPS, and water speed and depth sensors and the available actuator inputs include rudder angle and port and starboard throttles. In addition, our avionics box (which is the same as used in our work on helicopter control [13]) is interfaced with the notebook computer via a serial port. This avionics box provides a six degree-of-freedom Inertial Measurement Unit (IMU) with an update rate of 50 Hz. The developed HITL simulation platform is illustrated in Figures 2 and 3. The HITL simulation testbed includes a realtime USSV dynamics simulation software (running on Computer 1) and the notebook computer (Computer 2) which is used to run control algorithms onboard the USSV. The USSV dynamics simulation software on Computer 1 is designed to be flexible with all USSV dynamics parameters specified at run-time through text-based configuration files which can be manipulated either directly or through a GUI front-end. Computer 2 receives serial IMU data (which emulates data from the IMU in our avionics hardware) from Computer 1 with an update rate of 50 Hz. Computer 2 also interacts with Computer 1 through USB-to-CAN adapters. The software on Computer 1 includes a complete emulation of the CAN interface which will be seen by Computer 2 during operation on the USSV including all sensor messages and actuator status messages with the proper formats and update rates. The software on Computer 1 receives actuator commands through the CAN interface and computes a full six degreeof-freedom dynamic simulation of the ship. A PIC is used to provide an accurate timing source for real-time fidelity of the HITL simulation. The result of the dynamic simulation is visualized using an OpenGL GUI front-end (screenshot in Figure 4) which can be displayed on Computer 1 or can be exported to another computer via a network socket interface. The control software running on Computer 2 is structured as a multi-threaded application with separate threads for the following tasks: •
•
•
•
been designed to be flexible so that depending on available hardware, a subset of the USSVs could be simulated in HITL mode while the dynamics of the rest of the USSVs could be simulated purely in software. This feature of the HITL simulation testbed is currently being utilized (see Figure 5) in developing and testing a path planning and obstacle avoidance algorithm based on GODZILA [14] for USSV applications.
Sensors CANBUS USB to CAN adapter
Fig. 1.
Fig. 2.
Laptop Serial Data
Avionics Box
Architecture of Experimental Setup.
Architecture of USSV HITL Simulation Testbed.
Acccessing the sensors and actuators by reading messages from and writing messages to the CAN bus in the appropriate CAN message formats. The CAN messages from the various sensors and actuators are not synchronized and have different update rates. Serial communication with the avionics box to read IMU data. Number crunching required for navigation and control algorithm computations. Communicating via a network socket with a front-end program which provides a graphical user interface.
These threads communicate using a client-server architecture with double buffering. The HITL simulation testbed has been designed to be able to support multiple USSVs simultaneously (limited only by the processing, graphics, and I/O port capabilities of the computers being utilized). Furthermore, the testbed has
Fig. 3.
USSV HITL Simulation Testbed.
III. S IX DOF DYNAMIC M ODEL OF A USSV In this section, the USSV dynamic model developed in [10] is briefly summarized.
3661
point Ob by Ib , the net applied force expressed in body-fixed frame by F , and the net applied torque about Ob expressed in body-fixed frame by τ , the 6-DOF dynamic model of the ship can be written as p˙ = J(pr )v
(2)
MRB v˙ + CRB (v)v = F Jt (pr ) J(pr ) = 03×3
(3) 03×3 Jr (pr )
(4)
Jt (pr ) = Rib
Fig. 4.
(5) 1 sx ty cx ty cx −sx Jr (pr ) = 0 (6) 0 sx /cy cx /cy mI3×3 −mS(pG,b ) MRB = (7) mS(pG,b ) Ib
Screenshot of USSV simulation package.
CRB (v) =
mS(vr ) mS(pG,b )S(vr )
−mS(vr )S(pG,b ) −S(Ib vr )
(8)
where F is the 6 × 1 generalized force vector given by F = [F T , τ T ]T , I3×3 is the 3 × 3 identity matrix, and S(ω) with ω = [ωx , ωy , ωz ]T denotesthe skew-symmetric matrix 0 −ωz ωy 0 −ωx . S(ω) = ωz −ωy ωx 0 B. External Forces and Torques
Fig. 5.
GODZILA Testing Using Multi-USSV Simulator.
A. USSV Kinematics and Dynamics Consider an inertial frame Xi Yi Zi with origin Oi and a body-fixed frame Xb Yb Zb with origin Ob . The rotation matrix Rib which transforms vectors in body-fixed frame to inertial frame can be parametrized in terms of three angles θx , θy , and θz as Rib = Rz,θz Ry,θy Rx,θx where Rx,θx denotes the rotation matrix corresponding to a rotation about X-axis by angle θx , etc. Hence, cy cz sx sy cz − cx sz cx sy cz + sx sz Rib = cy sz sx sy sz + cx cz cx sy sz − sx cz (1) −sy sx cy cx cy where cx = cos(θx ), sx = sin(θx ), tx = tan(θx ), etc. The position and orientation of the ship relative to the inertial frame can be represented by p = [pTt , pTr ]T where pt = [x, y, z]T represents the coordinates of Ob as measured in the inertial frame and pr = [θx , θy , θz ]T are the rotation angles. The translational and angular velocities of the ship relative to inertial frame and expressed in body-fixed frame are denoted as vt and vr , respectively. Let v = [vtT , vrT ]T . Denote by G the center of mass of the rigid body and let pG,b denote the coordinates of G relative to the body-fixed frame. Denoting the mass of the ship by m, the moment of inertia about the
The external forces and torques acting on a USSV consist of the following components: 1) Hydrodynamic F H : Radiation-induced and damping forces and moments caused due to gravity and the hydrodynamic resistance of the surrounding fluid. 2) Environmental disturbances F E : forces and moments caused by ocean currents, waves, and wind. These include Froude-Kriloff and diffraction forces caused by incident waves. The generalized force vector due to environmental disturbances is the sum of three components: F E = F current + F wave + F wind caused due to the ocean currents, waves, and wind, respectively. Of these, F wave which is the most significant component is computed by integrating the dynamic pressure distribution over the submerged part of the ship hull. The dynamic pressure distribution is obtained through solutions of Laplace’s equation. The random wave pattern in an irregular sea is modeled using the Pierson-Moskowitz spectrum. 3) Propulsion F P : forces and moments due to actuators (thrusters/propellers, control surfaces, rudders, fins). The force and torque due to each control surface can be computed based on the advance velocity and the angle of attack corresponding to the fluid flow around that control surface and the total generalized force vector due to multiple control surfaces can be obtained by adding the components due to the individual control surfaces.
3662
The radiation-induced forces and moments F R can be decomposed into three sub-components: 1) Added mass due to the inertia of the surrounding liquid = −MA v˙ − CA (v)v 2) Radiation-induced potential damping due to energy carried away by generated surface waves = −DP (v)v, and 3) Restoring forces due to Archimedean effects (weight and buoyancy) = −g(p). These forces can be computed from the water plane area and the transverse and longitudinal metacentric heights. The damping forces and moments F D can also be decomposed into three sub-components: (a)
1) Skin friction = −DS (v)v, 2) Wave drift damping = −DW (v)v, and 3) Vortex shedding damping = −DM (v)v. Hence, FH = FR + FD = −MA v˙ − CA (v)v − DH (v)v − g(p) DH (v) = DP (v)+DS (v)+DW (v)+DM (v).
(9)
Using (3), the dynamics of the vehicle can be written as p˙ = J(p)v MH v+C ˙ H (v)v+DH (v)v+g(p) = F E +F P
(10)
where MH = MRB + MA and CH (v) = CRB (v) + CA (v). Detailed models of the forces and torques mentioned above and characterizations of the various coefficients and matrices appearing in the models of the forces and torques were developed in [10] in a form suitable for control design and are omitted here for brevity. IV. VALIDATION U SING E XPERIMENTAL DATA The fidelity of the developed USSV dynamic model and the HITL platform were tested using experimental data collected from two different USSVs (shown in Figure 6) in the Atlantic ocean. The parameters of the USSVs (which are approximately of length 12 m and mass 9000 kg) were identified via least squares identification based methods using the developed USSV dynamic model and extensive experimental data collected with various excitation signals. Some comparisons between experimentally observed USSV response and simulations using the identified USSV parameters for the PowerVent boat shown in Figure 6(a) are shown in Tables I and II and Figure 7. Due to confidentiality requirements, further details about ship parameters and experimental observations cannot be included in this paper. Based on the data in Tables I and II and additional comparisons that have been performed with a wide variety of excitation signals, it is seen that the developed HITL simulation platform accurately captures the dynamic response of the USSVs and the sensor and actuator behaviors.
(b) Fig. 6. USSVs used in experimental tests: (a) U. S. Navy PowerVent APTD (Advanced Propulsion Technology Demonstrator); (b) U. S. Navy USSV-HTF (High Tow Force).
V. C LOSED -L OOP C ONTROL An important focus in the development of the HITL platform and USSV dynamics mathematical modeling presented in this paper is to provide a framework to facilitate closedloop control law design and testing. For instance, consider the backstepping-based controller presented in [10]. Given a planar reference trajectory (xref , yref ), the control law from [10] is given by
3663
z1
= x˙ ref − kx,p (x − xref ) Z t −kx,i (x − xref )dτ
(11)
0
z2
= y˙ ref − kx,p (y − yref ) Z t (y − yref )dτ −kx,i
(12)
0
θz,ref vt,x,ref FT
= atan2(z1 , z2 ) z2 z1 = = cos(θz,ref ) sin(θz,ref ) = −MH,22 vt,y vr,z + dx,1 vt,x,ref
(13) (14)
+dx,2 |vt,x,ref |vt,x,ref −kx,d (vt,x −vt,x,ref )+MH,11 v˙ t,x,ref
(15)
TABLE I
2.5
T URN CIRCLE RADIUS rt AS A FUNCTION OF PERCENT THROTTLE AND RUDDER ANGLE ( NORMALIZED SO THAT EXPERIMENTAL READING OF
2
20% THROTTLE AND 11.5o RUDDER ANGLE IS
0.5
0
TABLE II
5
10
15
1000
AT ZERO RUDDER ANGLE ( NORMALIZED SO THAT EXPERIMENTAL
vst (Simulation) 0.955 1.500 1.909 2.227 3.500 8.727
0
40
45
50
2
10
0.5 0
0 −0.5 0
50 t(s)
−10
100
0.5
0
50 t(s)
−1
100
0
50 t(s)
100
0
50 t(s)
100
0
50 t(s)
100
0
500 x (m)
1000
0.25
θ (rad)
0.2
y
0
0.15 0.1
0
0.05
= −(MH,11 − MH,22 )vt,x vt,y
0
50 t(s)
0
100
y error (m)
x error (m)
1
−1
(16)
A. HITL Simulation Results A wave of frequency 1 rad/s, height 2.5 m (high end of sea state 4), and at direction 1.2 rad to the X-axis is introduced. The control objective is to track a straightline along the Xaxis at 8 m/s. The performance of the controller is illustrated in Figure 8. The maximum X and Y errors in the 100 seconds of simulation are 3.7 m and 20.3 m, respectively. B. Experimental Results The controller proposed in [10] has been tested on the USSVs described in Section IV. The performance of the controller for a sample experimental test of trajectory tracking is illustrated in Figures 9 and 10. In this test, the controller
6 4
10
0
50 t(s)
100
−10
2
0
50 t(s)
100
x 10
τR (Nm)
T
0
−1 −2
−2
−3
0
50 t(s)
100
−4
20
0
−1
0
30
1
1
2
−2
4
x 10
2
F (N)
where kx,p , kx,i , kx,d , kθz ,p , and kθz ,d are control gain parameters. It is to be noted that these parameters would be gain-scheduled in practice to achieve good dynamic performance over a wide range of operating conditions. Also, the control law (16) could be augmented with an innerloop rudder feedback controller to compensate for rudder dead zone and delay and to improve the rudder transient performance.
100
0
4
3
50 t(s)
20
2
0
z
0
30
3
+dθz ,1 vr,z,ref + dθz ,2 |vr,z,ref |vr,z,ref −kθ ,p (θz − θz,ref ) − kθ ,d (θ˙z − θ˙z,ref ) z
35
1
400
4
+MH,66 θ¨z,ref
30
1.5
600
x (m)
10% THROTTLE IS 1 UNIT ).
−0.5
τR
25 t (s)
20
200
vst (Experimental) 1.000 1.409 1.864 2.182 3.409 8.909
20
30
800
θx (rad)
% Throttle 10 30 40 50 60 80
0
Fig. 7. Acceleration profile at 50% throttle (normalized so that experimental reading of steady-state velocity at 10% throttle is 1 unit): Experimental blue ’x’ line; Simulation - green solid line.
S TEADY- STATE VELOCITY vst AS A FUNCTION OF PERCENT THROTTLE READING OF STEADY- STATE VELOCITY AT
1
z (m)
11.5 11.5 11.5 17.5 17.5 17.5 30 30 30
rt (Simulation) 1.003 1.397 2.594 0.890 1.281 0.710 0.752 0.412 0.857
θz (rad)
20 40 60 20 40 60 20 40 60
rt (Experimental) 1.000 1.412 2.546 0.910 1.322 0.699 0.725 0.334 0.890
vt,y (m/s)
Rudder Angle
1.5
y (m)
% Throttle
Normalized Velocity
1 UNIT ).
y (m)
TURN CIRCLE RADIUS AT
10 0
0
50 t(s)
100
−10
Fig. 8. Simulation results with backstepping-based controller : wave of frequency 1 rad/s, height 2.5 m, and angle 1.2 rad; forward speed 8 m/s.
was commanded to visit two waypoints (the two green *’s in Figure 9) and then, after visiting the second waypoint, to track a straight line of heading 135 degrees (the red +’s in Figure 9). VI. C ONCLUSION We have described the 6-DOF modeling, HITL simulation, and control design for USSVs. The developed HITL simulation platform incorporates a detailed model of the dynamics of the USSV, emulators for the hardware components onboard the USSV, and the actual hardware/software packages utilized for experimental USSV control. Comparisons of the response of the HITL simulator with experimental data demonstrate the fidelity of the simulator over a wide range of
3664
0
−500
20
400
15
300
10
−1000
y(m)
Heading (deg)
GPS Velocity (knots)
500
5
0
0
100
200
300
400
200
100
0
500
0
100
200
t (s)
300
400
500
300
400
500
t (s)
−1500 9
2
8
−2500
0
7
Roll (deg)
Pitch (deg)
−2000
6 5
2
−6
0
100
200
300
t (s) −3500 −1200
−1000
−800
−600
−400
−200
0
−4
4 3
−3000
−2
400
500
−8
0
100
200
t (s)
200
x(m)
Fig. 10. Data from experimental testing of trajectory tracking with smart steering controller: velocity, heading, pitch, roll.
Fig. 9. Experimental Results: Position in a coordinate frame locally tangential to Earth’s surface (with y axis pointing due north). Green circle: initial location. Green *’s: specified waypoints. Red +’s: specified straight line trajectory to be tracked after visiting the second waypoint.
actuator inputs. A backstepping controller was proposed and results under both HITL simulation and experimental testing were presented. As part of ongoing research, the developed HITL simulation platform is being used in the further design and evaluation of nonlinear dynamic controllers for USSVs leading to their implementation on the experimental USSV testbeds described in Section IV. Also, an important focus in the controller design is to explicitly take into account the 6-DOF dynamics of the USSV to attenuate roll and pitch oscillations due to wave and wind disturbances in high sea states. The controller development and refinement are greatly facilitated by the fidelity of the 6-DOF USSV dynamic model and the HITL platform. ACKNOWLEDGMENT The authors would like to thank Dr. Robert Brizzolara of ONR for support of the effort and technical review of the manuscript. We would also like to thank Mr. Eric Hansen for his support and for the pictures of the PowerVent ship. We would also like to record our appreciation for Jason Altice of WRSystems and Al Frontin and Sam Calabrese of NSWCCD for their on-ship support. R EFERENCES [1] T. I. Fossen, Guidance and Control of Ocean Vehicles. New York: John Wiley and Sons, 1994. [2] J. Goclowski and A. Gelb, “Dynamics of an automatic ship steering system,” IEEE Transactions on Automatic Control, vol. 11, no. 3, pp. 513–524, July 1966.
[3] M. R. Katebi, M. J. Grimble, and Y. Zhang, “H∞ robust control design for dynamic ship positioning,” IEE Proceedings - Control Theory and Applications, vol. 144, no. 2, pp. 110–120, Mar. 1997. [4] T. I. Fossen and A. Grovlen, “Nonlinear output feedback control of dynamically positioned ships using vectorial observer backstepping,” IEEE Transactions on Control Systems Technology, vol. 6, no. 1, pp. 121–128, Jan. 1998. [5] A. Loria, T. I. Fossen, and E. Panteley, “A separation principle for dynamic positioning of ships: theoretical and experimental results,” IEEE Transactions on Control Systems Technology, vol. 8, no. 2, pp. 332–343, Mar. 2000. [6] Y. Fang, E. Zergeroglu, M. S. de Queiroz, and D. M. Dawson, “Global output feedback control of dynamically positioned surface vessels: an adaptive control approach,” in Proceedings of the American Control Conference, Arlington, VA, June 2001, pp. 3109–3114. [7] F. Mazenc, K. Pettersen, and H. Nijmeijer, “Global uniform asymptotic stabilization of an underactuated surface vessel,” IEEE Transactions on Automatic Control, vol. 47, no. 10, pp. 1759–1762, Oct. 2002. [8] K. D. Do, Z. P. Jiang, and J. Pan, “Underactuated ship global tracking under relaxed conditions,” IEEE Transactions on Automatic Control, vol. 47, no. 9, pp. 1529–1536, Sep. 2002. [9] E. Lefeber, K. Y. Petterson, and H. Nijmeijer, “Tracking control of an underactuated ship,” IEEE Transactions on Control Systems Technology, vol. 11, no. 1, pp. 52–61, Jan. 2003. [10] P. Krishnamurthy, F. Khorrami, and S. Fujikawa, “A modeling framework for six degree-of-freedom control of unmanned sea surface vehicles,” in Proceedings of the IEEE Conference on Decision and Control/European Control Conference, Seville, Spain, Dec. 2005, pp. 2676–2681. [11] J. N. Newman, Marine Hydrodynamics. Cambridge: The MIT Press, 1977. [12] R. Bhattacharya, Dynamics of Marine Vehicles. New York: John Wiley and Sons, 1978. [13] T. L. Ng, P. Krishnamurthy, F. Khorrami, and S. Fujikawa, “Autonomous flight control and hardware-in-the-loop simulator for a small helicopter,” in Proceedings of the IFAC World Congress, Prague, Czech Republic, July 2005. [14] P. Krishnamurthy and F. Khorrami, “GODZILA: A low-resource algorithm for path planning in unknown environments,” in Proceedings of the American Control Conference, Portland, OR, June 2005, pp. 110–115, to appear in the Journal of Intelligent and Robotic Systems, 2007.
3665
