AUTONOMOUS PERPENDICULAR AND PARALLEL PARKING USING MULTI-SENSOR - PowerPoint PPT Presentation

AUTONOMOUS PERPENDICULAR AND PARALLEL PARKING USING MULTI-SENSOR BASED CONTROL David Pérez-Morales, Olivier Kermorgant, Salvador Domínguez-Quijada and Philippe Martinet Updated: 2017/09/18

Ov erview 1. Modeling and Notation 1.1 Car-like robot model and notation 1.2 Multi-sensor modeling 2. Perception 3. Interaction Model 3.1 Task 3.2 Constraints 4. Control 5. Results 5.1 Unconstrained cases - MATLAB 5.2 Constrained cases 1

M ODELING AND NOTATION

Car-Like Robot Rear-Wheel Driving y 0 ϕ x m             x ˙ cos θ 0       l fo       y m l ls ˙ M  y   sin θ   0  θ       ˙ v + φ y �       ˙ θ tan φ/ l wb 0 l wb              ˙      l tr φ 0 1       l ro (1) l rs Where v and ˙ φ are the driving and x x 0 steering velocities. Figure: Kinematic model diagram for a car-like rear-wheel driving robot 3

Experimental Setup Velocity, direction of travel, steering and turning signals can be controlled by computer. Figure: Robotized Renault ZOE 4

Multi-sensor modeling In a static environment, the sensor feature deriva- tive can be expressed as 1 : i W m s i � L i v i � ˙ L i v m (2) ( d i × 6 ) ( 6 × 6 ) ( 6 × 1 ) S 1 1 W m     1 W m  L 1 . . . 0    F 1 4     2 0 -2     -4 -6 . . . 5 10 15 20 25 30 35 40 45 ... sensor     S 2 L s � LW m � . . . (3) signal     . . .     ( d × 6 ) F m     control k W m 0 . . . L k     frame F 2 F O ( d × 6 k ) ( 6 k × 6 ) object 2 W m 4 frame 2 0 -2 s � L s v m ˙ (4) -4 -6 sensor 5 10 15 20 25 30 35 40 45 Object of F 0 signal interest Under a planar world assumption: Figure: Multi-sensor model i W m s i � L i v i � ˙ L i v m (5) ( 3 × 3 ) ( 3 × 1 ) ( d i × 3 ) where v m � [ v x m , v y m , ˙ θ ] T 1 Kermorgant and Chaumette, “Dealing with constraints in sensor-based robot control” 5

Multi-sensor modeling y 0 ϕ x m Assuming v y m � 0 (no slipping nor skidding) l fo y m l ls M θ y v m � [ v x m , ˙ θ ] T l wb (6) l tr dim ( L s ) � ( d × 2 ) l ro l rs x where v x m � v . x 0 Figure: Kinematic model diagram for a car-like rear-wheel driving robot 6

Weighted error The weighted multi-sensor error signal is defined as: e H � He (7) where e � s − s ∗ is the difference between the current sensor signal s and its desired value s ∗ and H is a diagonal positive semi-definite weighting matrix that depends on the current value of s . Its associated interaction matrix is L H � HL s . 7

PERCEPTION

Perception 9

Extraction of empty parking place Spot Length d 1 d / 2 1 c 13 c 12 c 23 c 22 h t p e l s D t o 3 d p S c 14 c 11 c 24 c 21 d / 2 2 d 2 10

Parking spots Figure: Parking spot model for reverse � parking maneuvers Figure: ⊥ parking spot model Figure: Parking spot model for forward � parking maneuvers Table: Pair of points through which each line passes Line Perpendicular Parallel (reverse) Parallel (forward) i L 1 ( i p 5 , i p 6 ) ( i p 5 , i p 6 ) ( i p 5 , i p 6 ) i L 2 ( i p 1 , i p 4 ) ( i p 3 , i p 4 ) ( i p 1 , i p 2 ) i L 3 ( i p 3 , i p 4 ) ( i p 1 , i p 4 ) ( i p 1 , i p 4 ) i L 4 ( i p 1 , i p 2 ) ( i p 1 , i p 2 ) ( i p 3 , i p 4 ) 11

INTERACTION MODEL

Interaction Model The sensor signals s i L j and reduced inter- action matrix L i L j are defined respectively as: � i u j ( 1 ) , i u j ( 2 ) , i h j ( 3 ) � T s i L j � (8)    i u j ( 2 )  0 0     − i u j ( 1 )   L i L j � 0 0 (9)     − i u j ( 2 ) i u j ( 1 )   0   Figure: Sensors’ configuration and sensor features 13

Task sensor features Task sensor features s t � [ s i L 1 , s i L 2 ] T (10) s t is obtained from S 1 for forward maneu- vers and from S 2 for reverse ones. The corresponding interaction matrix is defined as: L L + L ∗ L L t � (11) 2 where L L � [ L i L 1 , L i L 2 ] T and L ∗ L is equal to the value of L L at the desired pose. Figure: Sensors’ configuration and sensor features 14

Weighting of the task sensor features The associated weighting matrix H t is defined as: 2 , h t const 5 , h t const H t � diag ( h t 1 , h t , h t 4 , h t ) (12) 3 6 where the values h t const and h t const 6 are con- 3 3 stant while the values of h t ∀ i � 1 , 2 , 4 , 5 i are computed using the following smooth weighting function: h + h + e t ( i ) e t ( i ) i i h t h t i i h - h - i i Figure: Sensors’ configuration and s - s s s - + s s + i s * i s s s * i i i i i i sensor features Figure: Weighting function h t i 15

Constraints (reverse perpendicular case) Constrained sensor features s c � [ s 3 , s 4 , s 5 ] T (13) The corresponding interaction matrix: L c � [ L 3 , L 4 , L 5 ] T (14) Figure: Lateral constraint d 16

C ONTROL

Control Control law v m � argmin || L H t v m + λ e H t || 2 (15) s.t. Av m ≤ b with: A � [ L c , − L c ] T (16) c − s c ) , − α ( s − c − s c )] T b � [ α ( s + (17) where α is a gain constant, λ is the control gain and [ s − c , s + c ] is the desired interval in which we want to keep s c . 18

Bounding the control signals The control signals v and φ and their increments are bounded as shown below: | v | < v max (18) | φ | < φ max (19) ( v n − 1 − ∆ dec ) ≤ v n ≤ ( v n − 1 + ∆ acc ) (20) ( φ n − 1 − ∆ φ ) ≤ φ n ≤ ( φ n − 1 + ∆ φ ) (21) 2 v max v 0 max 0 0 d th d stop (T=0) stop Figure: Distance to stop line Distance to stop line Figure: Deceleration profile 19

RESULTS

Unconstrained cases - MATLAB Linear velocity evolution (km/h) Parking maneuver evolution 0 10 -1 8 -2 6 0 5 10 15 20 25 30 35 time 4 Steering angle evolution (degs) 0 2 -20 0 -40 -2 0 5 10 15 20 25 30 35 -2 0 2 4 6 8 10 time (a) Performed maneuver (b) Control signals Task errors Task weights 10 4 e t (1) h t 1 e t (2) h t 2 3 e t (3) h tconst 5 3 e t (4) h t e t (5) 4 2 h t e t (6) 5 h tconst 0 6 1 -5 0 0 5 10 15 20 25 30 35 0 5 10 15 20 25 30 35 time time (c) Task error signal (d) Task features’ weights Figure: Perpendicular reverse parking maneuver 21

Unconstrained cases - MATLAB Linear velocity evolution (km/h) Parking maneuver evolution 2 10 1 8 0 6 0 5 10 15 20 25 30 35 time 4 Steering angle evolution (degs) 0 2 -20 0 -40 -2 0 5 10 15 20 25 30 35 -8 -6 -4 -2 0 2 4 time (a) Performed maneuver (b) Control signals Task errors Task weights h t 4 3 1 h t 2 2 2.5 h tconst 3 0 2 h t 4 e t (1) h t -2 1.5 5 e t (2) h tconst e t (3) 6 -4 1 e t (4) e t (5) -6 0.5 e t (6) -8 0 0 5 10 15 20 25 30 35 0 5 10 15 20 25 30 35 time time (c) Task error signal (d) Task features’ weights Figure: Perpendicular forward parking maneuver 22

Unconstrained cases - MATLAB Linear velocity evolution (km/h) Parking maneuver evolution 0 8 -1 6 -2 4 0 5 10 15 20 25 30 35 time 2 Steering angle evolution (degs) 40 0 0 -2 -40 -4 0 5 10 15 20 25 30 35 -2 0 2 4 6 8 10 12 time (a) Performed maneuver (b) Control signals Task errors Task weights 4 4 h t 1 2 h t 3 2 h tconst 0 3 e t (1) h t 4 -2 e t (2) 2 h t 5 e t (3) h tconst -4 e t (4) 6 1 e t (5) -6 e t (6) -8 0 0 5 10 15 20 25 30 35 0 5 10 15 20 25 30 35 time time (c) Task error signal (d) Task features’ weights Figure: Parallel reverse parking maneuver 23

Unconstrained cases - MATLAB Linear velocity evolution (km/h) Parking maneuver evolution 2 8 1 6 0 4 0 5 10 15 20 25 30 35 time 2 Steering angle evolution (degs) 40 0 0 -2 -40 -4 0 5 10 15 20 25 30 35 -8 -6 -4 -2 0 2 4 time (a) Performed maneuver (b) Control signals Task errors Task weights 8 5 e t (1) h t 1 6 e t (2) 4 h t 2 e t (3) h tconst 3 e t (4) 4 3 h t 4 e t (5) h t 2 2 e t (6) 5 h tconst 6 0 1 -2 0 0 5 10 15 20 25 30 35 0 5 10 15 20 25 30 35 time time (c) Task error signal (d) Task features’ weights Figure: Parallel forward parking maneuver 24

Unconstrained cases - MATLAB Linear velocity evolution (km/h) Parking maneuver evolution 0 10 -1 8 -2 6 0 5 10 15 20 25 30 35 time 4 Steering angle evolution (degs) 50 2 0 0 -2 -50 0 5 10 15 20 25 30 35 -2 0 2 4 6 8 10 12 time (a) Performed maneuver (b) Control signals Task errors Task weights 10 4 h t 1 h t 2 5 3 h tconst 3 h t 4 0 2 h t e t (1) 5 h tconst e t (2) 6 -5 1 e t (3) e t (4) e t (5) -10 0 0 5 10 15 20 25 30 35 0 5 10 15 20 25 30 35 e t (6) time time (c) Task error signal (d) Task features’ weights Figure: Perpendicular reverse parking maneuver from far 25

Constrained cases - Fast prototyping environment Figure: Constrained perpendicular reverse parking maneuver 26