Liu Wenzhi. Research and Implementation of SLAM and Path Planning Algorithm Based on Lidar [D]. Harbin Institute of Technology, 20 18.
The principle of coordinate system transformation has been written in the previous article, and the equation is directly up-converted.
Here, his motion model chooses odometer-based motion model and speed-based motion model. In fact, they are all the same, and the overall thinking is the same. Odometer is a device to estimate the motion displacement of robot by counting the number of pulses output by photoelectric encoder in a certain period of time, mainly using photoelectric encoder. According to the photoelectric code wheel, the speed of the wheel at this time is calculated, and then the displacement increment of each wheel per unit time is obtained from the known wheel radius.
Higher mathematics knows that the increment of displacement per unit time is speed, and the distance traveled during this time can be obtained by integrating the speed in a certain time.
According to the above figure, we can find out the angular velocity of the robot's heading angle, the radius of arc motion and the change of the robot's angle, so as to solve the robot's current posture.
In fact, there are errors, so relying solely on the odometer will have great errors with the actual results, so it is necessary to introduce other external sensors to observe the external environment to correct these errors, thus improving the positioning accuracy.
Firstly, the endpoint coordinates measured by lidar need to be converted from polar coordinates and robot coordinates to world coordinates.
Skip this. You don't need to see this for the time being.
Introduction of path planning algorithm;
Because this algorithm will produce a lot of useless temporary paths, in short, it is slow, so there are other algorithms.
After understanding these two kinds of costs, the method we use is to add the estimated cost of each block to the current path cost, which can represent the distance from each path point to the end point. On the basis of BFS search process, choosing the path with the lowest total cost can save a lot of detours. (algorithm explanation/video/bv1bv411y79p? From = search & seid = 3623681329596549549)
In the local path planning algorithm, we choose DWA (dynamic window method), also known as dynamic window method. The dynamic window method mainly samples multiple groups of velocities in the velocity (v, w) space, and simulates the trajectory of the robot moving at these velocities in a certain time. After obtaining multiple sets of trajectories, these trajectories are evaluated and the speed corresponding to the optimal trajectory is selected to drive the robot to move.
State sampling means that according to the global path planning given before, both Dijkstra and A* can easily get state sampling, and DWA algorithm needs to establish two kinds of action sampling in advance:
But in any case, the above work is to convert the displacement of the robot into world coordinates, not robot coordinates. After speed sampling, we only need to judge the trajectory of the car and get the optimal solution. The following describes the method of speed sampling.
Sampling speed generally has the following three limitations:
After the speed range is determined, it is necessary to discretize the speed of the car according to the speed resolution and visualize the distance that the car will travel under different combinations of linear speed and angular speed at each moment.
Each of these tracks is connected by many small straight lines.
It is necessary to use an evaluation function to select the above trajectory and select the most suitable trajectory.
Finally, in order to make the three parameters play an equal role in the evaluation function, we use normalization to calculate the weight.
The overall algorithm flow is as follows: