FACOLTÀ DI INGEGNERIA DELL INFORMAZIONE ELECTIVE IN ROBOTICS. Quadrotor. Motion Planning Algorithms. Academic Year

Transcription

1 FACOLTÀ DI INGEGNERIA DELL INFORMAZIONE ELECTIVE IN ROBOTICS Quadrotor Motion Planning Algorithms Prof. Marilena Vendittelli Prof. Jean-Paul Laumond Jacopo Capolicchio Riccardo Spica Academic Year

2 Introduction Small-scale quadrotors Four pitch-fixed rotors disposed at the vertices of a square, whose directions of rotation are equal in pairs Vertical and lateral displacements, as well as yaw rotations, by varying the relative turning speeds of rotors Quadrotor - Motion Planning Algorithms Page 2

3 Introduction Pros and Cons Simple Mechanics Low cost Robustness High Maneuverability VTOL Low Payloads Poor sensors equipment Low computational capabilities Hovering Quadrotor - Motion Planning Algorithms Page 3

4 Dynamic Model Preliminaries SR I : world inertial frame SR B : body-attached frame Configuration = Position + Orientation of SR B w.r.t. SR I Assumptions: - Robot symmetry - No disturbances - Negligible gyroscopic and aerodynamic effects - Negligible motor dynamics - Low level controllers for blades rotational speed Quadrotor - Motion Planning Algorithms Page 4

5 Dynamic Model Preliminaries (2) f i = bω i 2 τ R,i = dω i 2 b: thrust factor d: drag factor. Quadrotor - Motion Planning Algorithms Page 5

6 Dynamic Model Newton-Euler approach (1) The orientation is described by using RPY angles (φ, θ, ψ): I R B = c ψ c ϑ c ψ s ϑ s φ s ψ c φ c ψ s ϑ c φ + s ψ s φ s ψ c ϑ s ψ s ϑ s φ + c ψ c φ s ψ s ϑ c φ c ψ s φ s ϑ c ϑ s φ c ϑ c φ Control inputs transformation: T = f 1 + f 2 + f 3 + f 4 τ φ = l f 2 f 4 τ θ = l f 1 f 3 τ ψ = τ R,1 + τ R,2 τ R,3 + τ R,4 Quadrotor - Motion Planning Algorithms Page 6

7 Dynamic Model Newton-Euler approach (2) The translational dynamics is governed by the Newton equation: m x y z = 0 0 mg I + R B 0 0 T The angular acceleration is governed by the Euler equation: I p q r + p q r I p q r = τ φ τ ϑ τ ψ For small roll and pitch angles: p q r = 1 0 sin ϑ 0 cos φ cos ϑ sin φ 0 sin φ cos ϑ cos φ φ ϑ ψ ~ φ θ ψ Quadrotor - Motion Planning Algorithms Page 7

8 Dynamic Model Newton-Euler approach (3) System state: ξ = x y z φ ϑ ψ x y z φ ϑ T ψ Then: x = cos ψ sin ϑ cos φ + sin ψ sin φ y = sin ψ sin ϑ cos φ cos ψ sin φ T m T m z = g cos ϑ cos φ T m φ = τ φ I x ϑ = τ ϑ I y ψ = τ ψ I z Quadrotor - Motion Planning Algorithms Page 8

9 Dynamic Model Differential flatness (1) Flat outputs: (x, y, z, ψ) From the dynamic model one obtains: and by derivation ϑ = atan φ = atan x cos ψ + y sin ψ z g x sin ψ y cos ψ z g cos ϑ ϑ = tan 2 ϑ z g x 3 + y ψ cos ψ + y 3 x ψ sin ψ z 3 tan ϑ 1 1 φ = 1 + tan 2 φ z g sin ψ y ψ + x 3 cos ϑ + y ϑ sin ϑ + cos ψ xψ + y 3 cos ϑ + y ϑ sin ϑ tan φ Quadrotor - Motion Planning Algorithms Page 9

10 Dynamic Model Differential flatness (2) ϑ = 2ϑ 2 tan ϑ z 3 ϑ z g tan 2 ϑ z g x 4 + 2y (3) ψ + y ψ x ψ 2 cos ψ + y 4 2x 3 ψ x ψ y ψ 2 sin ψ z 4 tan ϑ z (3) ϑ 1 + tan 2 ϑ φ = 2φ 2 tan φ z 3 φ z g tan 2 φ z g cos ψ (sin ϑ ϑ y 3 x + cos ϑϑ 2 y + sin ϑ ϑ y + cos ϑ y 4 + x 3 ψ + x ψ + x 3 ψ + y ψ 2 + sin ϑ ϑ y 3 x ψ + sin ψ (sin ϑ ϑ x 3 y + cos ϑϑ 2 x + sin ϑ ϑ x + cos ϑ x 4 + y 3 ψ + y ψ + y 3 ψ x ψ 2 + sin ϑ ϑ x 3 y ψ z 4 tan φ z (3) ϑ 1 + tan 2 φ φ Quadrotor - Motion Planning Algorithms Page 10

11 Dynamic Model Differential flatness (3) Finally the torque inputs are: τ φ = φ I x τ ϑ = ϑ I y while the thrust is: τ ψ = ψ I z T = m x 2 + y 2 + (z g) 2 For T all the derivatives of the flat outputs go to zero, then φ ϑ 0 0 and T τ φ τθ τ ψ mg Quadrotor - Motion Planning Algorithms Page 11

12 Dynamic Model Differential flatness (4) The robot is able to track any given trajectory of the 3D position and yaw angle provided that it is smooth eugh In particular to guarantee inputs continuity: the position trajectory has to be continue up to the fourth order derivative the yaw angle trajectory has to be continue up to the second order derivative To satisfy motors constraints T has to be large eugh Quadrotor - Motion Planning Algorithms Page 12

13 Solutions Motion Planning Introduction Aim : build a feasible trajectory going from a start to a goal, possibly considering yaw angle variations, in presence of obstacles and actuators constraints Visibility RRT Random input generation Closed-loop system Quadrotor - Motion Planning Algorithms Page 13

14 Visibility-Based Motion Planning Introduction The motion planning has been split in four steps: Creation of a visibility-based probabilistic roadmap in the space of flat outputs Searching of a piece-wise linear safe trajectory exploiting the roadmap Approximation of the computed trajectory through a smooth spline Time-scaling Quadrotor - Motion Planning Algorithms Page 14

15 Visibility-Based Motion Planning Visibility roadmap construction (1) Aim : build a roadmap with few des but still sufficient for solving the global planning problem For a given local method L, the visibility domain of q is the set of configurations reachable by q through L V L q = q CS free s. t. L q, q CS free q is the guard of V L q A set of guards constitutes a coverage of CS free if the union of their visibility domains covers CS free Note: such a finite coverage may t always exist, depending on both the shape of CS free and the local method L Quadrotor - Motion Planning Algorithms Page 15

16 Visibility-Based Motion Planning Visibility roadmap construction (2) A free configuration q is added to the roadmap iff one of the following conditions is satisfied: q is a guard q does t belong to the visibility domain of any other guard of the current roadmap it enlarges the coverage of CS free q is a connection q lies in the intersection of the visibility domains of at least two guards belonging to different connected components of the current roadmap it enhances the connectivity Quadrotor - Motion Planning Algorithms Page 16

17 Start ntry=0; n=# of des in the roadmap (0 at the beginning); ntry<m end Generate a random free configuration q (max_attempts tries) Vis_q = Ø; Vis_group_q = Ø; i=0 nvis = # of elements of vis_group_q i<n Add q to the roadmap as a guard; Create a new group containing q; ntry=0 nvis=0 r = de(i) Add q to the roadmap as a connection with an edge to any r of Vis_q; join the groups visible by q; ntry=0 nvis>1 r is a guard r ϵ V L (q) i=i+1 discard q; ntry=ntry+1 Vis_q = Vis_q U r; Vis_group_q = Vis_group_q U group(r) Quadrotor - Motion Planning Algorithms Page 17

18 Visibility-Based Motion Planning Visibility roadmap construction (4) Operating space: flat outputs σ = x y z ψ Collision checking: obstacle expansion arm lengt l = 0,25 m Local method: straight lines Quadrotor - Motion Planning Algorithms Page 18

19 Visibility-Based Motion Planning Visibility roadmap construction (5) = Guard = Connector = Sample Why t to exploit connections to increase the coverage? Quadrotor - Motion Planning Algorithms Page 19

20 Visibility-Based Motion Planning Visibility roadmap construction (6) In the alternative version of algorithm a free configuration q is added to the roadmap iff one of the following conditions is satisfied: q is a guard q does t belong to the visibility domain of any other de (either a guard or a connection) of the current roadmap q is a connection q lies in the intersection of the visibility domains of at least two des (either guards or connections) belonging to two different connected components of the current roadmap Quadrotor - Motion Planning Algorithms Page 20

21 Start ntry=0; n=# of des in the roadmap (0 at the beginning); ntry<m end Generate a random free configuration q (max_attempts tries) Vis_q = Ø; Vis_group_q = Ø; i=0 nvis = # of elements of vis_group_q i<n Add q to the roadmap as a guard; Create a new group containing q; ntry=0 nvis=0 r = de(i) i=i+1 Add q to the roadmap as a connection with an edge to any r of Vis_q; join the groups visible by q; ntry=0 nvis>1 r ϵ V L (q) Vis_q = Vis_q U r; Vis_group_q = Vis_group_q U group(r) discard q; ntry=ntry+1 Quadrotor - Motion Planning Algorithms Page 21

22 Visibility-Based Motion Planning Searching a safe trajectory (1) Aim : compute (if possible) a safe piece-wise linear path that goes from the starting position to the goal Such a trajectory requires the quadrotor to stop at each viapoint The searching of a safe solution is made iteratively: try to connect (using straight lines) start and goal to two des q s and q g belonging to the same connected component of the roadmap If success, a solution exists A* If t expand the roadmap and try again Quadrotor - Motion Planning Algorithms Page 22

23 Start Vis_start = Ø; Vis_group_start = Ø; Vis_goal = Ø; Vis_group_goal = Ø; ntry=0 i=0 r = de(i) r = de(i) r ϵ V L(goal) r ϵ V L(start) vis_goal = vis_goal U r; vis_group_gosl = vis_group_goal U group(r) Vis_start = Vis_start U r; Vis_group_start = Vis_group_start U group(r) i=i+1 i<n i=i+1 i<n nvis = # of elements of vis_group_goal nvis = # of elements of vis_group_start Add goal to the roadmap as a guard; Create a new group containing goal; ntry=0 nvis=0 Add start to the roadmap as a guard; Create a new group containing start; ntry=0 Add start to the roadmap as a connection with an edge to any r of Vis_start; join the groups visible by start; ntry=0 nvis=0 nvis>1 Add goal to the roadmap as a connection with an edge to any r of Vis_goal; join the groups visible by goal; ntry=0 nvis>1 Vis_group_start vis_group_goal = Ø i=0 Find a path with A* Path found ntry = ntry + 1 Expand graph end Path t found ntry<max_try Quadrotor - Motion Planning Algorithms Page 23

24 Visibility-Based Motion Planning Spline interpolation (1) Aim : compute a smooth solution approximating the piece-wise linear path obtained in the previous step Path computed by using a B-spline interpolating viapoints This kind of trajectory may significantly diverge from the safe piece-wise linear solution and consequently become unsafe Introduction of a viapoint in the middle of any unsafe stretch Quadrotor - Motion Planning Algorithms Page 24

25 Start Find an optimal trajectory interpolating all the n viapoints; safe = true; i=0; Add a viapoint in the middle of the stretch; n = n+1; safe=false; is ith stretch collision-free? i=i+1 i<n safe=true end Quadrotor - Motion Planning Algorithms Page 25

26 Visibility-Based Motion Planning Spline interpolation (3) Split the geometrical aspect from the temporal one s x τ s y τ s z τ s ψ τ C 4 : 0,1 R C 4 : 0,1 R C 4 : 0,1 R C 2 : 0,1 R 6 t order 4 t order such that s x τ i = x i, i = 1,, n s y τ i = y i, i = 1,, n s z τ i = z i, i = 1,, n s ψ τ i = ψ i, i = 1,, n with 0 = τ 1 < τ 2 < < τ n = 1 Quadrotor - Motion Planning Algorithms Page 26

27 Visibility-Based Motion Planning Spline interpolation (4) (τ 1, τ 2,, τ n ) are chosen by solving an optimization problem: Variables: δ i = τ i+1 τ i for i = 1,, n 1 Constraints: 0 < δ i < 1 for i = 1,, n 1 n 1 δ i i=1 = 1 Objective function: 0 1 a d 4 s x τ dτ d4 s y τ dτ d4 s z τ dτ b d2 s ψ τ dτ 2 2 dτ Quadrotor - Motion Planning Algorithms Page 27

28 Visibility-Based Motion Planning Time scaling (1) Aim : optimize the time needed to complete the trajectory Changing the time to navigate through the des by a factor of k ( s.t. t i = kτ i ) the result is simply a time-scaled version of the n-dimensional solution Quadrotor - Motion Planning Algorithms Page 28

29 Visibility-Based Motion Planning Time scaling (2) k is chosen by solving an optimization problem: Variables: k Constraints: T τ φ τ θ τ ψ T M τ φ,m τ θ,m τ ψ,m Objective function: k = T Quadrotor - Motion Planning Algorithms Page 29

30 Visibility-Based Motion Planning Alternative choice for ψ ψ is actuated independently we can neglect it during the roadmap construction Putting: ψ = atan2(y, x ) The robot points in the direction of motion and ψ = y x 2 y x y x x 2 ψ = y x 2 1 x y x 2 y x 2 x 2y x y x 3 2 y ψ 2 x Quadrotor - Motion Planning Algorithms Page 30

31 Visibility-Based Motion Planning Simulation scenario 7x7x3 m Quadrotor - Motion Planning Algorithms Page 31

32 Visibility-Based Motion Planning Start and Goal selection 6 4 Start Goal Quadrotor - Motion Planning Algorithms Page 32

33 Visibility-Based Motion Planning Roadmap construction 6 = Guard 4 = Connector Quadrotor - Motion Planning Algorithms Page 33

34 Visibility-Based Motion Planning Path computed by A* 6 = Guard 4 = Connector Quadrotor - Motion Planning Algorithms Page 34

35 Visibility-Based Motion Planning Path Computed after the Optimization 6 = Guard 4 = Connector Quadrotor - Motion Planning Algorithms Page 35

36 Visibility-Based Motion Planning Safe Path Computed after the Optimization 6 4 = Guard = Connector Quadrotor - Motion Planning Algorithms Page 36

37 T Visibility-Based Motion Planning Time Scaling x t 4 x t t Quadrotor - Motion Planning Algorithms Page 37

38 RRT-Based Motion Planning Introduction Key-idea: incrementally grow a search tree by applying control inputs over short time intervals to reach new des Features: Exploration biased toward unexplored portions of the space (Rapidly Exploring) Suitable for solving problems in high-dimensional spaces Takes into account both kinematic and dynamic constraints Generates a trajectory directly in the state space Provides control inputs needed to execute the trajectory Quadrotor - Motion Planning Algorithms Page 38

39 RRT-Based Motion Planning Basic iteration At each iteration: Extract a random state ξ ran Select ξ near (according to the metric) Compute u to steer the robot towards ξ ran Apply u for a time δt, reaching ξ near u ξ new If the path from ξ near to ξ new is safe, add ξ new to the tree and save u ξ start ξ new ξ ran Otherwise, discarded it Quadrotor - Motion Planning Algorithms Page 39

40 RRT-Based Motion Planning Trajectory reconstruction Once ξ last ξ goal < ε, the algorithm stops The solution trajectory can then be found by traversing the tree backwards from ξ last to ξ start The last stretch might be computed (if necessary) using any local planner ξ start ξ last ξ goal Quadrotor - Motion Planning Algorithms Page 40

41 RRT-Based Motion Planning Exploration vs. Exploitation The tree will eventually cover the connected component of CS free containing the start, coming arbitrarily close to any goal belonging to the same component Convergence is often slow switch between two phases: Exploration: the tree is expanded toward a random state Exploitation: the tree is expanded toward the goal (ξ ran = ξ goal ) ε-greedy strategy: ϵ = ϵ αk coin < ϵ exploration coin > ϵ exploitation Quadrotor - Motion Planning Algorithms Page 41

42 Start Create a tree with ξ start as route; k=0; Fails = 0; Compute ϵ(k) Extract a random "coin" in [0,1] Put ξ ran = ξ goal coin<ϵ Extract ξ ran Find ξ near in the tree; Compute u; Integrate for a time δt; Run collision check; Is colliding? Fails = Fails + 1; Add ξ new to the tree k = k+1; Fails = 0; ξ new ξ goal Fails < max_fails end Quadrotor - Motion Planning Algorithms Page 42

43 RRT-Based Motion Planning Random inputs choice At each iteration: Extract 10 random values of the motors velocities between 0 and 1000 rad/s Compute the corresponding thrust and torques using input transformation Integrate the equations of motion (ode45) for a constant time δt = 0.1 s Choose the input vector that brings (safely) the robot closer to ξ ran. Quadrotor - Motion Planning Algorithms Page 43

44 RRT-Based Motion Planning Random inputs choice (Simulation Results) 6 hours later Quadrotor - Motion Planning Algorithms Page 44

45 RRT-Based Motion Planning Closed-loop system Key-idea: use a combination of the RRT planning along with a controller to ensure that the quadrotor moves toward ξ ran Variable LQR controller: linearization around the current ξ near A = ξ ξ u= mg T ξ=ξ near B = K computed by Matlab lqr function ξ u u= mg T ξ=ξ near u = mg T K(ξ ξ ran ) Quadrotor - Motion Planning Algorithms Page 45

46 RRT-Based Motion Planning Closed-loop system (Simulation Results) Quadrotor - Motion Planning Algorithms Page 46

47 Conclusions (1) Visibility algorithm is suitable for finding a roadmap with few des easy to handle but, possibly t containing the shortest path = Guard = Connector Smart-cutting Quadrotor - Motion Planning Algorithms Page 47

48 Conclusions (2) The trajectory would be even smoother if the spline passed near to the des of the roadmap instead of interpolating them (more freedom to the optimizer) RRT planners seem to perform worse Enhancements may be achieved by: Tuning δt, ϵ and the number of random inputs Putting the system under the action of a controller Tuning the gains Q and R of the LQR filter or by using any other controller (even n-linear) Quadrotor - Motion Planning Algorithms Page 48

49 Quadrotor - Motion Planning Algorithms Pagina 49