Dual-Rate Extended Kalman Filter Based Path-Following Motion Control for an Unmanned Ground Vehicle: Realistic Simulation
Abstract
:1. Introduction
- Consideration of DREKF, which enables one to:
- -
- Design a fast-rate dynamic controller capable of reaching the desired specifications for the UGV and precisely following the predefined path.
- -
- Generate fast-rate state estimates from slow-rate measurements to be supplied to the dynamic controller.
- -
- Face non-linear UGV dynamics and possible Gaussian-like modeling and measurement uncertainties.
- Development of a powerful simulation tool, which takes into account complex modeling aspects to realistically represent the UGV behavior.
2. Problem Scenario
- Robot simulator, which contains some complex UGV modeling aspects as a result of using the features of the specialized Simscape Multibody simulation tool.
- Control structure, which includes a path tracking controller (in this case, the Pure Pursuit algorithm), an inverse kinematics computation block, a dynamic proportional integral (PI) controller, and a state estimator (in this case, the proposed DREKF).
- The UGV inverse kinematics block transforms the velocity references into dynamic references .
- From this dynamic reference and the estimated angular velocities , the dynamic PI controller computes the control signal to be applied to the UGV , which respectively are the control actions at period T for the right and left motors. These control actions will be applied under Zero Order Hold (ZOH) conditions.
- The UGV is equipped with a virtual beacon. Four fixed beacons are additionally placed on the walls of the simulation environment, emulating a beacon based indoor positioning system. The measurements are the distances between the virtual mobile beacon and the four fixed beacons, which are located in a known place with respect to the world system of the simulator. The simulation tool is able to work from these distances or, alternatively, from the direct pose information . Distances and pose can be equivalently deduced by applying the Pithagorean theorem (see, e.g., [26] and more details in Section 3).
- Any UGV output measurement , , or may be disturbed by Gaussian noises, which will be created from a set of independent seeds in order to generate a reproducible pseudo-random noise. As a consequence, the experiments developed with the simulator will be reproducible under the same conditions.
- The system state estimate is computed via the DREKF. The prediction step is generated at period T from the control actions . The correction step is also obtained at period T, but from data sensed at the two different periods, that is, , and , or . More details can be found in Section 3.
3. Dual-Rate Extended Kalman Filter
3.1. Kinematic and Dynamic UGV Modeling
3.2. DREKF Algorithm
- Prediction of the next state and propagation of the covariance :, where , being the expectation, and , and where and are Jacobian matrices computed in order to respectively linearize the process model about the current state and about the process noise:
- Prediction of the future output , being for , and for :
- Computation of the Kalman filter gain :
- Correction of the state and correction of the covariance :
3.3. Beacon Measurement Model
4. UGV Modeling. Simulation Tool
4.1. Preliminary Considerations
- Vision-based system: Using a zenithal camera the position and orientation of the vehicle can be measured. The camera sees what the vehicle is doing from above and can provide information about the time evolution of the coordinates and angular position . The main drawback is that the vehicle must lie under the camera, which is a great limitation for the desired trajectory.
- GPS: This is probably the best way to measure the real trajectory to be compared with the desired one. The main drawback is that it must be used outdoors, and the resolution is not suitable to be used with small vehicles as the one is being used in this work.
- Beacons: Suitable to be used indoors it will probably be the best solution. As mentioned in previous sections, position can be measured based on the distance from the vehicle to several (n) fixed beacons. Orientation can be measured based on information provided by an inertial measurement unit. The drawback is the measurement noise and the lack of precision when using small vehicles.
4.2. Modeling Aspects. Tool Description
4.2.1. Wheels-on-the-Air Simulation Model
4.2.2. Wheels-on-the-Ground Simulation Model
4.3. Parameter Adjustments
- -
- The geometric parameters of the UGV in (1), that is, the wheel radius m and the half track width between wheels m.
- -
- The parameters used for the pure pursuit algorithm: velocity constant reference m/s, and look ahead distance m.
- -
- The Gaussian noises are generated by considering zero mean and variance .
- -
- The solver chosen is Ode4 Runge–Kutta, running at a fixed step of ms.
5. Simulation
5.1. Cases Evaluated
- Direct pose and angular velocity: in this experiment, output measurements are directly sampled from the simulator block at different periods T = 0.1 s, T = 0.2 s, and T = 0.5 s. No noise is considered.
- Odometry: in this case, only angular velocities are sampled at T = 0.1 s. Pose data are estimated by odometry. Ideally, no noise may be considered. However, in real environments, where for instance an encoder may be needed to take the velocities, some measurement noise may appear. In this simulation, two cases are considered: without noise and with noise (assuming additive Gaussian noises and limited encoder resolution).
- Dual-rate Extended Kalman Filter: this is the case exposed in Section 2. Gaussian noises are assumed in pose and velocities . Two options are simulated: and .
- Dual-rate Extended Kalman Filter with beacon distances: this is the case stated in Section 3.2. Gaussian noises are assumed in pose and velocities . For the sake of clarity, only the option for will be presented.
5.2. Cost Indexes for Performance Assessment
- , which is based on the -norm, and its goal is to provide a measure (in meters) about how accurately the path is followed:
- , which is based on the -norm and is defined to know the maximum difference (in meters) between the desired path and the current UGV position:
- , which measures the total amount of time (in seconds) elapsed to arrive at the final destination:
6. Results
- The direct pose case at s (D.P. 0.2) worsens its behavior with respect to the nominal case, since the trajectory presents oscillations, which is confirmed by the clear increase of every cost index. On average, , , and increase their values , , and , respectively.
- The direct pose case at s (D.P. 0.5) presents an unstable response, not being able to track the path in any case.
- The odometry case without noise at s (Odom) does not show so many oscillations like the D.P. 0.2 case, but the tracking seems to be not so accurate as in the nominal (D.P. 0.1) case. This analysis is corroborated by the cost indexes, which show a worsening with respect to the nominal case: , , and are respectively increased , , and , which are lower increases than in the D.P 0.2 case. The worsening is due to the addition of a systematic numerical error over time that typically appears when odometry is used.
- The odometry case with noise at s (Odom N) depicts a considerable path tracking worsening for the square reference, and is incapable of following the Lissajous curve. The cost indexes confirm this statement, since they are highly increased or ∞, respectively.
- The DREKF case with (DREKF 10) shows an accurate path tracking, despite having scarce pose measurements (10 times less), and it assumes noise and non-linearities. The cost indexes indicate the achievement of satisfactory control properties, since , , and are slightly worsened with respect to the nominal case (, , and , respectively), and even and outperform the Odom case.
- The DREKF case with (DREKF 50) presents a worse response than DREKF 10, as expected (DREKF 50 is provided with 5 times less measurements). The cost indexes and with respect to the DREKF 10 case are increased and , respectively; is very similar. Despite having five times fewer measurements, if DREKF 50 is compared with D.P. 0.2, both cases reach similar cost indexes. The main difference between them obeys the way of path tracking: whereas the D.P. 0.2 case presents oscillations, the DREKF 50 case depicts a smooth trajectory with underdamped response.
- The DREKF case with using beacon distances (DREKF-D) depicts a quite close response to the DREKF 10 case, which is confirmed by achieving very similar cost index values.
7. Conclusions
Author Contributions
Funding
Institutional Review Board Statement
Informed Consent Statement
Data Availability Statement
Conflicts of Interest
Abbreviations
CAD | Computer Aided Design |
DC | Direct Current |
DREKF | Dual-Rate Extended Kalman Filter |
PI | Proportional Integral |
UGV | Unmanned Ground Vehicle |
References
- Lozano-Perez, T. Autonomous Robot Vehicles; Springer Science & Business Media: Berlin/Heidelberg, Germany, 2012. [Google Scholar]
- Salt, J.; Alcaina, J.; Cuenca, Á.; Baños, A. Multirate control strategies for avoiding sample losses. Application to UGV path tracking. ISA Trans. 2020, 101, 130–146. [Google Scholar] [CrossRef] [PubMed]
- Guérin, F.; Guinand, F.; Brethé, J.F.; Pelvillain, H. Towards an autonomous warehouse inventory scheme. In Proceedings of the 2016 IEEE Symposium Series on Computational Intelligence (SSCI), Athens, Greece, 6–9 December 2016; pp. 1–8. [Google Scholar]
- Naranjo, J.E.; Clavijo, M.; Jiménez, F.; Gomez, O.; Rivera, J.L.; Anguita, M. Autonomous vehicle for surveillance missions in off-road environment. In Proceedings of the IEEE Intelligent Vehicle Symposium, Gothenburg, Sweden, 19–22 June 2016; pp. 98–103. [Google Scholar]
- Masood, K.; Zoppi, M.; Fremont, V.; Molfino, R.M. From Drive-By-Wire to Autonomous Vehicle: Urban Freight Vehicle Perspectives. Sustainability 2021, 13, 1169. [Google Scholar] [CrossRef]
- Guastella, D.C.; Muscato, G. Learning-based methods of perception and navigation for ground vehicles in unstructured environments: A review. Sensors 2021, 21, 73. [Google Scholar] [CrossRef] [PubMed]
- Berns, K.; Nezhadfard, A.; Tosa, M.; Balta, H.; De Cubber, G. Unmanned ground robots for rescue tasks. In Search and Rescue Robotics-From Theory to Practice; IntechOpen: London, UK, 2017. [Google Scholar]
- Wang, Y. Design Example of Planetary Exploration Mobile Robot. In Space Robotics; Springer Nature Singapore Pte Ltd.: Singapore, 2021; pp. 289–319. [Google Scholar]
- Moisiadis, V.; Tsolakis, N.; Katikaridis, D.; Sørensen, C.G.; Pearson, S.; Bochtis, D. Mobile Robotics in Agricultural Operations: A Narrative Review on Planning Aspects. Appl. Sci. 2020, 10, 3453. [Google Scholar] [CrossRef]
- Aguiar, A.P.; Hespanha, J.P. Trajectory-tracking and path-following of underactuated autonomous vehicles with parametric modeling uncertainty. IEEE Trans. Autom. Control 2007, 52, 1362–1379. [Google Scholar] [CrossRef] [Green Version]
- Simon, D. Optimal State Estimation: Kalman, H Infinity, and Nonlinear Approaches; John Wiley & Sons: Hoboken, NJ, USA, 2006. [Google Scholar]
- Hesar, H.D.; Mohebbi, M. A Multi Rate Marginalized Particle Extended Kalman Filter for P and T Wave Segmentation in ECG Signals. IEEE J. Biomed. Health Inform. 2018, 23, 112–122. [Google Scholar] [CrossRef] [PubMed]
- Akhbari, M.; Shamsollahi, M.B.; Jutten, C. Twave alternans detection in ecg using Extended Kalman Filter and dualrate EKF. In Proceedings of the 22nd European Signal Processing Conference (EUSIPCO), Lisbon, Portugal, 1–5 September 2014; pp. 2500–2504. [Google Scholar]
- Grillo, C.; Vitrano, F. State estimation of a nonlinear unmanned aerial vehicle model using an Extended Kalman Filter. In Proceedings of the 15th AIAA International Space Planes and Hypersonic Systems and Technologies Conference, Dayton, OH, USA, 28 April–1 May 2008; p. 2529. [Google Scholar]
- Armesto, L.; Tornero, J.; Vincze, M. Fast ego-motion estimation with multi-rate fusion of inertial and vision. Int. J. Robot. Res. 2007, 26, 577–589. [Google Scholar] [CrossRef]
- Armesto, L.; Tornero, J. SLAM based on Kalman filter for multi-rate fusion of laser and encoder measurements. In Proceedings of the International Conference on Intelligent Robots and Systems (IROS), Sendai, Japan, 28 September–2 October 2004; Volume 2, pp. 1860–1865. [Google Scholar]
- Salt Ducajú, J.M.; Salt Llobregat, J.J.; Cuenca, Á.; Tomizuka, M. Autonomous Ground Vehicle Lane-Keeping LPV Model-Based Control: Dual-Rate State Estimation and Comparison of Different Real-Time Control Strategies. Sensors 2021, 21, 1531. [Google Scholar] [CrossRef] [PubMed]
- Cardona, M.; Garcia Cena, C.E.; Serrano, F.; Saltaren, R. ALICE: Conceptual development of a lower limb exoskeleton robot driven by an on-board musculoskeletal simulator. Sensors 2020, 20, 789. [Google Scholar] [CrossRef] [PubMed] [Green Version]
- Ji, Q.; Qian, Z.; Ren, L.; Ren, L. Torque Curve Optimization of Ankle Push-Off in Walking Bipedal Robots Using Genetic Algorithm. Sensors 2021, 21, 3435. [Google Scholar] [CrossRef] [PubMed]
- De Simone, M.C.; Russo, S.; Rivera, Z.B.; Guida, D. Multibody model of a UAV in presence of wind fields. In Proceedings of the International Conference on Control, Artificial Intelligence, Robotics & Optimization (ICCAIRO), Prague, Czech Republic, 20–22 May 2017; pp. 83–88. [Google Scholar]
- Szántó, A.; Hajdu, S. Vehicle Modelling and Simulation in Simulink. Int. J. Eng. Manag. Sci. 2019, 4, 260–265. [Google Scholar] [CrossRef]
- Dosoftei, C.; Horga, V.; Doroftei, I.; Popovici, T.; Custura, Ş. Simplified Mecanum Wheel Modelling using a Reduced Omni Wheel Model for Dynamic Simulation of an Omnidirectional Mobile Robot. In Proceedings of the International Conference and Exposition on Electrical and Power Engineering (EPE), Iasi, Romania, 22–23 October 2020; pp. 721–726. [Google Scholar]
- Coulter, R.C. Implementation of the Pure Pursuit Path Tracking Algorithm; Technical Report; Carnegie-Mellon UNIV Pittsburgh PA Robotics INST: Pittsburgh, PA, USA, 1992. [Google Scholar]
- Lundgren, M. Path Tracking and Obstacle Avoidance for a Miniature Robot. Master’s Thesis, Umeå University, Umeå, Sweden, 2003; p. 238. [Google Scholar]
- Cuenca, Á.; Zhan, W.; Salt, J.; Alcaina, J.; Tang, C.; Tomizuka, M. A remote control strategy for an autonomous vehicle with slow sensor using kalman filtering and dual-rate control. Sensors 2019, 19, 2983. [Google Scholar] [CrossRef] [PubMed] [Green Version]
- Cotera, P.; Velazquez, M.; Cruz, D.; Medina, L.; Bandala, M. Indoor robot positioning using an enhanced trilateration algorithm. Int. J. Adv. Robot. Syst. 2016, 13, 110. [Google Scholar] [CrossRef] [Green Version]
- Fukao, T.; Nakagawa, H.; Adachi, N. Adaptive tracking control of a nonholonomic mobile robot. IEEE Trans. Robot. Autom. 2000, 16, 609–615. [Google Scholar] [CrossRef]
- Åström, K.J.; Eykhoff, P. System identification—A survey. Automatica 1971, 7, 123–162. [Google Scholar] [CrossRef] [Green Version]
- Ogata, K. Discrete-Time Control Systems; Prentice-Hall, Inc.: Hoboken, NJ, USA, 1995. [Google Scholar]
- Reinhart, R.F.; Shareef, Z.; Steil, J.J. Hybrid analytical and data-driven modeling for feed-forward robot control. Sensors 2017, 17, 311. [Google Scholar] [CrossRef] [PubMed]
- Salt, J.; Albertos, P. Model-based multirate controllers design. IEEE Trans. Control Syst. Technol. 2005, 13, 988–997. [Google Scholar] [CrossRef]
Parameter | Value |
---|---|
Torque constant () | 0.00374 Nm |
Damping coefficient (B) | 5.3473 |
Moment of inertia (J) | 1.38083 kgm |
Parameter | Value |
---|---|
Static friction coeff. rubber-ground | 0.9285 |
Dynamic friction coeff. rubber-ground | 0.5059 |
Static friction coeff. steel-ground | 0.6619 |
Dynamic friction coeff. steel-ground | 0.4349 |
Reference | Square | Lissajous | ||||
---|---|---|---|---|---|---|
Experiment | ||||||
Direct pose (D.P. 0.1) | 0.01251 | 0.04947 | 40.2 | 0.01259 | 0.04513 | 92.9 |
Direct pose (D.P. 0.2) | 0.03417 | 0.09590 | 57.0 | 0.03688 | 0.10901 | 138.2 |
Direct pose (D.P. 0.5) | ∞ | ∞ | ∞ | ∞ | ∞ | ∞ |
Odometry w/o noise (Odom) | 0.02695 | 0.07053 | 41.1 | 0.02077 | 0.07999 | 94.3 |
Odometry w noise (Odom N) | 0.10844 | 0.36137 | 43.7 | ∞ | ∞ | ∞ |
DREKF (DREKF 10) | 0.01759 | 0.06215 | 45.1 | 0.01551 | 0.05451 | 105.0 |
DREKF (DREKF 50) | 0.03252 | 0.09978 | 46.2 | 0.03495 | 0.11363 | 106.6 |
DREKF beacon (DREKF-D) | 0.01739 | 0.07056 | 44.8 | 0.01577 | 0.06306 | 104.9 |
Publisher’s Note: MDPI stays neutral with regard to jurisdictional claims in published maps and institutional affiliations. |
© 2021 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (https://creativecommons.org/licenses/by/4.0/).
Share and Cite
Carbonell, R.; Cuenca, Á.; Casanova, V.; Pizá, R.; Salt Llobregat, J.J. Dual-Rate Extended Kalman Filter Based Path-Following Motion Control for an Unmanned Ground Vehicle: Realistic Simulation. Sensors 2021, 21, 7557. https://doi.org/10.3390/s21227557
Carbonell R, Cuenca Á, Casanova V, Pizá R, Salt Llobregat JJ. Dual-Rate Extended Kalman Filter Based Path-Following Motion Control for an Unmanned Ground Vehicle: Realistic Simulation. Sensors. 2021; 21(22):7557. https://doi.org/10.3390/s21227557
Chicago/Turabian StyleCarbonell, Rafael, Ángel Cuenca, Vicente Casanova, Ricardo Pizá, and Julián J. Salt Llobregat. 2021. "Dual-Rate Extended Kalman Filter Based Path-Following Motion Control for an Unmanned Ground Vehicle: Realistic Simulation" Sensors 21, no. 22: 7557. https://doi.org/10.3390/s21227557