Navigation. Global Pathing. The Idea. Diagram I: Overall Navigation

Transcription

1 Navigation Diagram I: Overall Navigation Global Pathing The Idea (focus on 2D coordinates Takes advantage of prior info: navigation space/ dimensions target destination location Risks: Map Resolution too high for RT computations Mitigations: reduce resolution create grid for pocketed/local area Colyette 1

2 project destination point, create sub destination I. Generate grid of course space Initialize a fully connected graph where nodes of uniformly distributed across the area of the navigation space. The edge costs are all the same r (selected resolution for the grid eg r=1m. Diagram II: Area Mapping *The grid depicted in diagram II is a basic grid. The edges between nodes can be between all nodes in relative vicinity, but the edge cost would be larger ( (2rm. II. Recursively apply node pathing algorithm (A* search until destination is reached. A. Apply A* search on current state (pos from localization module With a believed position the quadcopter should navigate through the nodes available edges towards its destination with some allowable error. Diagram III: Path Planned After calculating path, quadcopter would start execute set of controls reflecting that path. B. Poll for refreshed grid map flag from object detection module, if so update map and go back to part A. Colyette 2

3 New observations can be made by the quadcopter from the object detection model. In that case the map would be updated. Diagram IV shows detected obstacle that marks the area where 3 nodes are located forbidden for it detected a wide incoming obstacle.since the depth of obstacle is not know yet, the map is not updated for any other positions. Diagram IV: Path Planned AFTER Map Update note: backtracking may be a problem with new calculation of shortest path after object detection Should always use same start, use current localization to determine location in path. The Algorithm A* Search pseudo code 1. Mark current block 2. Assign as parent for all adj blocks 3. For each adj block, calculate G, H, and F G distance between current position and start H (Manhattan distance between current and end position F = G+H 4. Choose current block as min(f block, repeat 1 if H>0 methods for optimizing: H values can be precalculated. Not all G have to be recalculated... 1 This is Koenig s proposed RT Adaptive A* pseudo code S set of states of the search task, a set of states GOAL set of goal states, a set of states A( sets of actions, a set of actions for every state succ( successor function, a state for every state action pair 1 Sven Koenig. Real Time Adaptive A*. poh43qaaaaj:8k81kl MbHgC Colyette 3

4 variables lookahead number of states to expand at most, an integer larger than zero movements number of actions to execute at most, an integer larger than zero scurr current state of the agent, a state [USER] c current action costs, a float for every state action pair [USER] h current (consistent heuristics, a float for every state [USER] g g values, a float for every state [A*] CLOSED closed list of A* (= all expanded states, a set of states [A*] s state that A* was about to expand when it terminated, a state [A*] procedure realtime adaptive astar(: {01} while (scurr 6 GOAL do {02} lookahead := any desired integer greater than zero; {03} astar(; {04} if s = FAILURE then {05} return FAILURE; {06} for all s CLOSED do {07} h[s] := g[s ] + h[s ] g[s]; {08} movements := any desired integer greater than zero; {09} while (scurr 6= s AND movements > 0 do {10} a := the action in A(scurr on the cost minimal trajectory from scurr to s ; {11} scurr := succ(scurr, a; {12} movements := movements 1; {13} for any desired number of times (including zero do {14} increase any desired c[s, a] where s S and a A(s; {15} if any increased c[s, a] is on the cost minimal trajectory from scurr to s then {16} break; {17} return SUCCESS; 2 Localization (aka position state estimation Want estimate state via: 1. Sensor measurements z 2 Source Material from TUM: Computer Vision Group< Lectures < Colyette 4

5 2. Odometry/Control readings u Prior Info from Bayesian model: want to get probabilities of sensor readings from a given state. P(x P(z x P(z ~x //probability of sensor reading z given any other possible states of state x Bayesian Filter uses Bayes law: P (x z = [P (z xp (x ] { P (z} to solve for the motion model and the sensor model. Possible Variables Example x is a state for 2D, the dimension is 3 (x x,y x, ψ x in a global frame u is the control in 2D, the dimension is also 3 (x u,y u, ψ u in a local frame z is the observation of a visual marker(beacon, dimension also 3 (x z,y z, ψ z in a local frame motion function: g(x,u = x x +(cos(ψ x x u sin(ψ x y u Δt y+(sin(ψ x x u +cos(ψ x y u Δt ψ x +ψ u Δt The beacons in the contest can also help with localization, knowing their locations Suppose marker at m= (x g,y g, ψ g sensor model: z= h(x = (x g x x (cos(ψ x (y g y x sin(ψ x (x g x x (sin(ψ x +(y g y x cos(ψ x ψ b ψ x *h(x computes relative/local coordinates Motion Model where x are the coordinates of the quadcopter from p(x x,u //the current state give the last state and last control cmd also can be seen as a x = g(x,u the belief of the current position is represented as: ~Bel(x t = P(x t x t 1, u t Bel(x t 1 /// From Bayes Filter, g(x t 1 = N(x t, ~u t, ~ t //note u t is the mean as in an non linear system g(x t 1 ~ g(u t 1 + dg(u t 1 /du t 1 (x t 1 u t 1 ~ g(u t 1 + G (x t 1 u t 1 ~u t =g(u t 1 ~ t = G t G t T + Q where Q is process_oise (wind, vibrations, etc Sensor Model p(x z 1,...z n where each z is a different or older sensor reading Bel(x t = yp(z t x t ~Bel(x t ///from Bayes Filter Colyette 5

6 = N(x t, u t, t as in an non linear system u t = ~u t +K t (z t h(~u t //sensor reading collected z t t = (I K t H t ~ t where K t = (~ t H T (H t (~ t H t T + R 1 R > sensor_noise costly calculation: (H t (~ t H t T + R 1 due to inverse. note: for high R (sensor noise, K t decreases leaving more weight on state u t for high t (state uncertainty, K t increases leaving more weight on sensor observations, z t C(~u. Kalman Filter pro: better than bayes filter (discretized states, bad for 3D cubic growth provides predicted error radius via uniformly distributed models Kalman Filter Algorithm O(k n 2 where k > measurement dimensions, n > state dimensions For each time step, Predict: apply motion model Correct: apply sensor model The prediction and correction steps don t have to be in the same time step, nor same frequency. Ongoing SLAM Approach specific library in ROS example on a robot using a Pi Colyette 6