A high-speed mapping navigation method based on karto and teb

Abstract

The application relates to a high-speed mapping navigation method based on karto and teb, and belongs to the technical field of mapping navigation. A vehicle control system adjusts a vehicle running posture through input data of a vehicle-mounted odometer and a laser radar, and the adjustment process comprises a Karto algorithm SLAM map construction step, a positioning step, a global planning step, a TEB algorithm local planning step and a path tracking step. The application can accurately scan the surrounding environment and build a map under the condition that the vehicle is high-speed and the computing power is limited, so that an unmanned vehicle can collect data by relying on a vehicle-mounted odometer and a laser radar, accurately position and establish a map in high-speed motion, and perform navigation on the basis, realize autonomous path planning, and improve the upper limit of the driving speed of an automatic driving vehicle under the condition that the computing resources are limited.

Description

Technical Field

[0001] This invention relates to a high-speed mapping and navigation method based on karto and teb, belonging to the field of mapping and navigation technology. Background Technology

[0002] Mapping and navigation systems are crucial for enabling autonomous vehicles to plan routes automatically. They allow vehicles to autonomously construct surrounding maps using data collected by their sensors, and to determine their real-time location without human assistance. Subsequently, based on this map, the vehicle can use path planning algorithms to navigate, autonomously planning its route to its destination and avoiding obstacles along the way.

[0003] In this field, mapping and navigation systems mainly include Simultaneous Localization and Mapping (SLAM) algorithms and motion planning algorithms. However, implementing practical mapping and navigation systems directly on large autonomous vehicles still faces many challenges. Currently, researchers primarily design corresponding mapping and navigation systems for different vehicles and specific application scenarios. This study uses the vehicles and track settings of the National Undergraduate Intelligent Vehicle Competition (Outdoor Special Competition) as the research background. The autonomous vehicle is a small Kalman model equipped with an onboard odometer, LiDAR, and a CPU with limited computing power. The track is simplified compared to real-world environments but still possesses rich environmental elements, primarily including straightaways, right-angle curves, and circular curves. The track element boundaries are formed by fixed-size cones (28*28*63cm) placed at certain distances and angles. Under these conditions, the goal is to achieve accurate localization, mapping, and navigation functions for the vehicle using a CPU with limited computing power during high-speed operation.

[0004] Specifically, in terms of "mapping," traffic cones are crucial markers for autonomous driving systems to effectively identify the track. The placement of the cones determines the track's outline; therefore, the main goal of "mapping" is to achieve precise positioning of the cones in a high-speed vehicle environment. After completing "mapping," map data is needed for "navigation." The prerequisite for "navigation" is accurately determining the vehicle's own position. Subsequently, path planning algorithms combine the vehicle's position with map data to plan the vehicle's future direction, speed, and path, thereby truly achieving autonomous driving.

[0005] Currently, some problems and difficulties exist in this technology: 1. Although there are many existing mapping methods, they often perform poorly in real-world application environments with high-speed vehicles and limited computing power. 2. Many existing mapping and navigation methods are often fragmented and do not form a cohesive application system. Therefore, this invention is proposed. Summary of the Invention

[0006] To address the shortcomings of existing technologies, this invention provides a high-speed mapping and navigation method based on Karto and Teb. This method enables accurate scanning and map building of the surrounding environment even when vehicles are traveling at high speeds and computing power is limited. It allows unmanned vehicles to accurately locate and build maps while moving at high speeds, relying solely on data collected by onboard odometers and LiDAR. Based on this data, navigation can be performed, achieving autonomous path planning and increasing the upper limit of autonomous vehicle speed under limited computing resources.

[0007] The technical solution of the present invention is as follows:

[0008] A high-speed mapping and navigation method based on Karto and Teb, comprising the following steps:

[0009] (1) Karto algorithm SLAM map construction steps: Using the karto algorithm, based on the sparse graph optimization method, to realize localization and mapping in high-speed environment;

[0010] (2) Positioning steps: The EKF algorithm is used to integrate the data input from the vehicle's onboard odometer, lidar, and inertial measurement unit to perform vehicle positioning, thereby enabling navigation;

[0011] (3) Global planning steps: By comprehensively processing the input data from the vehicle odometer, lidar and inertial measurement unit and the map data, the shortest path to the target point is planned for the vehicle;

[0012] (4) Local planning steps of TEB algorithm: Based on the global shortest path obtained by global planning, the TEB algorithm is used to generate local target points in real time, and the environmental information scanned by the lidar is combined with it in real time to plan a safe path for the vehicle and realize the obstacle avoidance function.

[0013] (5) Path tracing steps: Abstract the mathematical model of the vehicle, adopt a pure tracing algorithm, and obtain the vehicle's turning angle, linear velocity and other data by inputting the path planned by the navigation algorithm.

[0014] According to a preferred embodiment of the present invention, in step (1), the Karto algorithm SLAM map construction step is specifically as follows:

[0015] ① Using grid mapping technology, the entire map is divided into a series of small grids, each grid corresponding to a logarithmic occupancy probability, that is, the probability that there is an obstacle at that point;

[0016] ② Data collected by the LiDAR sensor is extracted using scan-matching technology to generate a local map. Specifically, for a new input LiDAR scan, the data from the vehicle's odometer is first used as the predicted pose for the current scan. This predicted pose serves as prior knowledge for scan-matching. Then, using a sliding window method, all radar data within the nearest 24 meters stored in Karto are converted into a grid map with a resolution of 0.01 meters, thus generating a local map. Next, all translational and rotational poses within a certain range are traversed, and the pose with the highest score is selected. The pose score is determined by the occupancy probability of a radar point (a reflection point acquired by the LiDAR) corresponding to a cell in the grid map, as shown in the following formula:

[0017]

[0018] Among them, P l O represents the set of all radar points. p This represents the probability of radar point p being occupied by a cell in the grid map;

[0019] Finally, poses that differ significantly from the prior pose are penalized.

[0020] ③ Improve the accuracy of attitude estimation using optimization algorithms. Specifically, firstly, an error function can be constructed as follows:

[0021]

[0022] Where x is the pose parameter vector, e i (x) is the measurement error of the i-th lidar scan.

[0023] Subsequently, linear optimization is performed on the error function to obtain the Jacobian matrix J(x) and residual vector r(x) of the error function, as shown in the following formulas:

[0024]

[0025] r(x)=e(x)-J(x)·Δx (4)

[0026] Where Δx is the update amount of the parameter vector, as shown in the following formula:

[0027] Δx=(J(x) T ·J(x))―(J(x) T ·r(x)) (5)

[0028] Then, the update amount Δx is added to the current parameter vector x to obtain the new parameter vector:

[0029] x new =x + Δx (6)

[0030] Repeat the steps from equation (3) to equation (6) until Δx is less than the set threshold, then x optimization is complete;

[0031] Through the above steps, the optimization algorithm can continuously optimize the robot's pose, thereby obtaining a more accurate pose.

[0032] ④ During long-term SLAM processes, factors such as sensor errors and environmental changes may lead to map bias and inaccurate attitude estimation. It is necessary to periodically perform loop closure detection (i.e., whether the vehicle has returned to previously visited areas) to correct map bias and attitude estimation errors. The loop closure detection steps are as follows:

[0033] Feature extraction: Extract key feature points from the data transmitted back by the LiDAR. Feature points include prominent corners, edges, etc.

[0034] Feature matching: Match the features of the current frame with historical features. If the features of the current frame are similar to a certain historical feature, it is considered that the vehicle has made a loop, that is, the vehicle has reached the area it has been in the past.

[0035] When loop closures exist, optimization is performed by combining the poses of historical frames and the current frame, using the least squares method. The specific optimization method is explained in section ③. It is worth noting that the optimization algorithm and loop closure detection in the Karto algorithm are combined in a single function; the optimization algorithm for loop closure detection is the one described in section ③.

[0036] Map update: Update the map based on the optimized pose information.

[0037] According to a preferred embodiment of the present invention, in step (2), the positioning mainly consists of two parts: prediction and updating. The specific steps are as follows:

[0038] In the prediction part, the algorithm uses the previous state estimate and the input control quantity to predict the current state estimate, as shown in the following formula:

[0039]

[0040] in, Let f be the state estimate at time k, and let f be the nonlinear motion model function. k Let k be the input control quantity at time k;

[0041] In the update section, the algorithm uses the predicted state estimate and the measurement input to update the current state estimate. Specifically, the algorithm linearizes the measurement model around the predicted state estimate and then applies Kalman gain to correct the predicted state estimate. The mathematical formula is as follows:

[0042]

[0043] Where K is the Kalman gain, P is the covariance matrix of the error, H is the Jacobian matrix of the measurement model, R is the covariance matrix of the measurement noise, z is the measurement input value, and h is the nonlinear measurement model.

[0044] According to a preferred embodiment of the present invention, the global planning step in step (3) is specifically as follows:

[0045] For global path planning, based on the completed mapping and localization, we find the trajectory topology map nodes closest to the start and end points, and use the A* algorithm to find the shortest path connecting these two nodes on the topology map. The A* algorithm is an existing algorithm that is a very classic, concise, and heuristic path planning algorithm.

[0046] According to a preferred embodiment of the present invention, in step (4), the local planning steps of the TEB algorithm are as follows:

[0047] First, establish the vehicle's state space to describe its position and attitude on the map, as shown in the following formula:

[0048] X a =(x i ,y i ,β i ) T ∈R 2 ×S 1 (9)

[0049] Where, x i ,y i ,β i These represent the vehicle's x-coordinate, y-coordinate, and tilt angle relative to the x-coordinate on the map, respectively.

[0050] The sequence of vehicle poses in space is described by the following formula:

[0051] Q = {X} i} i＝0,1,…,n n∈N (10)

[0052] By merging the pose sequence Q with the time series, we obtain:

[0053] B: = (Q, τ) (11)

[0054] By employing weighted multi-objective optimization, the optimal path point, i.e., the optimal Q, is obtained.

[0055] f(B) = Σ a γ k f k (B)

[0056] B *=arg B minf(B) (12)

[0057] Among them, B * For the optimal result, f(B) is the objective function considering various constraints, and γ k The weights of various constraints are then assigned. Subsequently, a cost function is introduced to penalize cases where the constraints are not met. The cost function is shown in the following equation:

[0058]

[0059] Where x represents the robot pose, x r Here, S represents the constraint value, n represents the polynomial coefficient, and ∈ represents a small displacement near the constraint value.

[0060] Then, based on the concept of elastic band, two sets of constraints are introduced: one set is tracking constraint and obstacle avoidance constraint, and the other set is velocity constraint.

[0061] Specifically, regarding the concept of the elastic band, the core of the TEB algorithm is to treat the path as an elastic band, connecting the starting point and the target point, and allowing it to deform. The external cause of deformation is to treat the constraints as external forces applied to the elastic band.

[0062] Tracking constraints and obstacle avoidance constraints:

[0063] This set of constraints has two main objectives: to fit the path obtained from the global planning as closely as possible, and to avoid obstacles as much as possible. The cost functions corresponding to these two objectives are very similar: one applies force to the elastic band to pull it towards the global path, and the other applies force to the elastic band to keep it away from obstacles. The specific formulas are as follows:

[0064]

[0065]

[0066] Where, d min，j For the pose sequence and the global planning point sequence or obstacle z j The closest distance, This represents the maximum allowed distance between the pose sequence and the global planning point sequence. S represents the minimum permissible distance from the pose sequence to the obstacle, where S represents the scaling factor, n represents the polynomial coefficient, and ∈ represents a small displacement near the constraint value.

[0067] Speed ​​constraints

[0068] The dynamic constraints determined by velocity can be represented by a penalty function of the constraints. The average linear velocity of the vehicle motion can be obtained from adjacent poses x. i+1x i It is calculated with time interval ΔT;

[0069]

[0070] Similar to the constraints in tracking and obstacle avoidance, the constraint on linear velocity is expressed as:

[0071]

[0072] In the above formula, v i x represents the average linear velocity of the robot's motion. i y represents the x-coordinate of the i-th item in the robot pose sequence. i β represents the ordinate of the i-th item in the robot pose sequence. i ΔT represents the tilt angle of the i-th item in the robot pose sequence, and ΔT represents the time interval (the time interval between the previous and next items in the pose sequence).

[0073] Based on the defined objective function and constraints, the problem has been transformed into a pure optimization problem. Using the graph optimization algorithm from the g2o general-purpose graph optimization library in the ROS system, a linear equation is defined, the solver is PCG, and the iteration strategy is GN. Repeated iterations yield the final safe vehicle trajectory. Graph optimization libraries are very mature algorithm libraries for graph optimization.

[0074] According to a preferred embodiment of the present invention, in step (5), the path tracing step is as follows:

[0075] Based on the type of vehicle model used, its steering model is abstracted as follows: Figure 3 As shown.

[0076] To facilitate analysis, the model is simplified by imagining a tire at the center of the front wheel and another at the center of the rear wheel to represent the movement of the front and rear wheels; this is the so-called single-vehicle model, as shown below. Figure 4 As shown;

[0077] If the control variables are the rear wheel speed v and the front wheel steering angle δ, and the state variables are the coordinates x, y and the heading angle θ in the world coordinate system, then the kinematic model of Ackermann steering is expressed as:

[0078]

[0079] The corresponding odometer model is:

[0080]

[0081] Based on the above equations, we can obtain:

[0082]

[0083] In the above equation, δ is the steering angle we require, and R is an unknown quantity. Since the curvature k is reciprocal to the radius R, the above equation can be rewritten as follows:

[0084] δ=tan -1 (kL) (20)

[0085] The solution then becomes solving the equation on the right, where L is known, is the wheelbase of the vehicle, and the unknown quantity is the curvature k. Next, we solve for k.

[0086] From the Law of Sines, we can obtain:

[0087]

[0088]

[0089]

[0090] Considering time, equation (20) can be expressed as:

[0091]

[0092] Based on the above mathematical modeling calculations, the pure tracking navigation algorithm becomes a process of solving for α at each time step;

[0093] The vehicle's current position is obtained by reading filtered data from the onboard odometer and used as input for the pure tracking algorithm.

[0094] Based on the target object's position and its own position, the servo motor angle is calculated using the mathematically derived formula (22) to obtain the vehicle's driving angle and angular velocity. Specifically, given the target object's position and its own position, i.e., given l d Given α, δ can be calculated by assuming the vehicle's wheelbase L is known.

[0095] Based on the calculated δ value and the user-set driving speed, the values ​​are input into the drive functions of the vehicle's motor and servo motor to control the vehicle to perform the corresponding actions.

[0096] The beneficial effects of this invention are as follows:

[0097] This invention designs a high-speed mapping and navigation method based on Karto and TEB, primarily addressing two major problems in current autonomous vehicles: limited computing power and reduced mapping accuracy under high-speed operation. It utilizes the Karto SLAM algorithm and TEB as a local planner to achieve accurate mapping and navigation of autonomous vehicles at high speeds. This improves the safe driving speed of autonomous vehicles, enhances the accuracy and reliability of mapping and navigation systems, reduces the demands on CPU performance, and makes autonomous vehicles more reliable and efficient. Attached Figure Description

[0098] Figure 1 This is a map created by the present invention under conditions of high-speed movement and complex scenes;

[0099] Figure 2 This is a system architecture diagram of the navigation system of the present invention;

[0100] Figure 3 This is an abstract diagram of the Ackerman car model used in this invention;

[0101] Figure 4 This is a simplified schematic diagram of a single vehicle model according to the present invention;

[0102] Figure 5 This is a tf coordinate relationship diagram successfully configured for this invention;

[0103] Figure 6 The figure shows the test results of an embodiment of the present invention;

[0104] Figure 7 This is a diagram of the mapping and navigation system architecture of the present invention. Detailed Implementation

[0105] The present invention will be further described below with reference to the embodiments and accompanying drawings, but is not limited thereto.

[0106] Example 1:

[0107] This embodiment provides a high-speed mapping and navigation method based on Karto and Teb, the steps of which are as follows:

[0108] (1) Karto algorithm SLAM map construction steps: Using the karto algorithm, based on the sparse graph optimization method, to realize localization and mapping in high-speed environment;

[0109] (2) Positioning steps: The EKF algorithm is used to combine the data from the vehicle odometer and the LiDAR to locate the vehicle, thereby allowing navigation to proceed;

[0110] (3) Global planning steps: By comprehensively processing the input data from the vehicle's onboard odometer and lidar, and map data, the shortest path to the target point is planned for the vehicle.

[0111] (4) Local planning steps of TEB algorithm: Based on the global shortest path obtained by global planning, the TEB algorithm is used to generate local target points in real time, and the environmental information scanned by the lidar is combined with it in real time to plan a safe path for the vehicle and realize the obstacle avoidance function.

[0112] (5) Path tracing steps: Abstract the mathematical model of the vehicle, adopt a pure tracing algorithm, and obtain the vehicle's turning angle, linear velocity and other data by inputting the path planned by the navigation algorithm.

[0113] According to a preferred embodiment of the present invention, in step (1), the Karto algorithm SLAM map construction step is specifically as follows:

[0114] ① Using grid mapping technology, the entire map is divided into a series of small grids, each grid corresponding to a logarithmic occupancy probability, that is, the probability that there is an obstacle at that point;

[0115] ② Data collected by the LiDAR sensor is extracted using scan-matching technology to generate a local map. Specifically, for a new input LiDAR scan, the data from the vehicle's odometer is first used as the predicted pose for the current scan. This predicted pose serves as prior knowledge for scan-matching. Then, using a sliding window method, all radar data within the nearest 24 meters stored in Karto are converted into a grid map with a resolution of 0.01 meters, thus generating a local map. Next, all translational and rotational poses within a certain range are traversed, and the pose with the highest score is selected. The pose score is determined by the occupancy probability of a radar point (a reflection point acquired by the LiDAR) corresponding to a cell in the grid map, as shown in the following formula:

[0116]

[0117] Among them, P l O represents the set of all radar points. p This represents the probability of radar point p being occupied by a cell in the grid map;

[0118] Finally, poses that differ significantly from the prior pose are penalized.

[0119] ③ Improve the accuracy of attitude estimation using optimization algorithms. Specifically, firstly, an error function can be constructed as follows:

[0120]

[0121] Where x is the pose parameter vector, e i (x) is the measurement error of the i-th lidar scan.

[0122] Subsequently, linear optimization is performed on the error function to obtain the Jacobian matrix J(x) and residual vector r(x) of the error function, as shown in the following formulas:

[0123]

[0124] r(x)=e(x)-J(x)·Δx (4)

[0125] Where Δx is the update amount of the parameter vector, as shown in the following formula:

[0126] Δx=(J(x) T·J(x))-(J(x) T ·r(x)) (5)

[0127] Then, the update amount Δx is added to the current parameter vector x to obtain the new parameter vector:

[0128] x ncw =x + Δx (6)

[0129] Repeat the steps from equation (3) to equation (6) until Δx is less than the set threshold, then x optimization is complete;

[0130] Through the above steps, the optimization algorithm can continuously optimize the robot's pose, thereby obtaining a more accurate pose.

[0131] ④ During long-term SLAM processes, factors such as sensor errors and environmental changes may lead to map bias and inaccurate attitude estimation. It is necessary to periodically perform loop closure detection (i.e., whether the vehicle has returned to previously visited areas) to correct map bias and attitude estimation errors. The loop closure detection steps are as follows:

[0132] Feature extraction: Extract key feature points from the data transmitted back by the LiDAR. Feature points include prominent corners, edges, etc.

[0133] Feature matching: Match the features of the current frame with historical features. If the features of the current frame are similar to a certain historical feature, it is considered that the vehicle has made a loop, that is, the vehicle has reached the area it has been in the past.

[0134] When loop closures exist, optimization is performed by combining the poses of historical frames and the current frame, using the least squares method. The specific optimization method is explained in section ③. It is worth noting that the optimization algorithm and loop closure detection in the Karto algorithm are combined in a single function; the optimization algorithm for loop closure detection is the one described in section ③.

[0135] Map update: Update the map based on the optimized pose information.

[0136] According to a preferred embodiment of the present invention, in step (2), the positioning mainly consists of two parts: prediction and updating. The specific steps are as follows:

[0137] In the prediction part, the algorithm uses the previous state estimate and the input control quantity to predict the current state estimate, as shown in the following formula:

[0138]

[0139] in, Let f be the state estimate at time k, and let f be the nonlinear motion model function. k Let k be the input control quantity at time k;

[0140] In the update section, the algorithm uses the predicted state estimate and the measurement input to update the current state estimate. Specifically, the algorithm linearizes the measurement model around the predicted state estimate and then applies Kalman gain to correct the predicted state estimate. The mathematical formula is as follows:

[0141]

[0142] Where K is the Kalman gain, P is the covariance matrix of the error, H is the Jacobian matrix of the measurement model, R is the covariance matrix of the measurement noise, z is the measurement input value, and h is the nonlinear measurement model.

[0143] According to a preferred embodiment of the present invention, the global planning step in step (3) is specifically as follows:

[0144] For global path planning, based on the completed mapping and localization, we find the trajectory topology map nodes closest to the start and end points, and use the A* algorithm to find the shortest path connecting these two nodes on the topology map. The A* algorithm is an existing algorithm that is a very classic, concise, and heuristic path planning algorithm.

[0145] According to a preferred embodiment of the present invention, in step (4), the local planning steps of the TEB algorithm are as follows:

[0146] First, establish the vehicle's state space to describe its position and attitude on the map, as shown in the following formula:

[0147] X a =(x i y i ,β i ) T ∈R 2 ×S 1 (9)

[0148] Where, x i y i ,β i These represent the vehicle's x-coordinate, y-coordinate, and tilt angle relative to the x-coordinate on the map, respectively.

[0149] The sequence of vehicle poses in space is described by the following formula:

[0150] Q = {X} i} i＝0，1，...，n n∈N (10)

[0151] By merging the pose sequence Q with the time series, we obtain:

[0152] B: = (Q, τ) (11)

[0153] By employing weighted multi-objective optimization, the optimal path point, i.e., the optimal Q, is obtained.

[0154] f(B)=∑ a γ k f k (B)

[0155] B * =arg B minf(B) (12)

[0156] Among them, B * For the optimal result, f(B) is the objective function considering various constraints, and γ k The weights of various constraints are then assigned. Subsequently, a cost function is introduced to penalize cases where the constraints are not met. The cost function is shown in the following equation:

[0157]

[0158] Where x represents the robot pose, x r Here, S represents the constraint value, n represents the polynomial coefficient, and ∈ represents a small displacement near the constraint value.

[0159] Then, based on the concept of elastic band, two sets of constraints are introduced: one set is tracking constraint and obstacle avoidance constraint, and the other set is velocity constraint.

[0160] Specifically, regarding the concept of the elastic band, the core of the TEB algorithm is to treat the path as an elastic band, connecting the starting point and the target point, and allowing it to deform. The external cause of deformation is to treat the constraints as external forces applied to the elastic band.

[0161] Tracking constraints and obstacle avoidance constraints:

[0162] This set of constraints has two main objectives: to fit the path obtained from the global planning as closely as possible, and to avoid obstacles as much as possible. The cost functions corresponding to these two objectives are very similar: one applies force to the elastic band to pull it towards the global path, and the other applies force to the elastic band to keep it away from obstacles. The specific formulas are as follows:

[0163]

[0164]

[0165] Where, d min，j For the pose sequence and the global planning point sequence or obstacle z j The closest distance, This represents the maximum allowed distance between the pose sequence and the global planning point sequence. S represents the minimum permissible distance from the pose sequence to the obstacle, where S represents the scaling factor, n represents the polynomial coefficient, and ∈ represents a small displacement near the constraint value.

[0166] Speed ​​constraints

[0167] The dynamic constraints determined by velocity can be represented by a penalty function of the constraints. The average linear velocity of the vehicle motion can be obtained from adjacent poses x. i+1 x i It is calculated with time interval ΔT;

[0168]

[0169] Similar to the constraints in tracking and obstacle avoidance, the constraint on linear velocity is expressed as:

[0170]

[0171] In the above formula, v i x represents the average linear velocity of the robot's motion. i y represents the x-coordinate of the i-th item in the robot pose sequence. i β represents the ordinate of the i-th item in the robot pose sequence. i ΔT represents the tilt angle of the i-th item in the robot pose sequence, and ΔT represents the time interval (the time interval between the previous and next items in the pose sequence).

[0172] Based on the defined objective function and constraints, the problem has been transformed into a pure optimization problem. Using the graph optimization algorithm from the g2o general-purpose graph optimization library in the ROS system, a linear equation is defined, the solver is PCG, and the iteration strategy is GN. Repeated iterations yield the final safe vehicle trajectory. Graph optimization libraries are very mature algorithm libraries for graph optimization.

[0173] According to a preferred embodiment of the present invention, in step (5), the path tracing step is as follows:

[0174] Based on the type of vehicle model used, its steering model is abstracted as follows: Figure 4 As shown.

[0175] To facilitate analysis, the model is simplified by imagining a tire at the center of the front wheel and another at the center of the rear wheel to represent the movement of the front and rear wheels; this is the so-called single-vehicle model, as shown below. Figure 5 As shown;

[0176] If the control variables are the rear wheel speed v and the front wheel steering angle δ, and the state variables are the coordinates x, y and the heading angle θ in the world coordinate system, then the kinematic model of Ackermann steering is expressed as:

[0177]

[0178] The corresponding odometer model is:

[0179]

[0180] Based on the above equations, we can obtain:

[0181]

[0182] In the above equation, δ is the steering angle we require, and R is an unknown quantity. Since the curvature k is reciprocal to the radius R, the above equation can be rewritten as follows:

[0183] δ=tan -1 (kL) (20)

[0184] The solution then becomes solving the equation on the right, where L is known, is the wheelbase of the vehicle, and the unknown quantity is the curvature k. Next, we solve for k.

[0185] From the Law of Sines, we can obtain:

[0186]

[0187]

[0188]

[0189] Considering time, equation (20) can be expressed as:

[0190]

[0191] Based on the above mathematical modeling calculations, the pure tracking navigation algorithm becomes a process of solving for α at each time step;

[0192] The vehicle's current position is obtained by reading filtered data from the onboard odometer and used as input for the pure tracking algorithm.

[0193] Based on the target object's position and its own position, the servo motor angle is calculated using the mathematically derived formula (22) to obtain the vehicle's driving angle and angular velocity. Specifically, given the target object's position and its own position, i.e., given l d Given α, δ can be calculated by assuming the vehicle's wheelbase L is known.

[0194] Based on the calculated δ value and the user-set driving speed, the values ​​are input into the drive functions of the vehicle's motor and servo motor to control the vehicle to perform the corresponding actions.

[0195] Finally, the successfully configured TF coordinate relationship is as follows: Figure 5As shown. TF coordinate relationships specifically refer to a set of rules for converting data read by various sensors inside a vehicle. Since multiple sensors are often installed in different locations on the same vehicle, and the data recorded by these sensors are obviously not based on the same origin, it is necessary to construct TF coordinate relationships to coordinate the data from sensors in different locations.

[0196] Testing navigation algorithms:

[0197] To verify that the navigation algorithm could function correctly, a real-vehicle test was conducted on the race track, and the results were as follows: Figure 6 As shown. From Figure 6 The six accompanying figures show the vehicle repeatedly reaching the locally planned target point under the pure tracking algorithm. Figure 6 The paths shown in (c) and (d) demonstrate that the vehicle avoided the bends and roadside obstacles, showcasing the algorithm's local obstacle avoidance capabilities.

Claims

1. A high-speed mapping and navigation method based on Karto and Teb, characterized in that, The steps are as follows: (1) Karto algorithm SLAM map construction steps: Using the Karto algorithm, based on the sparse graph optimization method, localization and mapping in high-speed environment are realized; The specific steps for Karto algorithm SLAM map construction are as follows: ① Using grid mapping technology, the entire map is divided into a series of small grids, each grid corresponding to a logarithmic occupancy probability, that is, the probability that there is an obstacle at that point; ② Data collected by the LiDAR sensor is extracted using scan-match technology to generate a local map. Specifically, for a new input LiDAR scan data, the data from the vehicle's odometer is first used as the predicted pose for the current scan. This predicted pose serves as prior knowledge for scan-match. Then, using a sliding window method, all radar data within the nearest 24 meters stored in Karto are converted into a grid map with a resolution of 0.01 meters, thus generating a local map. Next, all translational and rotational poses within a certain range are traversed, and the pose with the highest score is selected. The pose score is determined by the occupancy probability of the radar point corresponding to a cell in the grid map, as shown in the following formula: （1） in, Represents the set of all radar points. This represents the probability of radar point p being occupied by a cell in the grid map; Finally, poses that differ significantly from the prior pose are penalized. ③ Improve the accuracy of attitude estimation using optimization algorithms. Specifically, firstly, an error function can be constructed as follows: （2） Where x is the pose parameter vector, It is the measurement error of the i-th lidar scan; Subsequently, linear optimization is performed on the error function to obtain the Jacobian matrix J(x) and residual vector r(x) of the error function, as shown in the following formula: （3） r(x)=e(x)-J(x)· (4) in, It is the update amount of the parameter vector, and the formula is as follows: （5） Then, update the amount Add to the current parameter vector x to obtain the new parameter vector: （6） Repeat the steps from equation (3) to equation (6) until... If the value is less than the set threshold, then x optimization is complete; ④ Perform loopback detection on the vehicle to correct map bias and attitude estimation errors; (2) Positioning steps: The EKF algorithm is used to integrate the data input from the vehicle odometer, lidar and inertial measurement unit to perform vehicle positioning, thereby allowing navigation to proceed; (3) Global planning steps: By comprehensively processing the input data from the vehicle's onboard odometer, lidar, and inertial measurement unit, and map data, the shortest path to the target point is planned for the vehicle; (4) Local planning steps of TEB algorithm: Based on the global shortest path obtained by global planning, the TEB algorithm is used to generate local target points in real time, and the environmental information scanned by the lidar is combined with it in real time to plan a safe path for the vehicle and realize the obstacle avoidance function. The local planning steps of the TEB algorithm are as follows: First, establish the vehicle's state space to describe its position and attitude on the map, as shown in the following formula: （9） in, These represent the vehicle's x-coordinate, y-coordinate, and tilt angle relative to the x-coordinate on the map, respectively. The sequence of vehicle poses in space is described by the following formula: （10） By merging the pose sequence Q with the time series, we obtain: （11） By employing weighted multi-objective optimization, the optimal path point, i.e., the optimal Q, is obtained. （12） in, For the optimal result, To consider the objective function under various constraints, The weights for various constraints; Subsequently, a cost function is introduced to penalize cases where the constraints are not met. The cost function is shown in the following equation: （13） in Represents the vehicle's position. For constraint values, Indicates the scaling factor. Represents the polynomial coefficients. This represents a small displacement near the constraint value; Then, based on the concept of elastic bands, two sets of constraints are introduced: one set is tracking constraint and obstacle avoidance constraint, and the other set is velocity constraint. Tracking constraints and obstacle avoidance constraints: This set of constraints has two main objectives: to conform to the path obtained from the global planning and to avoid obstacles. The cost functions corresponding to these two objectives are very similar: one is to apply force to the elastic band to pull it toward the global path, and the other is to apply force to the elastic band to keep it away from obstacles. The specific formulas are as follows: （14） in, For pose sequences and global planning point sequences or obstacles The closest distance, This represents the maximum allowed distance between the pose sequence and the global planning point sequence. This represents the minimum permissible distance from the pose sequence to the obstacle. Indicates the scaling factor. Represents the polynomial coefficients. This represents a small displacement near the constraint value; Speed ​​constraints: The dynamic constraints determined by velocity can be represented by a penalty function of the constraints. The average linear velocity of the vehicle motion can be obtained through adjacent poses. , With time interval Calculated; （15） The constraint on linear velocity is expressed as: （16） In the above formula, The average linear velocity representing the vehicle's motion. Represents the x-coordinate of the i-th item in the vehicle pose sequence. Represents the ordinate of the i-th item in the vehicle pose sequence. Represents the tilt angle of the i-th item in the vehicle pose sequence. Represents a time interval; Based on the set objective function and constraints, the graph optimization algorithm in the general graph optimization library g2o in the ROS system is used. The linear equation is set, the solver is PCG, the iteration strategy is GN, and the final vehicle safety trajectory is obtained through repeated iterations. (5) Path tracing steps: Abstract the mathematical model of the vehicle, adopt a pure tracing algorithm, and obtain the vehicle's turning angle and linear velocity data by inputting the path planned by the navigation algorithm.

2. The high-speed mapping and navigation method based on Karto and Teb as described in claim 1, characterized in that, In step ④, the loop closure detection steps are as follows: Feature extraction: Extract key feature points from the data transmitted back by the LiDAR. Feature points are significant corners or edges. Feature matching: Match the features of the current frame with historical features. If the features of the current frame are similar to a certain historical feature, it is considered that the vehicle has made a loop, that is, the vehicle has reached the area it has been in the past. When loops exist, optimization is performed by combining the poses of historical frames and the current frame, using the least squares method. Map update: Update the map based on the optimized pose information.

3. The high-speed mapping and navigation method based on Karto and Teb as described in claim 2, characterized in that, In step (2), localization is mainly divided into two parts: prediction and update. The specific steps are as follows: In the prediction part, the algorithm uses the previous state estimate and the input control quantity to predict the current state estimate, as shown in the following formula: （7） in, Let f be the state estimate at time k, and let f be the nonlinear motion model function. Let k be the input control quantity at time k; In the update section, the algorithm uses the predicted state estimate and the measurement input to update the current state estimate. Specifically, the algorithm linearizes the measurement model around the predicted state estimate and then applies Kalman gain to correct the predicted state estimate. The mathematical formula is as follows: （8） Where K is the Kalman gain, P is the covariance matrix of the error, H is the Jacobian matrix of the measurement model, R is the covariance matrix of the measurement noise, z is the measurement input value, and h is the nonlinear measurement model.

4. The high-speed mapping and navigation method based on Karto and Teb as described in claim 3, characterized in that, In step (3), the global planning steps are as follows: For global path planning, based on the completed mapping and localization, we find the trajectory topology map nodes that are closest to the starting point and the ending point, and use the A* algorithm to find the shortest path connecting these two nodes on the topology map.

5. The high-speed mapping and navigation method based on Karto and Teb as described in claim 4, characterized in that, In step (5), the path tracing steps are as follows: To facilitate analysis, the model is simplified by imagining a tire at the center of the front wheel and another at the center of the rear wheel to represent the movement of the front and rear wheels, which is the so-called single-vehicle model. The control quantity is selected from the rear wheel speed. and front wheel steering angle State variables are selected from coordinates in the world coordinate system. and heading angle The kinematic model of Ackermann steering is then expressed as: （17） The corresponding odometer model is: （18） Based on the above equations, we can obtain: （19） The above formula That is the steering angle we requested. As an unknown quantity, due to curvature With radius The above equation can be rewritten as a reciprocal relationship: （20） The solution then becomes solving the expression on the right side of the equation, where Given , where is the wheelbase of the vehicle; the unknown quantity is the curvature. Next, we will solve this problem. ; From the Law of Sines, we can obtain: （21） Considering time, equation (20) can be expressed as: （22） Based on the above mathematical modeling calculations, the pure tracking navigation algorithm becomes a process of solving for α at each time step; The vehicle's current position is obtained by reading filtered data from the onboard odometer and used as input for the pure tracking algorithm. Based on the target object's position and its own position, the servo motor angle is calculated using the mathematically derived formula (22) to obtain the vehicle's driving angle and angular velocity. Specifically, given the target object's position and its own position, i.e., given the target object's position and its own position, the servo motor angle is calculated. and Given that the vehicle's wheelbase L is known by default, it can be calculated that... ; Based on calculations The value is the driving speed set by the user, which is input into the drive functions of the vehicle's motor and servo motor to control the vehicle to perform the corresponding actions.

Images