Lecture 8: Kinematics: Path and Trajectory Planning

Transcription

1 Lecture 8: Kinematics: Path and Trajectory Planning Concept of Configuration Space c Anton Shiriaev. 5EL158: Lecture 8 p. 1/20

2 Lecture 8: Kinematics: Path and Trajectory Planning Concept of Configuration Space Path Planning Potential Field Approach Probabilistic Road Map Method c Anton Shiriaev. 5EL158: Lecture 8 p. 1/20

3 Lecture 8: Kinematics: Path and Trajectory Planning Concept of Configuration Space Path Planning Potential Field Approach Probabilistic Road Map Method Trajectory Planning c Anton Shiriaev. 5EL158: Lecture 8 p. 1/20

4 Concept of Configuration Space Given a robot with n-links, A complete specification of location of the robot is called its configuration c Anton Shiriaev. 5EL158: Lecture 8 p. 2/20

5 Concept of Configuration Space Given a robot with n-links, A complete specification of location of the robot is called its configuration The set of all possible configurations is known as the configuration space Q = { q } c Anton Shiriaev. 5EL158: Lecture 8 p. 2/20

6 Concept of Configuration Space Given a robot with n-links, A complete specification of location of the robot is called its configuration The set of all possible configurations is known as the configuration space Q = { q } For example, for 1-link revolute arm Q is the set of all possible orientations of the link, i.e. Q = S 1 or Q = SO(2) c Anton Shiriaev. 5EL158: Lecture 8 p. 2/20

7 Concept of Configuration Space Given a robot with n-links, A complete specification of location of the robot is called its configuration The set of all possible configurations is known as the configuration space Q = { q } For example, for 1-link revolute arm Q is the set of all possible orientations of the link, i.e. Q = S 1 or Q = SO(2) For example, for 2-link planar arm with revolute joints Q = S 1 S 1 = T 2 torus c Anton Shiriaev. 5EL158: Lecture 8 p. 2/20

8 Concept of Configuration Space Given a robot with n-links, A complete specification of location of the robot is called its configuration The set of all possible configurations is known as the configuration space Q = { q } For example, for 1-link revolute arm Q is the set of all possible orientations of the link, i.e. Q = S 1 or Q = SO(2) For example, for 2-link planar arm with revolute joints Q = S 1 S 1 = T 2 torus For example, for a rigid object moving on a plane Q = { x, y, θ } = R 2 S 1 c Anton Shiriaev. 5EL158: Lecture 8 p. 2/20

9 Concept of Configuration Space Given a robot with n-links and its configuration space, Denote W the subset of R 3 where the robot moves. It is called workspace of the robot c Anton Shiriaev. 5EL158: Lecture 8 p. 3/20

10 Concept of Configuration Space Given a robot with n-links and its configuration space, Denote W the subset of R 3 where the robot moves. It is called workspace of the robot The workspace W might contains obstacles O i c Anton Shiriaev. 5EL158: Lecture 8 p. 3/20

11 Concept of Configuration Space Given a robot with n-links and its configuration space, Denote W the subset of R 3 where the robot moves. It is called workspace of the robot The workspace W might contains obstacles O i Denote A a subset of workspace W, which is occupied by the robot, A = A(q) c Anton Shiriaev. 5EL158: Lecture 8 p. 3/20

12 Concept of Configuration Space Given a robot with n-links and its configuration space, Denote W the subset of R 3 where the robot moves. It is called workspace of the robot The workspace W might contains obstacles O i Denote A a subset of workspace W, which is occupied by the robot, A = A(q) Introduce a subset of configuration space that occupied by obstacles QO := { q Q : A(q) O i, i } c Anton Shiriaev. 5EL158: Lecture 8 p. 3/20

13 Concept of Configuration Space Given a robot with n-links and its configuration space, Denote W the subset of R 3 where the robot moves. It is called workspace of the robot The workspace W might contains obstacles O i Denote A a subset of workspace W, which is occupied by the robot, A = A(q) Introduce a subset of configuration space that occupied by obstacles QO := { q Q : A(q) O i, i } Then collision-free configurations are defined by Q free := Q \ QO c Anton Shiriaev. 5EL158: Lecture 8 p. 3/20

14 (a) The end-effector of the robot has a from of triangle. It moves in a plane. The plane contains a rectangular obstacle. (b) QO is the set with the dashed boundary c Anton Shiriaev. 5EL158: Lecture 8 p. 4/20

15 (a) Two-links planar arm robot. The workspace has a single square obstacle. (b) The configuration space and the set QO occupied by the obstacle is in gray. c Anton Shiriaev. 5EL158: Lecture 8 p. 5/20

16 Lecture 8: Kinematics: Path and Trajectory Planning Concept of Configuration Space Path Planning Potential Field Approach Probabilistic Road Map Method Trajectory Planning c Anton Shiriaev. 5EL158: Lecture 8 p. 6/20

17 Path Planning Problem of Path Planning is the task to find a path in the configuration space Q that connects an initial configuration q s to a final configuration q f that does not collide any obstacle as the robot traverses the path. c Anton Shiriaev. 5EL158: Lecture 8 p. 7/20

18 Path Planning Problem of Path Planning is the task to find a path in the configuration space Q that connects an initial configuration q s to a final configuration q f that does not collide any obstacle as the robot traverses the path. Formally, the task is to find a continuous function γ( ) such that γ : [0, 1] Q free with γ(0) = q s, and γ(1) = q f c Anton Shiriaev. 5EL158: Lecture 8 p. 7/20

19 Path Planning Problem of Path Planning is the task to find a path in the configuration space Q that connects an initial configuration q s to a final configuration q f that does not collide any obstacle as the robot traverses the path. Formally, the task is to find a continuous function γ( ) such that γ : [0, 1] Q free with γ(0) = q s, and γ(1) = q f Common additional requirements: Some intermediate points q i can be given Smoothness of a path Optimality (length, curvature, etc) c Anton Shiriaev. 5EL158: Lecture 8 p. 7/20

20 Path Planning: Potential Field Approach Basic idea: Treat a robot as a particle under an influence of an artificial potential field U( ); c Anton Shiriaev. 5EL158: Lecture 8 p. 8/20

21 Path Planning: Potential Field Approach Basic idea: Treat a robot as a particle under an influence of an artificial potential field U( ); Function U( ) should have global minimum at q f this point is attractive maximum or to be + in the points of QO these points repel the robot c Anton Shiriaev. 5EL158: Lecture 8 p. 8/20

22 Path Planning: Potential Field Approach Basic idea: Treat a robot as a particle under an influence of an artificial potential field U( ); Function U( ) should have global minimum at q f this point is attractive maximum or to be + in the points of QO these points repel the robot Try to find such function U( ) constructed in a simple from, where we can easily add or remove an obstacle and change q f. The common form for U( ) is U(q) = U att (q)+ ( ) U rep (1) (q) + U(2) rep (q) + + U(N) rep (q) c Anton Shiriaev. 5EL158: Lecture 8 p. 8/20

23 Path Planning: Probabilistic Road Map Method Basic idea: Sample randomly the configuration space Q; Those samples that belong to QO are disregarded; c Anton Shiriaev. 5EL158: Lecture 8 p. 9/20

24 Path Planning: Probabilistic Road Map Method Basic idea: Sample randomly the configuration space Q; Those samples that belong to QO are disregarded; Connecting Pairs of Configuration, e.g. Choose the way measure the distance d( ) in Q Choose ε > 0 and find k neighbors of distance no more than ε that can be connected to the current one This step will result in fragmentation of the workspace consisting of several disjoint components c Anton Shiriaev. 5EL158: Lecture 8 p. 9/20

25 Path Planning: Probabilistic Road Map Method Basic idea: Sample randomly the configuration space Q; Those samples that belong to QO are disregarded; Connecting Pairs of Configuration, e.g. Choose the way measure the distance d( ) in Q Choose ε > 0 and find k neighbors of distance no more than ε that can be connected to the current one This step will result in fragmentation of the workspace consisting of several disjoint components Make enhancement, that is, try to connect disjoint components c Anton Shiriaev. 5EL158: Lecture 8 p. 9/20

26 Path Planning: Probabilistic Road Map Method Basic idea: Sample randomly the configuration space Q; Those samples that belong to QO are disregarded; Connecting Pairs of Configuration, e.g. Choose the way measure the distance d( ) in Q Choose ε > 0 and find k neighbors of distance no more than ε that can be connected to the current one This step will result in fragmentation of the workspace consisting of several disjoint components Make enhancement, that is, try to connect disjoint components Try to compute a smooth path from a family of points c Anton Shiriaev. 5EL158: Lecture 8 p. 9/20

27 Steps in constructing probabilistic roadmap c Anton Shiriaev. 5EL158: Lecture 8 p. 10/20

28 Lecture 8: Kinematics: Path and Trajectory Planning Concept of Configuration Space Path Planning Potential Field Approach Probabilistic Road Map Method Trajectory Planning c Anton Shiriaev. 5EL158: Lecture 8 p. 11/20

29 Trajectory Planning Trajectory is a path γ : [0, 1] Q free with explicit parametrization of time [T s, T f ] t τ [0, 1] : q(t) = γ(τ) Q free c Anton Shiriaev. 5EL158: Lecture 8 p. 12/20

30 Trajectory Planning Trajectory is a path γ : [0, 1] Q free with explicit parametrization of time [T s, T f ] t τ [0, 1] : q(t) = γ(τ) Q free This means that we make specifications on velocity d q(t) of a motion; dt acceleration d2 dt 2 q(t) of a motion; jerk d3 dt 3 q(t) of a motion;... c Anton Shiriaev. 5EL158: Lecture 8 p. 12/20

31 Trajectory Planning Trajectory is a path γ : [0, 1] Q free with explicit parametrization of time [T s, T f ] t τ [0, 1] : q(t) = γ(τ) Q free This means that we make specifications on velocity d q(t) of a motion; dt acceleration d2 dt 2 q(t) of a motion; jerk d3 dt 3 q(t) of a motion;... In fact, it is common that the path is not given completely, but as a family of snap-shots q s, q 1, q 2, q 3,..., q f So that we have substantial freedom in generating trajectories. c Anton Shiriaev. 5EL158: Lecture 8 p. 12/20

32 Decomposition of a path into segments with fast and slow velocity profiles c Anton Shiriaev. 5EL158: Lecture 8 p. 13/20

33 Trajectories for Point to Point Motion Consider the i th joint of a robot and suppose that the specification at time t = t 0 is : q i (t 0 ) = q 0, d dt q(t 0) = v 0 c Anton Shiriaev. 5EL158: Lecture 8 p. 14/20

34 Trajectories for Point to Point Motion Consider the i th joint of a robot and suppose that the specification at time t = t 0 is : q i (t 0 ) = q 0, at time t = t 1 is : q i (t 1 ) = q 1, d dt q(t 0) = v 0 d dt q(t 1) = v 1 c Anton Shiriaev. 5EL158: Lecture 8 p. 14/20

35 Trajectories for Point to Point Motion Consider the i th joint of a robot and suppose that the specification at time t = t 0 is : q i (t 0 ) = q 0, at time t = t f is : q i (t f ) = q f, d dt q(t 0) = v 0 d dt q(t f) = v f In addition, we might be given constraints of accelerations d 2 dt 2 q(t 0 ) = α 0 d 2 dt 2 q(t f ) = α f c Anton Shiriaev. 5EL158: Lecture 8 p. 14/20

36 Trajectories for Point to Point Motion Consider the i th joint of a robot and suppose that the specification at time t = t 0 is : q i (t 0 ) = q 0, at time t = t f is : q i (t f ) = q f, d dt q(t 0) = v 0 d dt q(t f) = v f In addition, we might be given constraints of accelerations d 2 dt 2 q(t 0 ) = α 0 d 2 dt 2 q(t f ) = α f If we choose to generate a polynomial q(t) = a 0 + a 1 t + a 2 t a m t m that will satisfy the interpolation constraints, what degree this polynomial should be chosen? c Anton Shiriaev. 5EL158: Lecture 8 p. 14/20

37 Trajectories for Point to Point Motion The interpolation constraints at time t = t 0 is : q i (t 0 ) = q 0, at time t = t f is : q i (t f ) = q f, d dt q(t 0) = v 0 d dt q(t f) = v f for the 3 rd -order polynomial q(t) = a 0 + a 1 t + a 2 t 2 + a 3 t 3, d dt q(t) = a 1 + 2a 2 t + 3a 3 t 2 are c Anton Shiriaev. 5EL158: Lecture 8 p. 15/20

38 Trajectories for Point to Point Motion The interpolation constraints at time t = t 0 is : q i (t 0 ) = q 0, at time t = t f is : q i (t f ) = q f, d dt q(t 0) = v 0 d dt q(t f) = v f for the 3 rd -order polynomial q(t) = a 0 + a 1 t + a 2 t 2 + a 3 t 3, d dt q(t) = a 1 + 2a 2 t + 3a 3 t 2 are q 0 = a 0 + a 1 t 0 + a 2 t a 3t 3 0 v 0 = a 1 + 2a 2 t 0 + 3a 3 t 2 0 q f = a 0 + a 1 t f + a 2 t 2 f + a 3t 3 f v f = a 1 + 2a 2 t f + 3a 3 t 2 f c Anton Shiriaev. 5EL158: Lecture 8 p. 15/20

39 Trajectories for Point to Point Motion The equations q 0 = a 0 + a 1 t 0 + a 2 t a 3t 3 0 v 0 = a 1 + 2a 2 t 0 + 3a 3 t 2 0 q f = a 0 + a 1 t f + a 2 t 2 f + a 3t 3 f v f = a 1 + 2a 2 t f + 3a 3 t 2 f written in matrix form are q 0 v 0 q f = 1 t 0 t 2 0 t t 0 3t t f t 2 f t 3 f a 0 a 1 a 2 v f 0 1 2t f 3t 2 f a 3 c Anton Shiriaev. 5EL158: Lecture 8 p. 16/20

40 Trajectories for Point to Point Motion The equations q 0 = a 0 + a 1 t 0 + a 2 t a 3t 3 0 v 0 = a 1 + 2a 2 t 0 + 3a 3 t 2 0 q f = a 0 + a 1 t f + a 2 t 2 f + a 3t 3 f v f = a 1 + 2a 2 t f + 3a 3 t 2 f written in matrix form are q 0 v 0 q f = 1 t 0 t 2 0 t t 0 3t t f t 2 f t 3 f a 0 a 1 a 2 v f 0 1 2t f 3t 2 f a 3 What is the determinant of this matrix? c Anton Shiriaev. 5EL158: Lecture 8 p. 16/20

41 The parameters for interpolation t = 0 and t f = 1, q 0 = 10 and q f = 20, v 0 = v f = 0 what is wrong with the trajectory? c Anton Shiriaev. 5EL158: Lecture 8 p. 17/20

42 Trajectories for Point to Point Motion Consider the i th joint of a robot and suppose that the specification at time t = t 0 is : q i (t 0 ) = q 0, at time t = t f is : q i (t f ) = q f, d dt q(t 0) = v 0 d dt q(t f) = v f and additional constraints of accelerations d 2 dt 2 q(t 0 ) = α 0 d 2 dt 2 q(t f ) = α f c Anton Shiriaev. 5EL158: Lecture 8 p. 18/20

43 Trajectories for Point to Point Motion Consider the i th joint of a robot and suppose that the specification at time t = t 0 is : q i (t 0 ) = q 0, at time t = t f is : q i (t f ) = q f, d dt q(t 0) = v 0 d dt q(t f) = v f and additional constraints of accelerations d 2 dt 2 q(t 0 ) = α 0 d 2 dt 2 q(t f ) = α f To find interpolating polynomial we need to choose a polynomial of order 5 q(t) = a 0 + a 1 t + a 2 t 2 + a 3 t 3 + a 4 t 4 + a 5 t 5 c Anton Shiriaev. 5EL158: Lecture 8 p. 18/20

44 The parameters for interpolation t = 0 and t f = 2, q 0 = 0 and q f = 20, v 0 = v f = 0 c Anton Shiriaev. 5EL158: Lecture 8 p. 19/20

45 Interpolation by LSPB: Linear segments with parabolic blends c Anton Shiriaev. 5EL158: Lecture 8 p. 20/20