Robotic Perception and Action: Vehicle SLAM Assignment

Transcription

1 Robotic Perception and Action: Vehicle SLAM Assignment Mariolino De Cecco Mariolino De Cecco, Mattia Tavernini 1

2 CONTENTS Vehicle SLAM Assignment Contents Assignment Scenario 3 Odometry Localization Matrix C Xk Matrix J Φwk Matrix C wk Matrix S wk Matrix I k Data Provided Aruco Localization UWB Positioning Problem 1 8 (Odometry Incremental Localization) Problem 2 8 (Odometry and Aruco Incremental Localization) Problem 3 9 (Odometry and Aruco SLAM) Problem 4 9 (Odometry, Aruco and UWB SLAM) Results 10 Page 2 of 10

3 Vehicle SLAM Assignment Assignment Scenario Consider a Differential Drive Vehicle ables to move in an unknown environment with the following sensors: Rotational encoders on both the wheel drive (Odometry Localization); RGB Camera capable to detect a certain number of artificial landmarks placed in the environment in unknown positions and orientations. An onboard computer is capable to collect the images and to extract the bi-dimensional pose of the landmarks identified (Aruco Localization); Ultra Wide Band Positioning, and indoor positioning system based on the radio technology with a set of receivers placed in the environment and a reference tag on the vehicle (UWB Positioning). Figure 1: Differential drive kinematic model and development robot The aim of the work is to perform the localization of the vehicle and when it is possible, to map the artificial landmark placed in the environment. In the next paragraphs there are more details on the localization system and the data provided. Odometry Localization Odometry (Incremental) Localization can be done with the knowledge of the vehicle kinematic model, the vehicle kinematic parameters and the encoder readings. Since the vehicle is controlled by the two driving wheels, the rotational encoders are mounted on the wheel. It can be assumed that both the encoders have the same characteristics. The measurements acquired are the incremental number of counts on wheels. The vehicle is localized by using the discrete form of odometric navigation reported in [1] and [2]: x k+1 = x k + π nrkr R +n Lk R L n 0 cos(δ k ) y k+1 = y k + π nrkr R +n Lk R L n 0 sin(δ k ) δ k+1 = δ k + 2π nrkr R n Lk R L where: b n 0 (1) x k+1 is the update vehicle x position starting from the previous knowledge x k, y k+1 is the update vehicle y position starting from the previous knowledge y k, δ k+1 is the update vehicle δ attitude starting from the previous knowledge δ k, n Rk is the incremental counter of the encoder on the right wheel, n Lk is the incremental counter of the encoder on the left wheel, n 0 is the counter per revolution of both the encoder (assuming that they have same characteristics), Assignment Scenario continued on next page... Page 3 of 10

4 Odometry Localization Vehicle SLAM Assignment Assignment Scenario (continued) R R is the radius of the right wheel, R L is the radius of the left wheel, b is the vehicle wheelbase. In the same article is reported the method to use in order to estimate the odometric localization uncertainty, starting from the uncertainty of the wheel radii, the wheelbase and the initial vehicle attitude. This formulation already include the uncertainty propagation, and id performed in this way: in which: C Xk+1 = C Xk + J Φwk C wk J T Φ wk + J Φwk S wk I T k + I k S wk J T Φ wk (2) In the next paragraphs are described these matrices. I k = I k 1 + J Φk 1 S wk 1 (3) Matrix C Xk The matrix C Xk represents the covariance matrix of the vehicle pose X k at the discrete time step k. With the previous formulation it is possible to estimate the covariance of the vehicle pose at each step, taking already into account the propagation uncertainty (and so the drift). Matrix J Φwk The matrix J Φwk is the Jacobian of the non linear function Φ wk that permits to perform the pose update: X k+1 = X k + Φ wk w k is the vector of the variables that affect the accuracy of the vehicle pose, in this case it is: w k = [R R, R L, b, δ k ] This means that the parameters that affect the accuracy on vehicle pose estimation are the uncertainty on wheel radii, the wheelbase and the attitude at the previous step. The Jacobian J Φwk has to be computed on the discrete vehicle pose update of equation (1). J Φwk = (x k+1 x k ) R R (y k+1 y k ) (x k+1 x k ) R L (y k+1 y k ) (x k+1 x k ) b (y k+1 y k ) R R (δ k+1 δ k ) R L (δ k+1 δ k ) b (δ k+1 δ k ) R R R L b (x k+1 x k ) δ k (y k+1 y k ) δ k (δ k+1 δ k ) δ k The algebraic formulation of the derivatives in the Jacobian are always the same, the Jacobian must be evaluated each time with the current values. Matrix C wk This matrix take care of the uncertainties of the variables on vector w k. Assuming that all this variables are independents, it can be simplified as: σ 2 R R C wk = 0 σr 2 L σb σδ 2 k The element σr 2 R, σr 2 L and σb 2 are always the same and are given in the parameter section. On the contrary, the value of σδ 2 k must be update at every iteration with the previous value that can be found in C Xk. Assignment Scenario continued on next page... Page 4 of 10

5 Aruco Localization Vehicle SLAM Assignment Assignment Scenario (continued) Matrix S wk The elements of the matrix S wk are simply the square roots of C wk elements. Matrix I k This matrix represents the integral terms of the encoder uncertainty propagation, it can be computed by using the equation (3). It can be initialized as < 3 4 > zeros matrix at the first step. Data Provided The data provided for this localization is the encoderreading.txt file, that is organized as follow: The other pose number Right encoder counter tick Left encoder counter tick n tickr tickl parameters for the vehicle localization are: Table 1: encoderreading.txt file organization Parameters Description Value R R Radius of the right wheel [meters] 0.1 R L Radius of the left wheel [meters] 0.1 b Vehicle wheelbase [meters] 0.4 n 0 Count per revolution (both encoders) 8000 σ RR Standard deviation on Right wheel radius [meters] 1e-4 σ RL Standard deviation on Right wheel radius [meters] 1e-4 σ b Standard deviation on wheelbase [meters] 1e-4 σ δk Initial Standard deviation on vehicle attitude [rads] 1e-4 Table 2: Vehicle kinematic model parameters Aruco Localization The Aruco Localization is based on a calibrated camera and a set of known markers on the ceiling as depicted on (Figure 2). Once the transformation between the camera sensor and the vehicle is known it is possible to estimate the pose of the Aruco marker in the vehicle reference system. The Aruco Localization is achieved by means of a computer vision algorithm that is capable to detect the Aruco marker and to estimate their 3D pose (Figure 3). Since the camera has a planar motion (it is mounted on the vehicle identified by its pose X = (x, y, δ)) and the markers are assumed to be parallel to the camera motion, the 3D pose can be translated into a 2D pose. The data provided for this localization are, for each vehicle pose and for each Aruco marker, the marker unique-id, its estimated bi-dimensional pose in the vehicle reference system and its uncertainty. This information are defined as follow: i is the vehicle pose index i = 1..n, Assignment Scenario continued on next page... Page 5 of 10

6 Aruco Localization Vehicle SLAM Assignment Assignment Scenario (continued) (a) Camera (b) Aruco Marker on ceiling Figure 2: Camera mounted on vehicle (a) used to detect the Aruco marker on ceiling (b) Figure 3: Software to detect the Aruco marker and estimated their 3D pose j is the landmark index j = 1..m, Zj i = (x j, y j, δ j ) i is the j th Aruco marker pose measurement from the i th vehicle pose, σxx 2 σxy 2 σ xδ 2 C Z i j = σyx 2 σyy 2 σyδ 2 σδx 2 σδy 2 σδδ 2 i j is the covariance matrix associated to the measurement Z i j. All this information are collected in the text file ArucoReading.txt. pose id landmark id x i j y i j δ i j σ xx σ xy σ xδ σ yy σ yδ σ δδ i j n j Table 3: ArucoReading.txt file organization Assignment Scenario continued on next page... Page 6 of 10

7 UWB Positioning Vehicle SLAM Assignment Assignment Scenario (continued) UWB Positioning The Ultra Wide Band (UWB) Positioning is a method that in the last years has been improved. Its configuration is very similar to the GPS Positioning, with the only difference that in this case is the tag that emits a UWB signal to the antennas positioned in the environment (Figure 4). Figure 4: Similarity between GPS positioning and UWB positioning The antennas can only return the distance of the tag (placed on the vehicle). The vehicle position has to be evaluated by using a triangulation algorithm as depicted in Figure 5. It must be noticed that the tag is described only by its position: with triangulation it is not possible to evaluate the vehicle attitude. For this reason the method is called Positioning (that means find the position (x i, y i ) of the vehicle)and not Localization (that means find the pose (x i, y i, δ i ) of the vehicle). Figure 5: Position estimation by triangulation The information achievable from this measurement system are: i is the vehicle pose index i = 1..p, k is the antenna index k = 1..m, rk i is the j th antenna distance measurement from the i th vehicle pose, σ r i k is the standard deviation associated to the distance measurement r i k. All this information are collected in the text file UwbReading.txt. Only for this positioning system are given a guess solution for the antenna positions in the environment, collected in the text file UwbAntennaGuess.txt. Page 7 of 10

8 Vehicle SLAM Assignment Assignment Scenario pose id antenna id r i k σ r i k i k.... n k.... Table 4: UwbReading.txt file organization antenna id x position y position k p.... Table 5: UwbAntennaGuess.txt file organization Problem 1 (Odometry Incremental Localization) In this first problem is request to perform the vehicle localization using the raw encoder data in the encoderreading.txt file. The incremental pose has to be estimated using the Equation 1 while the uncertainty can be estimated with the Equation 2. It is important to notice that the formulation of Equation 2 already include the uncertainty propagation. Problem 2 (Odometry and Aruco Incremental Localization) In this second problem, it is introduced the Aruco measurement. In particular is required to perform an Incremental Localization using the Aruco measurement (like the class exercise without encoder data). All the necessary information for this step can be found in the ArucoReading.txt file. N.B.: As in real measurement the measurement ratio of the Aruco markers is lower than the encoder. This means that while using encoder there is a measurement for each vehicle pose, using the Aruco only in some vehicle poses i there is a measurement. Facultative: As facultative improvement try to fuse the information of the Odometry Incremental Localization with the Aruco Incremental Localization. As hint, both the localization must be perform at same time, and the uncertainty propagation of encoder Equation 2 has to start from the previous fused (with Aruco) C Xk when there is an Aruco measurement. Page 8 of 10

9 Vehicle SLAM Assignment Problem 2 Problem 3 (Odometry and Aruco SLAM) Use the information of odometry and Aruco markers to perform a SLAM (Simultaneous Localization and Mapping) with the GTSAM library. The data to use in this case are: file encoderreading.txt file ArucoReading.txt As initial solution for the vehicle poses graph you can use the odometry incremental localization. Regarding the landmark initial pose for the graph, select the most appropriate way and justify the choice. Problem 4 (Odometry, Aruco and UWB SLAM) By using all the information given, develop a SLAM with the GTSAM library using in addition, with respect to the Problem 3, the information of the UWB sensors. The data to use in this case are: file encoderreading.txt file ArucoReading.txt file UwbReading.txt For the initial solution of the UWB Antenna positions use the guess in the file UwbAntennaGuess.txt. Provide the map of the Aruco markers and UWB antennas. N.B.1:: UWB antennas are identified with a position and not a pose N.B.1:: UWB antennas and Aruco markers are different landmarks, placed in different place in the environment. Page 9 of 10

10 Vehicle SLAM Assignment Problem 4 Results They are provided two datasets (a short and a long path). For each problem (and dataset), provide (when it is possible) the following data: vehiclepose.mat Structure with the reconstructed vehicle pose and covariance: ArucoMap.mat Structure with the solved Aruco map: UwbMap.mat Structure with the solved UWB map: vehiclep ose(..).pose = [x, y, δ] vehiclep ose(..).covari =< 3x3 > matrix ArucoMap(..).pose = [x, y, δ] ArucoM ap(..).covari =< 3x3 > matrix UwbMap(..).position = [x, y] UwbMap(..).covariance =< 2x2 > matrix References [1] Mariolino De Cecco, Luca Baglivo, and Francesco Angrilli. Real-Time Uncertainty Estimation of Autonomous Guided Vehicle Trajectory Taking Into Account Correlated and Uncorrelated Effects. IEEE T. Instrumentation and Measurement, 56(3): , [2] Mariolino De Cecco. Sensor fusion of inertial-odometric navigation as a function of the actual manoeuvres of autonomous guided vehicles. Measurement Science and Technology, 14(5):643, Page 10 of 10