Cooperative Motion Control of Multiple Autonomous Marine

Transcription

1 Cooperative Motion Control of Multiple Autonomous Marine Collision Avoidance in Dynamic Environments EECI Graduate School on Control Supélec, Feb , 2011

2 Outline Motivation/Objectives Cooperative Motion Control Architecture Collision Avoidance System Predicting a Collision Collision Avoidance Simulation Results/Study case scenarios

3 Motivation/Objectives Vehicles performing a cooperative mission may encounter unforeseen obstacles

4 Motivation/Objectives Vehicles performing a cooperative mission encounter unforeseen obstacles Objectives - Development of a collision avoidance system in dynamic environments - Collision prediction module (Multimodel Kalman filter) - Collision avoidance (path re-planning using Harmonic potential fields or controlling the velocity) - Integration into the cooperative motion control arquitecture - Development of a Matlab simulator platform

5 Cooperative Motion Control Architecture Mission/Path Plan Agents n Environment

6 Cooperative Motion Control Agents Architecture Path-Following Controller Input: predefined path and velocity profile Output: commands that drive the vehicle to the desired path Coordination Controller Input: the position of the variables that parameterizes the path of each vehicle that communicates with it Output: The velocity command to correct the vehicle's position in the formation Collision Avoidance is implemented at the coordination level, together with the Path- Following controller

7 Cooperative Path-following

8 Collision Avoidance System Operates on the vehicle's desired Paths and Velocities 1. Target Tracking & Collision Prediction 2. Collision avoidance Path re-planing Velocity Control

9 Collision Prediction Module - Obstacle (target) dynamic model ẋ = v cos θ ẏ = v sin θ θ = ω Tracks the Target by computing estimates for v and w Compares the Targets probable trajectory with the vehicle path

10 Collision Prediction Module Interactive Multi Model Kalman Filter Bank of KF running in parallel Output is the weighted sum of the state estimations produced by each KF State Model x j k+1 = xj k + t sv j k cos θj k y j k+1 = yj k + t sv j k sin θj k θ j k+1 = θj k + t sω j + η j θk v j k+1 = vj k + ηj vk ω j [w min,w max ]

11 Collision Prediction Module

12 Collision Prediction Module A ob r ob V vh t c V ob A Vh r Vh W t 0 = [t 0,t 0 + δ] δ = v 0 a + ζ a) v 0 velocity of the vehicle at time instant t 0 A ob r ob a maximal breakage deceleration of the vehicle undergoing translational motion t c ζ desired safety margin V ob t c V vh A Vh b) r Vh

13 Collision Prediction Module V vh A ob r ob W t 0 = [t 0,t 0 + δ] t c A Vh r Vh δ = v 0 a + ζ V ob a) A 0 vh polygonial region that bound the area occupied by the vehicle at t 0 A ob r ob A 0 ob idem, but for the obstacle t c V ob t c V vh A t vh = Φ v (x v (.),y v (.),A 0 vh), t W t 0 A t ob = Φ o (x o (.),y o (.),A 0 ob), t W t 0 A Vh b) r Vh

14 Collision Prediction Module V vh A ob r ob W t 0 = [t 0,t 0 + δ] t c A Vh r Vh δ = v 0 a + ζ V ob a) A 0 vh polygonial region that bound the area occupied by the vehicle at t 0 A ob r ob A 0 ob idem, but for the obstacle t c V ob t c V vh A t c vh A t c ob A Vh b) r Vh

15 Collision Avoidance Path Planing Spatially deconflict the paths Artificial potential approach The obstacles to be avoided are represented by a repulsive artificial potential, and the goal is represented by an attractive potential Fast Computation Harmonic Potential Fields Free from local minima Jin-Ho Kim and P. Khosla (1992). Real-time obstacle avoidance using harmonic potential functions. IEEE Transactions on Robotics and Automation, Vol. 8, No. 3, June 1992.

16 Goal Uniform Field Panel method

17 Condition Jin-Ho Kim and P. Khosla (1992). Real-time obstacle avoidance using harmonic potential functions. IEEE Transactions on Robotics and Automation, Vol. 8, No. 3, June 1992.

18 Collision Avoidance Path Planning A free path to the goal is obtained following the velocity field v

19 Potential Field Path Planing

20 Potential Field Path Planing

21 Potential Field Path Planing

22 Velocity Correction Idea: Change the speed that the vehicle travels along the path to avoid collision with a dynamic obstacle

23 Velocity Correction The virtual target avoids collision for: (γ g,t g ) (γ A,t f ) Deceleration Inter-vehicle coordination is achieved through Right of way Priority is granted to the vehicle traveling on starboard tack Correction action is taken by the vehicle traveling on the left

24 NetMarSys Simulator

25 Scenario 1 Dynamic Obstacle

26 Scenario 2 Dynamic Obstacle

27 Scenario 3 Bottleneck

28 Conclusions We proposed a collision avoidance system for autonomous vehicles working in dynamic environments, and showed how to integrate it in a typical cooperative motion control architecture The problem was decoupled into a collision prediction stage (multi-model Kalman filter), and a collision avoidance maneuver (path re-planning using Harmonic potential fields or controlling the velocity) The efficiency of the solutions was illustrated in simulation