RECENT DEVELOPMENTS FOR AN OBSTACLE AVOIDANCE SYSTEM FOR A SMALL AUV

Transcription

1 RECEN DEVELOPMENS FOR AN OBSACLE AVOIDANCE SYSEM FOR A SMALL AUV Douglas Horner and Oleg Yakimenko Naval Postgraduate School Monterey, CA USA Abstract: Improvements in high resolution small orward looking sonar (FLS) and computer processing have made it possible to develop an obstacle avoidance system (OAS) or small diameter Autonomous Underwater Vehicles (AUV). An AUV with such a system can maneuver around unanticipated obstacles that may be proud o the ocean loor. his ability can prevent serious damage to the vehicle or the environment. his paper discusses developments in control and computer vision techniques o an OAS designed to vertically avoid obstacles ound on the ocean loor. Results are presented rom recent in-water testing. Copyright 00 IFAC Keywords: Obstacle Avoidance, Optimal Control, Navigation, Obstacle Detection. INRODUCION We discuss progress in a developmental OAS mounted on the ront o a small AUV. he scope o the problem is limited by considering the avoidance o obstacles proud o the ocean loor. he maneuver around the obstacle is in the vertical plane. In other words, i an obstacle is detected in the path o the AUV, the AUV will maneuver up over the obstacle and return to the pre-planned altitude. Frequently AUVs conduct side scan sonar surveys o the bottom, these surveys are conducted close to the ocean loor to limit gaps in sonar coverage.. he discussion ocuses around the image processing and the control necessary to avoid an unanticipated obstacle. his is a continuation o the work irst reported in OCEANS 005 [Horner]. he image processing section ocuses on tracking the ocean loor using the orward looking sonar. Using the vehicle state together with a hough space representation o the image, an Extended Kalman Filter (EKF) is constructed to give a predictive estimate where the ocean loor is in the sonar image space [Mills]. Once the ocean loor is reliably tracked, obstacles and eatures can be tracked that are proud o the ocean loor. When an obstacle is high enough o the ocean loor to be a threat to the vehicle, the pre-planned vehicle path is modiied. A direct method or calculating real-time, near optimal polynomial traectories [Yakimenko] is presented or the avoidance behavior. he calculation takes into consideration an optimization unction based, in part, on the idea o minimizing the altitude change to ensure that sensor coverage stays consistent. he real time traectory planning has the secondary beneit o being able to react to additional obstacles that may have been occluded by the irst identiied obstacle. he paper is organized as ollows: Section two describes the obstacle avoidance ramework. Section three gives a description o the sonar. Section our discusses sonar image processing. Section ive discusses the AUV motion model. Section six discusses optimal problem ormulation and Section seven describes near optimal traectory generation and Section eight describes recent results.. FRAMEWORK he obstacle avoidance ramework (Fig. ) consists o the environmental map, a planning module, a localization module and the sensors and actuators or the control o the robot. he environmental map is the world according to the robot. his can include a priori knowledge, or example the positions o charted underwater obstacles. It takes as input the sensor and state inormation rom the robot and translates the sensor inormation into the global space. he localization module takes as input the most current position estimate rom the robot s navigation model and select eatures rom environmental model to provide (i possible) a better estimate o the robots position. he planning module is responsible or coming up with a traectory that the robot can ollow that is ree o obstacles and in some sense optimal. It consists o the deliberative and reactive planners. hey take inormation rom the environment and localization modules to produce the traectory that the robot controller tracks. he deliberative planner uses the environmental map to produce traectories to a goal state that is ree o obstacles. he reactive planner takes recent sensor inormation to produce a traectory which will avoid an unanticipated obstacle but provides no guarantees o reaching the end goal state.

2 3. SONAR DESCRIPION he orward looking sonar is a Blazed Array 450 KHz prototype built by Blueview echnologies. he system has a total o six staves, our oriented loor is accomplished by searching or a point in the hough space. Figure. Example o a vertical sonar image with a obstacle located approximately 35M rom the AUV Figure. Obstacle Avoidance Framework horizontally and two oriented vertically. Each individual stave has an approximate 3 degrees ield o view with degrees o vertical aperture. he range resolution is.09m and the angular resolution is a nominal. degree. he staves oriented horizontally combine together create a single sonar image. his image has a 90 degree ield o view with degrees o vertical ambiguity. he staves oriented vertically combine to produce a single image. his image has a 46 degree ield o view and has degrees o horizontal ambiguity. For this paper, only the vertical images are considered. Figure is an example o a vertical image. 4. SONAR IMAGE PROCESSING his section describes some o the techniques used to detect and track an obstacle using the vertically mounted FLS arrays. 4. Detection o the Ocean Floor he primary technique used to detect the ocean loor is the Hough ransorm [Hough]. It is a common technique in image processing or the identiication o lines rom a set o D points [Duda and Hart]. In mobile robotics, the Hough ransorm (H) has been used or a range o applications including vision based sel-localization [Iocchi and Nardi]. he H uses the parameterized line equation ρ = xcosθ + ysinθ () where θ is the angle o the line to the x-axis, ρ is the perpendicular distance rom the line and the origin o a selected point in the image space, and x and y represent a point on the line. A transormation rom the original image to a hough image converts the ( x, y ) image points into the ( ρ, θ ) hough space. A line in the image space is represented as a point in the hough space. In this case, detection o the ocean Consistent tracking o the ocean loor is made easier by taking into account the state o the vehicle and using this inormation to track the position o the ocean loor in the hough image space. Speciically, an EKF has been used to estimate where the point (i.e. the ocean loor) should be in the hough space given the current AUV position and a motion model. he EKF takes as input AUV pitch, roll, heading, heading rate, surge, and heave, altitude and ocean loor point in the hough space ( ρ, θ ) and produces a predictive estimate o ocean loor in the hough space. his is converted back into the image space and is used as the reerence line in the search or obstacles. 4. Detection o Obstacles In previous work, using a model or the motion o the REMUS AUV an obstacle was determined to be a collision threat to the AUV when the leading edge slope o the obstacle is greater than a nominal 45 degree angle to the ocean loor. his is the angle at which the Doppler Velocity Log (DVL) reporting at Hz can no longer provide altitude data quickly enough or the vehicle control to overcome the altitude change. his is used as the template or determining when to activate an avoidance behavior. With each image, the ocean loor is irst localized. Next a search box is constructed or the relevant area orward o the AUV. he area is searched using the cvblobslib library. When an obect is detected a threshold number o times and is a threat to the vehicle it is classiied as an obstacle. he Hough EKF also helps in separating out obstacles in the sonar image. By tracking the ocean loor in the hough space obstacles can be separated rom the ocean loor by the angle o the obstacle in the image space and the distance rom the image center ρ. Figure 3 shows the original image (rom Figure ) in the hough space. Notice the obstacle is visible as a separate curved line where the peak

3 intensity is at the approximate coordinate ( ρ =, θ = 9). 6. OPIMAL PROBLEM FORMULAION he next step is to ormulate the general optimization problem as applied to the problem at hand. We post it as ollows. here is set o admissible traectories [ θ ] z() t = x(), t z(), t w(), t q(), t () t S, 5 5 { z } S t Z E t t t = (), 0, (5) that satisies a set o ordinary dierential equations ()-(3) z = ( z, δ, c) =,,...,5 i i s i Figure 3. Hough representation o the vertical sonar image shown above. 5. AUV MODEL his section describes the AUV model used by the reactive traectory planner. Consider the twodimensional kinematics equation o motion or the vertical channel or the AUV. x = wsinθ + ucosθ z = wcosθ usinθ () where x is the AUV longitudinal position, z is its altitude with respect to the ocean loor, u and w are the orward and heave velocities respectully and θ is the pitch angle. Next consider the dynamics equation or the REMUS AUV or vertical motion using the standard state-space representation with the state vector x () t [ w, v = q, θ ] : (3) x () t = A x () t + B δ () t v v v v Here q is the pitch rate and δ s is the dive plane delection, the matrices A and B (4) are the multiplication o the inverse o the mass matrix with the motion model. he derivation o the matrices A and B and the coeicients used or the orces, moments and mass moments o inertia are taken rom [Healey]. mzw Zq 0 Zw mu0 + Zq 0 Av = M w IyyMq 0 Mw Mq zbbzgmg mzw Zq 0 Zδ s Bv = Mw IyyMq 0 M δs s (4) where δ () t s is the only control and P c = { c, c,..., c p,}, c C is the vector o AUV, characteristics with initial conditions ( z ( t0), δs( t0)), terminal conditions ( z ( t ), δs( t )) and the ollowing constraints on state space controls and control derivatives (listed in order): η( t, z) = { η( t, z), η( t, z),..., ηw( t, z)} 0 ξ( t, z) = { ξ( t, z, δs), ξ( t, z, δs),..., ξv( t, z, δs)} 0 π( t, z) = { π ( t, z, δ ), π ( t, z, δ ),..., π ( t, z, δ )} 0 s s v s (6) he problem is to ind an optimal traectory and optimal control δ s opt () t that minimizes some perormance index, or instance some integral index t J = (, t z, δ ) dt t0 0 s () For an AUV constraints (8) result in ) vertically avoiding the obstacle, ) staying within the constraints on the dive plane delection δ s, and 3) accounting or plane delection dynamics (modeling as a irst-order system): η( z) = zx ( obs ) zobs > 0 ξδ ( s) = δsmax δs( t) 0 πδ ( ) = δ δ( t) 0 s smax s (8) As a perormance index (9) it is natural to use the combined one that ) minimizes the distance traveled to avoid the obstacle, and ) maximizes the amount o time with the pitch o the AUV between nominal θ o 0 and -3 degrees: nom t ( ( ) 0) ( θ( ) θnom ) dt (9) t0 J = zt z + k t where k is some scaling coeicient (o the order o ). his later one is to try and ensure the sonar is looking orward a maximum amount o time during the avoidance maneuver.

4 . NEAR OPIMAL CONROL FOR REACIVE OBSACLE AVOIDANCE When an obstacle is identiied by the image processing routine, an OA behavior is activated and the position and maximum height o the obstacle is sent to this process. Originally both with the NPS ARIES and REMUS AUVs reactive OA behavior was achieved by applying an additive Gaussian altitude curve over the obstacle position. his proved to be an adequate irst solution but had two detractions. First, any obstacle proud o the ocean loor ensoniied by the FLS, may occlude any obects behind it. Using a Gaussian additive altitude approach the AUV runs the risk o navigating into another obstacle on the backside o the Gaussian curve. Second, the implementation doesn t permit an AUV to react to moving obects. Instead the approach is to use near optimal real-time polynomial traectory path planner or reactive obstacle avoidance. he traectory generation and optimization procedures [Yakimenko] are now described or AUV navigation. his direct-methodbased procedure assures the ollowing: the boundary conditions including high-order derivatives are satisied a priori, the control commands are smooth and physically realizable, the method is very robust and is not sensitive to small variations in the input parameters; only a ew variable parameters are used, thus ensuring that the iterative process during optimization converges well and that the continuous update o the solution allows reliable path ollowing even with no standard eedback. Another important eature o this method is that it allows handling any compound perormance index, like in (). hereore, we are not limited to simple standard indices like time, uel expenditure, etc.. Generating a Candidate raectory Consider a desired AUV traectory as p( τ ) = [ x( τ), z( τ)] where τ is some abstract argument, the virtual arc, and is deined by τ = [0; τ ], where τ is the total length o the virtual arc. Each coordinate x i, i =, (or simplicity o notation we assume x ( τ ) x ( τ ) and x ( τ ) z( τ )) is represented by a polynomial o degree N with the orm: = = M * k i( τ) ikτ,, k = 0 x a i (0) he degree o the polynomial M is determined by the number o boundary conditions that must be satisied. In our case the desired traectory includes constraints on the initial and inal position, velocity and acceleration. In this case the minimal order o the polynomial is 5. However, to increase the number o varied parameters and allow or more lexibility o the candidate traectory the order o polynomials can be higher than 5. Say we go with the seventh-order polynomial, so that x ( τ) = i k = 3 k x i( τ) = aikτ k = max(, k ) x i( τ ) = aikτ k = max(,( k )( k )) (max(, k 3))! k xi( τ) = aikτ, k! k = 0 a τ k 3 ik k () he coeicients aik, i =,, k = 0,..., can be determined by solving the ollowing system o linear algebraic equations ai 0 xi a i x i a i x i ai 3 x i = τ τ 6τ 4τ 60τ 0τ 0τ ai 4 xi τ τ 6τ τ 0τ 30τ ai 5 x i τ τ 3τ 4τ 5τ a x i6 i τ τ τ τ a x i i () As seen, no matter what value o the inal arc τ we have, the boundary conditions will be unconditionally satisied. But varying this value allows varying the shape o the candidate traectory. Figure 4 demonstrates an example when an AUV navigates at 3m altitude at.5m/s and in order to avoid some obstacle has to perorm a pop-up maneuver. Changing the value o τ allows avoiding obstacles with dierent height. Figure 4. Variances o the candidate traectory while changing the value o τ. Now what about our ree parameters which in our case are components o the initial and inal erk, x i0

5 and x i, i =, (5)? Having these additional varied parameters allows to urther change the overall shape o the traectory to be able to vary the combined perormance index () we chose. o this end Fig.5 shows an example o an AUV navigating at 3m altitude at.5m/s and trying to avoid a 6m obstacle located in between initial and inal points. Figure 6. Example o the dynamic reconiguration o the traectory.. Inverse Dynamics Figure 5. Symmetric varying o x 0 and x an obstacle with optimizing τ. to avoid Now, let us address the reason or choosing some abstract parameter τ (not time, not path length) as an argument or the reerence unctions ()-(3). Assume or a moment that τ t. In this case once we deined the traectory we unambiguously deined the speed proile along this traectory as well since V() t = u + w = x () t + z () t (3) Obviously we cannot allow this and would want to vary the speed proile independently. With an abstract argument it becomes possible via introducing the speed actor λ such that Now instead o (6) we have dτ λτ ( ) = (4) dt = ( ) ( ) + ( ) (5) V( τ ) λτ x τ z τ and by varying λ( τ ) can achieve any speed proile we want. hereore using a virtual arc becomes a key element in the proposed approach. All together the robustness o the approach allows generating the traectory to accommodate or sudden changes like newly discovered obstacles. As an example Fig. 6 demonstrates the scenario when an AUV avoiding the irst obstacle discovers that there is another one right behind the irst and to avoid the collision a new traectory starting exactly with the current states and control (up to the second-order derivatives o the states) needs to be generated. he method allows doing this and assures smooth nonshock transition. Having the candidate traectory determined by (3)- (5) the goal now is use inverse dynamics with the AUV model to calculate the remaining states and dive plane commands that is required to ollow this candidate traectory. In this way we can ensure that the candidate traectory does not exceed vehicle or coniguration space constraints. Using the relation (6) or any parameter ζ, dζ dτ ζ ( τ ) = = ζ ( τ) λ( τ ) (6) dτ dt and AUV motion model the it is possible to convert the equations o motion into the τ domain x = w + u x = w + u z = wcos θ usin θ z = λ ( wcos θusin θ) θ= q θ = λ q sin θ cos θ λ ( sinθ cos θ) w = Aw + Aq+ A3θ+ B δs w = λ ( Aw + Aq+ A3 θ+ B δs) q = A w+ Aq+ A3θ + B δs q = λ ( A w+ A q+ A3θ+ B δs) () We will urther use a slightly modiied scheme and having the boundary conditions needed to be satisied and some initial guesses on the varied (ree) parameters we only establish a reerence polynomial or the vertical coordinate z( τ ) (3). Now we proceed with a numerical procedure or computing all remaining states along the reerence traectory over a ixed set o N points (or instance, N=00) equidistantly placed along the virtual arc [0; τ ] with the interval o τ Δ τ = (8) N We take the very next value or the virtual arc τ = τ +Δ τ =,..., N (9) and compute the corresponding parameters o the traectory rom pz ( τ ) and pz ( τ ) (3). Having these, we proceed with computing the corresponding time period Δ t and elapsed time t :

6 z z Δ t = t t w t = t +Δt ( t =0) (0) Next we compute the current value o the speed actor: Δτ λ = ( λ = u0 =.5) Δt () Having those we can exploit two o the equations in the virtual domain (0) as ollows: 8. RESULS Demonstrations were recently completed in an exercise sponsored by the Oice o Naval Research in Panama City FL. he obstacle was a sunken 60m oil supply ship. At its highest point the vessel was approximately meters o the ocean loor. Runs were made at the vessel perpendicular to the direction o the keel. he optimal traectory planner was not implemented on these runs. Figure shows a side scan sonar image which shows the response o the AUV to the sunken ship. x = λ ( w sinθ + u cos θ ) 0 ( θ δ ) w = λ A w + A q + A + B x = x +Δτ x w = w +Δτ w 3 s; he next step is to compute the pitch angle, pitch rate and pitch acceleration: () θ q q 0 = cos λ w + u 0 θ θ t q q t = θ Δ = θ Δ ux + wz (3) Finally, we compute the dive plane delection required to ollow the traectory δ s; = ( q Aw Aq A3 θ ) (4) B.3 Optimization When all parameters (states and control) are computed in each o N points, we can compute the perormance index and the penalty unction. Speciically, according to (0), the penalty unction can be ormed as ollows: min 0; ( obs ) δ δ Δ=, k, k max 0; max 0; ( zx z ) obs ( δs; δsmax) ( δ s; δsmax) (5) where the penalties on violation o constraints on controls and their derivatives are scaled with the corresponding actors. Now the problem can be solved say in the MALAB development environment with the built-in mincon unction or minsearch unction. In the latter case we have to combine the perormance index and the penalty unction together. Figure. Side scan sonar image rom the AUV during an reactive avoidance manuever. REFERENCES Duda, R.O. and Hart, P.E., (9) Use o the Hough transorm to detect lines and curves in pictures. Communications o the ACM, 5():-5. Healey, A. J.,Obstacle Avoidance While Bottom Following or the REMUS Autonomous Underwater Vehicle. Proceedings o the IFAC Conerence, Lisbon, Portugal, July 5-, 004. Horner D.P., Healey, A..J., Kragelund, S.K. (005) AUV Experiments in Obstacle Avoidance.. Proceedings o the IEEE Oceans 005 Conerence, September 8-3, 005, Washington DC. Hough P. (96) Method and means or recognizing complex patterns, U.S. Patent No. 3,069,654. echnical report, HelpMate Robotics Inc. Iocchi, L. and Nardi, D. (999). Hough transorm based localization or mobile robots. Advances in Intelligent Systems and Computer Science, (Mastorakis, N. editor) pages World Scientiic Engineering Society. Mill,S., Pridmore,., Hills, M. racking in a Hough Space with the Extended Kalman Filter. Visual Inormation Engineering, 003. VIE 003. Volume, Issue, -9 July 003 Page(s): Yakimenko, O. Direct Method or Rapid Prototyping o Near Optimal Aircrat raectories. AIAA Jourmal o Guidance, Control, and Dynamics, 3(5): , 000.

7