UUV On-Board Path Planning in a Dynamic Environment for the Manta Test Vehicle

Piero Miotto John Wilde Alberico Menoui Draper Laboratory Draper Laboratory Naval Undersea Warfare Center Aerospace Control Group Decision Systems Divisioii Newport Code 8231, B130212 555 Technology Square 555 Technology Square 1176 Howell Street, Cambridge, MA, 02139. US Cambridge, MA, 02139, US Newport, RI, 02841 [email protected] [email protected] [email protected]

Abstract - This paper presents recent work in the The missions require UUVs to operate with areas of simulation, mission planning, and mission contacts present for extended periods of time in execution for an unmanned undersea vehicle (UUV). The UUV we consider is the Manta Test Vehicle (MY, complex, partially known environments. The ability operated by the Naval Undersea Warfare Center (NUWC) to dynamically plan vehicle paths to avoid dynamic in Newport, RI. A 6-Dof Simulink model of the MTV contacts and create safe vehicle trajectories in vehicle dynamics augmented with an autopilot is used to complex shallow water (SW) environments is test the algorithms. extremely important for mission success and The on-board mission planner generates reference trajectories for the vehicle to follow, taking into overall system survivability. Due to the types of consideration bathymetry data and moving obstacles that missions, the expected complexity of the are within the forward-looking sonar range. A trajectory environment, and changing tactical situations, consists of a sequence of waypoints and associated significant levels of re-planning are expected to be headings from the current vehicle location and orientation to the goal. Trajectory generation takes into performed during the! mission. Additionally, Target consideration the dynamic capabilities of the MTV. The Motion Analysis ('TMA) maneuvers must be D' algorithm - an extension to the Dijkstra shortest-path conducted while balancing route following, algorithm which allows efficient re-planning when arc- obstacle avoidance, and contact avoidance costs change - is used to generate and maintain a safe trajectory. Trajectory re-planning is triggered when the concerns. sonar detects an obstacle in the trajectory currently being Many algorithms for path planning/generation followed. used in robotics research programs have been A Model Predictive Control (MPC) algorithm is designed to function in static environments. Due inserted between the D' algorithm and the vehicle inner to the dynamic nature of the anticipated loop autopilot. The MPC algorithm issues the reference commands to the autopilot to allow the vehicle to follow operational environments, alternate algorithms are the planned trajectory. The cost function within the MPC being investigated by Draper Laboratory and algorithm can be changed depending on the guidance NUWC. task. The MPC algorithm uses a full nonlinear model of This paper describes how a D* algorithm, a B- the MTV vehicle to project ahead the output trajectory and employs orthogonal Laguerre polynomials to create Spline curve generator, and a Model Predictive basis functions that are used in the synthesis of Control algorithm can be integrated to form an on- reference commands to the autopilot. The MPC controller board path planning, guidance, and control also provides a second layer of obstacle avoidance system. capability and keeps the vehicle on-track in the presence of a current. II. UNDERWATER VEHICLE MODEL I. INTRODUCTION The motion of underwater vehicles is analyzed by considering two coordinate frames as shown in Since 1996 the US Navy's Naval Undersea Fig. The kinematics of the body-fixed frame, 0, Warfare Center (NUWC) has been developing a 1, is expressed relative to the earth-fixed frame, concept for undersea warfare in the new century. OE, which is considered inertial. The MTV dynamics Playing a major role in this futuristic concept are model presented here follows the notation of [I] mission-reconfigurable unmanned undersea and [2], and assumes forward motion with vehicles (MRUUVs) that will be capable of carrying unidirectional propeller rotation. out surveillance, tactical oceanography, mine The mathematical framework used is the warfare, and anti-submarine warfare missions. standard set of six-degrees-of-freedom (6-Dof) The Manta Test Vehicle (MTV) is the first equations of motion for a rigid body. The state prototype. vectors v = [U v w p rl'and q = [x y z 4 0 d'arc

0-933957-30-0 2454 ~

used to describe the vehicle's dynamics: v is a The mapping between the MTV's actual vehicle vector of linear and angular rates in the body-fixed control surfaces, 6 = [st,, &fbd 6btm s,,,]~, and the frame: (x, y, z) is the position of the vehicle's origin "virtual" ones, 6, = [Sa Se &IT,used in the 6-Dof relative to the earth-fixed frame; 4, 8, and yare the equations of motion, is roll, pitch, and yaw angles (i.e., the Euler angles). r' O '1

Each actual control surface is driven through various joints and linkages by a high-performance permanent-magnet DC motor. Control surface displacement is achieved by controlling the armature voltage, U,, applied to the motor windings. Fig. 1 NUWC's Manta Test Vehicle (MTV) and coordinate The MTV's propeller is driven by a high- frame definitions. performance permanent-magnet synchronous The control input vector is U = [n 6, 6, &IT, motor. More details on the modeling and control of where n>O is the speed of the propeller, and Se, Se, this motor can be found in 131. The nonlinear 6, are the angular positions of the aileron, elevator, torque load due to the hydrodynamics of the and rudder control surfaces. With these definitions, propeller is the state equations are where Q&O and Qnva

2455 keeps the vehicle on the trajectory commanded by headings. The legal state transitions are shown in the guidance algorithm. Fig, 3. From any given state there is a maximum of In the current design there is not such a clear three neighboring states. distinction between the two functions. In general the D* and the B-Spline algorithms can be viewed as. the guidance function and the MPC and the inner loop autopilot as the control function. Fig. 2 0' 459 SO" shows the Simulink implementation of the MN control system described in this paper.

135* 1800 22P

270" 315'

Fig. 3 State Transitions Our choice of rriap cell-size and legal state transitions enables a minimum vehicle-turn radius Fig. 2: Vehicle Control System constraint to be met. Past applications used a finer heading resolution, allowing state transitions in The path planning and control scheme increments of 10 degrees. This leads to a much presented in this paper is divided in 3 stages. larger search space. Using the MPC- approach In the first stage a minimum path is computed together with a spline generation algorithm-allows by a D* algorithm from the current UUV position to a smaller search :space while still generating the target position based on bathymetry data and feasible trajectories. the current location of the obstacles. The path The cost of making a state transition, also generated by the D' algorithm cannot be directly called the arc-cost, i:jthe distance from the center tracked by the control function because of the of the current cell to the center of the next cell. sharp corners connecting consecutive segments. This performance index defines a shortest-path The second stage uses a B-Spline algorithm to type problem, but could easily be modified to generate a trajectory that the UUV can fly over the include other costs. next waypoints. The optimum 6-Spline path is N A C++ implementation of the D' algorithm [4] is generated incrementally as the vehicle proceeds used to find the optimum path through the map. along the trajectory. The algorithm is an extension of Dijkstra's The third stage implements a Model Predictive shortest-path algorithm giving the advantage of Control algorithm to generate the steering efficient re-planning if arc-costs change during commands that keep the UUV on the desired traversal of the path. Rather than re-solving the trajectory. entire shortest-path problem when some arc-costs The three stages are described in detail in the change, a reduced set of states are re-opened and next sections. the algorithm determines the (possibly new) minimum-cost path from its current position to the A. D' Planning Algorithm The responsibility of the mission planner is to goal. maintain a list of future waypoints for the MPC algorithm to follow. These waypoints must define Obstacle Avoidance a path which the test vehicle is capable of tracking The vehicle's sensors can determine the and which allows the vehicle to travel at its traversability of any 'cell within a specified distance commanded depth while avoiding collisions with from the current vehicle position. If an obstacle is moving obstacles. detected along the v'ehicle's planned path, the map Our path planning algorithm operates on a 5 km is updated and the re-planning function is called. square map of bathymetry data (divided into 50m In Fig. 4 an obstacle is shown crossing the cells) for a section of the Narragansett Bay, RI. vehicle's planned path. However, given the limited For the purposes of path planning, the vehicle vehicle sensor range this obstacle goes state is defined by its cell and one of eight possible undetected and no replanning is performed.

2456 ~

at 0.1 Hz, this gives a margin to increase the planning search space or the rate at which the algorithm is executed.

X (grid cells)

Fig. 4 Undetected Obstacle

In Fig. 5, the vehicle has moved forward, 28 30 32 34 36 38 40 42 bringing the obstacle within sensor range. The X (grid cells) obstacle is detected and the map is updated to reflect the cells that are now invalid (X=37 and Y= Fig. 6 Alternate plan behind obstacle 70,71,72). The planner strategy is to conduct a simple re-plan the first time an obstacle is I Vehicle State's I Time Is) 1 detected. The vehicle then follows the new path. Considered In Fig. 5 we see the vehicle following the new path Initial Plan (Fig. 4) 19678 passing in front of the obstacle, the dotted line Replan (Fig. 5) 0.016 represents . Alternate Plan (Fig. 6) 0.031 Final Plan 9280 0.109

60 Tab. 1 D'algorithm runtime ..... B. 6-Spline Reference Trajectory The trajectory calculated by the D* algorithm is a set of waypoints connected by path segments. Two adjacent segments can lie on the same line or m 68 at angle of +45 degrees. Fig. 7 shows an hypothetical trajectory with 12 waypoints. 70 The B-Spline is a technique used in computer graphics to generate smooth curves. An 72 introduction to spline curves can be found in 28 reference [9]. The B-Spline algorithm is applied to 5 Fig. 5 Obstacle Detected. Simple Replan waypoints in the D* trajectory. The first control point is the waypoint just behind the current However, if the new path becomes blocked location of the vehicle. In Fig. large tilled circles again by the same obstacle (in case of a dynamic 7 indicate the spline control points, the indicate obstacle), an alternate path is planned. Different X past waypoints, and empty circles future strategies can be implemented to evade a blocking waypoints. Fig. also shows how the degree T of dynamic obstacle. Fig. 6. shows a simple strategy 7 the spline affects the shape of the curve. For T that brings the vehicle behind the obstacle. Once = 1 the point behind the vehicle is reached a new path the spline curves are just connecting the waypoints to the target is calculated. with straight segments, increasing the degree we get smoother curves between the waypoints. The One of the major concerns in using the D' distance between the waypoints and the degree of algorithm in a real time application is computation the spline are selected based on the dynamics of speed. Tab. 1 shows the processing time required the vehicle. for these scenarios, running on a pentium IV (2.2GH.z). The D* algorithm is currently executed

2457 ~

BOO reference trajectory i:j defined in terms of x (East)

700 and y (North) positioris on the map. The MPC methodology is based on the on line 600 solution of a finite horizon open loop control

500 problem subject to plant dynamics and constraints on states, outputs, and inputs. In our application 400 the plant is formed by the close loop autopilot- vehicle system. 300 Fig. 9 shows the basic principle of model 200 predictive control.

100 0 W iW 150 2W 250 300 Fig. 7: BSpline Curves Fig. 8 shows the response of the MTV vehicle . to a step command in the y (North) direction. The Nominal Oufpuf: p,(t) vehicle has difficulties tracking low degree B- Spline (order 1 or 2), while it is able to well track B- Splines of order 3 and 4. Higher order splines generate smoother trajectories and can be used in areas where it is not required to remain close to I - I ~ Prediction Horizon :(r+Tp) j Time the segments connecting the waypoints. The order of the spline can be changed by the autopilot Fig. 9: Principle of Model Predictive Control during the mission. Once the B-Spline curve over the next 5 waypoints is generated it is passed to the MPC. MPC is based on the following points: The MPC is then responsible to track the trajectory At each instant ,I, the future outputs J(t + j) as close as possible. In the MTV implementation are predicted over a determined finite horizon the B-Spline algorithm is running at 2Hz, the same T,. An internal model of the plant is used by rate as the MPC and it is incorporated in the block MPC to predict the future output of the plant. In called MPC in Fig. 2. this application the internal model is nonlinear.

(5)

The future control signals are calculated by optimizing a performance index to keep the process as close as possible to a reference trajectory yR(t).In the current application the index is a weighted quadratic function of the squared distance between the predicted output trajectory and the reference trajectory (d(s)). The optimization problem is minJ(y(t), W;T,) III.1

G(Y(r),ii(z))= d(r)'Qd(r) subject to equation (5) and the following C. MPC Based Autopilot constraints: The MPC is responsible for tracking the B- ii(r)E U, Vre[t, t+T,,] Spline reference trajectory by generating yaw rate (7) commands for the inner loop autopilot. The y(r)E Y, VrE[t,f+Tp]

2458 where T,, is the prediction horizon. Q in L,,(r)=l equation 6 is the weighting matrix. Q is a L, (r)= -r+ 1 diagonal matrix that allows different weightings L, (r)=( r ' -4r + 2)/ 2 of present and future errors. L3(r)= (-r' + 9r' - 18r +6)/6 In equation 6 the squared distance between The results presented in this paper use a 12'h the reference trajectory and the predicted order Laguerre polynomial basis. The response of trajectory is defined as the plant to the fhbasis function over the prediction = (t(r)- <#(r)Y+ (Fi(r)- vn(r))2 (8) horizon is where 6 and 7 are the North and East L;(r) Vrc[f,'+Ti.] (13) position respectively. The difference between the fh basis output and At each sampling instants only the current the nominal output is optimum control signal is sent to the plant E, =jj,(r)-j,,(r) Vre[t,f+T,] while all the future ones are neglected Collecting all the basis input and output It'(t) =u'(r,y(t),T,), r ~[f,I+&] (9) deviation from the nominal in matrix form, we where 6 is the sampling time. This process is have: repeated at the next cycle. B=[b, b2 ... b,,] E=[&, ... &,] In order to. reduce the optimization problem defined in equation 6 to a linear quadratic The output of the plant can be expressed as the optimization problem, the output predicted sum of the nominal output and a linear trajectory is expressed as a linear combination of combination of the basis output deviation from the variations from a nominal trajectory. The nominal nominal: trajectory is obtained holding the previous input y(r)= j,,,(r) + Ea V r E [t, f + T,.] u(t -6) over the whole horizon T,, . This is also called the free response: the response of the pant when the previous input is held constant over the prediction horizon. The nominal input (14) iiv(r)= u(t -a), Vr~[f, r+ T,] (10) The cost function of the MPC control problem can when applied to the plant model produces the then be expressed in the following form nominal output trajectory J =(j(r)-y~(r))TQ(j(r)-y~(r)) jy(r) Vrck t+T,1 (11) =(j,~(r)+EEa-y,(r))TQ(~.~(r)+Ea-y,(r)) (1 5) A set of polynomial basis functions, b,(s),are =(Ea~(y,(r)-j,(r)))TQ(Ea-(yR(r)-j,,,(r))) then added to the nominal input: = (Ea-&,(r))'Q(Ea- &,(r)) ii,(r)= i&(r)+ bj(r), VrE [t, f +T,] (12) Where ~~(7)is the difference between the Different basis functions can be used to perturb the nominal input. The stability and the reference trajectory and the free response. In our convergence of the MPC control problem is strictly application the output is already the distance related to the type and the perturbation level of square from the reference trajectory and so y,(r) basis function selected. The most common basis is equal to zero. The MPC is so reduced to a functions are step input equally or logarithmically constrained LQ problem in alpha. spaced over the prediction horizon. Although these 1 "(a)= min-a'Ha-F'a basis gave good results we found that Laguerre e2 orthonormal basis functions helped to form a acA numerically well posed LQ problem. The first few where H and F can be easily calculated from Laguerre polynomials are equation 15. At this point we assume a linear relationship between the basis input (B) and output (E). The

2459 optimum a' is used to calculate the optimum input is moving North along the 2500 m East meridian. at time t: Several plans are calculated along way to generate alternate paths around the moving U'(t) = ii,(t)+ B(t)a' (16) obstacle. Initially the vehicle tries a path in front of As we mentioned earlier the optimum input is the obstacle, then due to its lower relative velocity, applied to the plant and the process is repeated at the D* algorithm switches to a path behind the the next sampling time. obstacle. Fig. 11 shows the trajectory followed by the IV. APPLICATION RESULTS MTV vehicle during a mission with 4 legs. The first The algorithms described in the previous leg brings the vehicle from the Start position to section were implemented in C and C++, and point A with a required heading of 180", the vehicle integrated in a Simulink simulation of the MTV test next proceeds to points B (required heading 270"). vehicle. Fig. 2 shows the top level diagram of the C (225"),and D (18Clo)before returning to the End Flight Control System Simulink subsystem. The point. The circle around point C is necessary to Scheduler calls the various software tasks at their achieve the 225" heading. The vectors in Fig. 11 appropriate rates. Considering the slow dynamic represent the direction of a 3 knot current. A response capability of the vehicle, the D* algorithm perfect measurement of the current is used in the is called at 0.1 Hz, the B-splinelMPC controller at MPC to calculabe the optimum steering 0.5 Hz, and the inner loop autopilot at 25 Hz. commands. Finally Fig. 12 shows the reference One of the key points in the design of the MPC trajectory generated by the B-Spline algorithm to controller is the definition of the prediction horizon. follow the waypoints out of a narrow inlet. The horizon needs to be long enough to capture the slow dynamics that are relevant in the calculation of the cost function. The cost function is calculated based on the distance from a reference trajectory defined as North and East positions. The vehicle 90" turning time is a function of speed and it is of the order of 1 minute. Therefore, in order to track 90" turns a prediction horizon of at least 1 minute is necessary to formulate a well posed MPC problem. The MPC controller uses a 2 Hz discrete model of the plant to predict the output trajectory. With one input control variable, the heading rate, and a prediction horizon of one minute the MPC optimum control problem has a search space of 120 variables (if no blocking is applied to the input). Fortunately the basis function approach reduces the size of the problem from 120 down to 12 independent bases, drastically reducing the complexity and the computation time. The slow turning capability of the vehicle justifies the decision to run the D* algorithm at O.1Hz. There is no benefit in updating the plan faster than 0.1 Hz because of the limited capability of the vehicle to avoid any obstacles that appear in close range. In the simulation the MTV vehicle operates between 2 and 7 knots at a constant depth of 20 ft. The simulations were conducted using a map and bathymetry data of the Narragansett Bay. An example of replanning in order to avoid a East moving obstacle is shown in Fig. IO. The obstacle Fig, 11 Path with Multiple Objectives

2460 4200 International Conference on Robotics and Automation, 4300 May 1994.

MOO [5] Stentz A., "The focussed D* algorithm for real-time replanning", Proceedings of the lntemational Joint 4500 Conference on Artificial Intelligence. August 1995. 4600 -F [6] Stentz A., "Map-Based Strategies for Robot Navigation ~ 4700 in Unknown Environments", Proceedings AAA1 96 Spring 4800 Symposium on Planning with Incomplete Information for Robot Problems, 1996. [7] Maciejowski J. M.. Predictive Control with Constraints, Prentice Hall, Pearson Education 2002. -5100-:=L-:=U5200 [8] Judd K. B., and McLain T. W. "Spline based Path Planning for Unmanned Air Vehicles", AIAA-20014238. 7000 7100 7200 7300 7400 7500 7600 7700 [9] Chankin, G., "An algorithm for high speed curve x Iml generation", Computer Graphics and Image Processing, Fig, 12 Spline Fit for first leg of initial path 3 (1974), pp. 346-349.

VI. CONCLUSIONS This paper presents the results of a feasibility study for on-board path planning and vehicle control algorithms for the Manta Test Vehicle. The path planner, operating at 0.1 Hz, calculates waypoints that steer clear of obstacles in the environment. An inner-loop autopilot, running at a faster rate, generates vehicle commands based on MPC and spline-generation algorithms. Together, these approaches demonstrate the capability for real-time avoidance of moving obstacles, subject to the dynamic constraints of the test vehicle. Future work involves exercising these algorithms under Draper Laboratory's high fidelity C simulation framework. The path planning algorithm will also be augmented with a method for determining a minimum distance path that meets a given probability measure for successful traversal.

Acknowledgements The authors wish to acknowledge current sponsorship of this work by the Office of Naval Research, under the UUV Project, Autonomous Operations Future Naval Capability (AOFNC) program.

References T. I. Fossen, Guidance and Control of Ocean Vehicles, Wiley, 1994. SNAME, Nomenclature for Treating the Motion of a Submerged Body Through a Fluid, Tech. and Res. Bull. No. 1-5. April 1950. A. Menoui, T. C. Gagliardi, and S. E. Lyshevski, "Dynamics and Control of MTV: a Multipurpose Unmanned Underwater Vehicle", Proceedings of the American Control Conference, Chicago, IL. 2000, pp. 70-74. Stentz A., "Optimal and Efficient Path Planning for Partially-Known Environments", Proceedings IEEE

246 1 本文献由“学霸图书馆-文献云下载”收集自网络，仅供学习交流使用。

学霸图书馆（www.xuebalib.com）是一个“整合众多图书馆数据库资源，

提供一站式文献检索和下载服务”的24 小时在线不限IP 图书馆。 图书馆致力于便利、促进学习与科研，提供最强文献下载服务。

图书馆导航：

图书馆首页 文献云下载 图书馆入口 外文数据库大全 疑难文献辅助工具