Visual tracking and recognition of intelligent service robots used in sensor fusion theory (III)
Based on the vision based SLAM (Simultaneous Localization and Mapping)
Localization and SLAM
Development and improvement of robot motion planning and control system, the robot can perform real-time motion planning. In a completely autonomous manner to move freely in a dynamic environment, complete the required tasks. Robot motion planning and system architecture to simultaneous localization and mapping environment, and path planning algorithms and automatic navigation of the core , will cover sensing, perception, intelligence, control, mechanism, actuation and so on.
Fig 1.A.1.
This project type includes wheeled robot platform. Main objective in establishing a variety of prospective mobile robot motion planning theory, which includes differential wheeled robot coordination and control, instant wheeled robot obstacle avoidance mechanism, instant wheeled robot path planning system, dynamic environments SLAM, multi-robot coordination and control system. Furthermore, in order to make the robot perceptual system is more complete in this project with the imaging system as an aid to improve the accuracy of perceptual system in order to achieve more sophisticated real-time motion planning system.
Manner in accordance with the classification system modules, the project can be divided into coordinated control system, simultaneous localization and mapping systems, route planning and automatic navigation system, image analysis and control systems, sensing systems, robotics community intelligence system. The current plan is so modular architecture of the modular system developed forward-looking technology, and finally then each module systems into complete motion planning system.
Fig 1.A.2.Campaign planning system's modular architecture
Currently, there are three ways to achieve SLAM: Topological, rid-Based, Feature-Based methods. In Topological way, is to the environment as a picture, using the identification of each object in the environment, or the environment in a manner to achieve positioning function. While this approach can be done through the large size of the positioning environment, but in fact the environment to achieve identification is quite difficult, and therefore less commonly used.
Grid-Based approach is also widely used to handle the problem of localization and map building. A typical approach is to set the environment into the global map with local map. Global map is used to store all the information about the environment, while the regional map is used to update the global map information. Through the detection area map in the global map of the relative position of the robot can achieve the purpose of positioning. However, due to the need for information has been saved Regional Map Grid-Based therefore require considerable amount of computation and storage space. Compared Topological and Grid-Based in two ways, through feature extraction to achieve localization and construct maps seem much more intuitive. You can get through to the edge of the image, water chestnut, corners, lines, and feature information, and even can also use color, shape, length and other characteristics to understand the environment. A feature map (Feature-Based Map) normally expressed by the following formula, which is the feature if, N refers to the number of features in the map.
Map-Matching
Use robot sensors built out of local maps and compare the global map to the global map as a reference to correct the current position of the robot. Location estimate to match the local map and the global map. Calculation is more complex, and uncertainty is much greater than the use of landmarks do estimate. However, it may be more quickly complete the establishment of the map, but this method has significant to the surrounding environment in order to complete the shape feature. For example at right angles, use this form to make comparison and combination of characteristics. The disadvantage is that the characteristics of the environment do not easily be retrieved fusion, data fusion by virtue of the robot detected local map information, find out if there are other obvious features rectangular pattern, and then with the global coordinate characteristics than do pair. We can’t effective integration, location when corridor case. This approach is too restrictive by the surrounding environment, and its uncertainty is much larger than estimates using the results of a landmark to do.
SLAMAlgorithm
For now, the algorithms used for multi-robot localization include triangle localization, Kalman Filter, MCL(Monte Carlo Localization), Particle Filter, Mixture MCL, and Grid-Based Markov localization. Triangle localization provides us a simple and fast update for geometric shape and position. But the position info acquired by this method didn’t consider the error of sensor and robot by a probabilistic model. As a result, the error of sensors and the probability of localized position cannot be effectively compared.
Using statistic algorithm to mathematically formulate the uncertainty of position estimation, which is taken as the indicator of localization, is for now generally acknowledged by robotic localization research community. For example, algorithms such as Kalman Filter, MCL, Mixture MCL, Grid-Based Markov localization are established upon Probability Theory. These localization algorithms share the same properties, that is, fusing multiple data to get the optimal position estimation. The more the data fused together, the more the accuracy it will get.
The concept of Kalman Filter is to estimate the output from the next moment from the state at this moment and the input from the next moment. Thus we must have the current state of the robot when using Kalman Filter. Besides, Kalman Filter uses Gaussian distribution to estimate and fuse the sensor error effectively by its recursive algorithm.
Kalman Filter is a precise and simple approach. However, this method suppose that error distribution of the sensor model during fusion must be Gaussian form and linear. The advantage of Kalman Filter is on the fast speed of fusion and convergence. If the error distribution of the sensor is Gaussian form, then the estimation will be highly accurate. However, the shortcoming is as mention above, that is the error distribution must be Gaussian distribution, otherwise the estimation will be inaccurate.
Thanks to the springing up of multi-robot system and SLAM research area, many researchers proposed Markov localization algorithm. The main principleis also based on probabilistic to estimate the position of robots. This approach allows the robots to estimate the robot to estimate the position relative to each other in a global map. It compares the local map and the global map from start to finish and find out the most likely position where the robot might be. No initial pose is required and no fear of the failure, but the calculation load is quite heavy, especially at the very beginning. In order to lower down the timing during localization process, MCL algorithm is then proposed. MCL(Monte Carlo Localization) doesn’t compare the local map and the global map thoroughly. In contrast, the most important part of MLC is the Believe Function. The way it uses to estimate and correct the position is sampling and resampling repeatedly. It defines certain number of sample points, or called particles, on the global map, and gives every particle a probability. When the particles converge, it means that the position of the robot is most likely there. This approach indeed reduces the calculation load of Markov localization. The calculation time depends on the dimension of the map and the precision of partitioning. In other words, the calculation load depends on the number of particle and as a result makes it difficult to meet the realtime requirement. On the other hand, the advantage of MLC is on the tolerance of different error distribution of sensor model, not only limited to Gaussian.
From the comparison of Kalman Filter and MCL, we find that MCL is more stable and robust in the case of large map. However from other perspective, Kalman Filter is more precise than MCL on localization. As long as the input sensor error model is Gaussian distribution, the efficiency and precision of Kalman Filter will rise significantly. Besides, the convergence of Kalman Filter is faster than MCL, hence it is more suitable for realtime application. Since Kalman is proposed earlier than MCL, it has been researched in a deeper sense, and these two algorithms both have its advantages, tKalman Filter is still widely used. More improvements on Kalman Filter are proposed, such as Extended Kalman filter(EKF), Unscented Kalman Filter(UKF), etc. In conclusiob, Kalman Filter is often used on localization problem when the map is smaller, while MCL is often used on larger map. If SLAM is a linear problem and the sensor error is of Gaussian form, then Kalman Filter can guarantee the convergence and optimal solution. However SLAM is rarely a linear problem, and that why Extended Kalman filter(EKF) is proposed to solve the nonlinearity.
Suppose that xt represent the position of the robot at time t, and ut+1 represent the control signal at t+1, then we can get xt+1 from the fusion of xt and ut+1. Compare the result with the observation zt+1 from the robot, we can precisely locate the position of the robot. Keep on this process, the position of the robot at every moment is obtained.
Fig.4 Formulation of SLAM
0is the equation for SLAM--estimate the position at this moment based on the position from the last moment and command from this moment. Also, compare with observation data from sensor to obtain the precise position at this moment.
0is the problem we have to handle illustrated by Durrant-Whyte. Since the robot cannot get position of itself and the landmark in the environment, the robot need to estimate the environment by sensor and by which it estimate the position of itself and the landmark. For example, when the robot is at xk-1, it observes two landmark mi. After marking two previously unknown landmarks on the map, the robot at the same time can estimate the relative position of itself. Moreover, the distance that the robot has moved and the estimation of the landmark can’t be perfectly precise, so there is uncertainty in those estimations. At xk, the robot has observed three landmark mi, mj. Besides adding the landmark mj into the map, since it’s the second time to observe mi, the position these two landmark mi are updated and the uncertainty of them is lowered down, which means the reliability of the landmark is raised up. Meanwhile, the robot’s position can be estimated again based on the observed landmark. Repeating this process, the robot can accomplish the task of simultaneously localization and maping.
Fig.5 Concept of SLAM algorithm
In SLAM research area, one of the main issues is to solve the probabilistic problem, that is, the uncertainty of robot pose and map. Kalman filter is a good approach, though with the disadvantage of the requirement of linear system. Thus, Extended Kalman Filter, which is a feature-based approach, is a common used solution. EKF has the advantage of fast convergence, high precision, good stability, and etc. The algorithm is easy to implement, and is able to apply on nonlinear system and realtime system. This approach can effectively eliminate data error and keep the estimation of system state correctly.
We will use EKF for data fusion to eliminate error and noise, and seek for the improved version of EKF-- Unscented Kalman filter(UKF) to reach a better efficiency and precise result. Using vision to mark out the position of the landmark and then calculate the covariance intersection of ultrasonic and visual data to accomplish SLAM and obstacle avoidance task.
Unscented Kalman Filter(UKF) is an improved algorithm based on EKF. EKF uses the coefficient of first order derivative of Taylor series, and that means it has to calculate the Jacobians repeatedly. On the other hand, UKF uses linear regression to acquire a Gaussian mean so that it doesn’t have to calculate the Jacobian at every estimate and update step. To sum up, both UKF and EKF have the high speed of convergence, while UKF is more linear than EKF on the first and second coefficient of Taylor series. The accuracy will be higher than EKF. As illustrated in ___, we can see that the estimated error of UKF almost cover the correct error, but the estimated error of EKF is not so accurate. Besides, UKF reduces the step of repeated calculation of Jacobians, so the algorithm will be simpler.
CU can be used to optimize the linear matrix inequality (LMI) by finding one of the k in ellipsoid. The second order equation is as follow:And the minimum can be obtained by solving the maximization problem of the following determinant.