ORB-SLAM and intelligent optimization combined vehicle path optimization method and system, computer storage medium and program

Abstract

The invention discloses a vehicle path optimization method combining ORB-SLAM and intelligent optimization. The method comprises the following steps that a sparse map and a grid map are established through an ORB-SLAM 3 algorithm according to image data obtained by a vehicle camera in real time and movement of a vehicle; creating a plurality of vehicle driving path scatter point coordinate positions through an experimental design method based on the established grid map; selecting the vehicle driving path scatter point coordinate position with the shortest distance from the starting position to the final position of the vehicle as the optimal scatter point coordinate position; determining a secondary precise optimization range of each optimal scatter point coordinate position based on the optimal scatter point coordinate position, and optimizing the population individual coordinate position by adopting an optimization algorithm to obtain a vehicle driving optimal path point; and performing multi-point navigation on the vehicle according to the optimal path points, and issuing a next optimal path point to the vehicle when the vehicle reaches a set range of one optimal path point until all the optimal path points are issued. According to the invention, the vehicle path planning precision and optimization efficiency can be improved.

Description

Technical Field

[0001] The invention relates to a vehicle path optimization method, system, computer storage medium and program, and belongs to the technical field of path planning. Background Art

[0002] During vehicle driving, environmental changes (such as traffic conditions, road conditions, etc.) will have a significant impact on path selection. Therefore, a system is needed that can make timely decisions and behavior planning based on environmental changes to ensure that the vehicle can drive safely and efficiently.

[0003] Traditional path planning methods usually rely on static maps and simple algorithms, which face many challenges in dynamic environments. Static maps cannot reflect real-time changes in the environment, resulting in a lack of flexibility in path planning, and map data needs to be updated regularly, otherwise outdated information may be used, affecting navigation results. Simple algorithms perform poorly in real-time changing environments, especially in large-scale environments, where the computational complexity is high, which may result in long computation times and affect efficiency. In the face of dynamic obstacles and emergencies, accurate path planning may not be provided. Summary of the invention

[0004] In view of the above-mentioned defects in the prior art, the task of the present invention is to provide a vehicle path optimization method combining ORB-SLAM and intelligent optimization to improve the vehicle path planning accuracy and optimization efficiency. Another task of the present invention is to provide a vehicle path optimization system combining ORB-SLAM and intelligent optimization, and to provide a computer storage medium and program.

[0005] The technical solution of the present invention is as follows: A vehicle path optimization method combining ORB-SLAM and intelligent optimization, comprising the steps of: S1, the ORB-SLAM3 algorithm creates a sparse map and a grid map based on the image data acquired in real time by the vehicle camera and the movement of the vehicle; S2, creating multiple scattered coordinate positions of vehicle driving paths through an experimental design method based on the established grid map; S3, selecting the scattered point coordinate position of the vehicle driving path with the shortest distance from the vehicle starting position to the end position as the optimal scattered point coordinate position; S4, determining the secondary precise optimization range of each best scattered point coordinate position based on the best scattered point coordinate position, and optimizing the individual coordinate positions of the population using an optimization algorithm to obtain the optimal path point; S5. Perform multi-point navigation on the vehicle according to the optimal path points, and publish the next optimal path point to the vehicle when the vehicle reaches a set range of an optimal path point, until all optimal path points are published.

[0006] Furthermore, the current position of the vehicle is determined by pure positioning by ORB-SLAM3 based on the image data acquired in real time by the vehicle camera and the established sparse map.

[0007] Furthermore, step S3 includes first determining whether each scattered point coordinate position can hit an obstacle, and canceling the scattered point combination that hits the obstacle; calculating the distance from the starting position to the end position through the scattered point combination coordinates that cannot hit the obstacle; comparing the driving distances of all sample vehicle driving paths, and finding the optimal scattered point coordinate position corresponding to the vehicle driving path with the shortest distance.

[0008] Furthermore, in step S4, the optimization algorithm is used to optimize the coordinate positions of the individuals in the population to obtain the optimal path point, which specifically includes: Step S401, set the iterative calculation step number s=1 and the maximum iterative step number, randomly generate a population D(s) within the optimization range, and each individual in D(s) consists of the coordinate position of a scattered point; Step S402, calculating the distance traveled by each individual in the population D(s); Step S403: For individuals in population D(s) Update to get a new population D(s+1), where: and Remove individuals from the population D(s) Two random individuals other than the above, μ is the set control parameter; Step S404, calculating the distance traveled by each individual in the new population D(s+1); Step S405, compare the travel distances of individuals in the new population D(s+1) and the original population D(s). If the travel distance of individuals in the new population D(s+1) is smaller than that of individuals in the original population D(s), individuals in the new population D(s+1) are used to replace individuals in the original population D(s). Otherwise, individuals in the original population D(s) are retained to form a new population D(s+2). Step S406: When s does not reach the maximum number of iterations, set s=s+3 and return to step S402; otherwise, end the optimization.

[0009] Furthermore, μ is 0-2.

[0010] Furthermore, the formula for calculating the travel distance of each individual in the population D(s) in step S402 is: , where n≥3; The formula for calculating the distance traveled by each individual in the population D(s+1) in step S404 is: , where n≥3.

[0011] Furthermore, in step S2, the creation of multiple scattered coordinate positions of the vehicle driving path is constructed using an optimal Latin hypercube algorithm.

[0012] Another technical solution of the present invention is: a vehicle path optimization system combining ORB-SLAM and intelligent optimization, comprising the following modules: Initial processing module: used by the ORB-SLAM3 algorithm to create sparse maps and grid maps based on the image data acquired in real time by the vehicle camera and the movement of the vehicle; Scatter point creation module: used to create multiple scattered point coordinate positions of vehicle driving paths through experimental design method based on the established grid map; The best scattered point selection module is used to select the scattered point coordinate position of the vehicle driving path with the shortest distance from the vehicle starting position to the end position as the best scattered point coordinate position; Optimal path point determination module: used to determine the secondary precise optimization range of each optimal scattered point coordinate position based on the optimal scattered point coordinate position, and use the optimization algorithm to optimize the individual coordinate position of the population to obtain the optimal path point; And, a navigation module: used to perform multi-point navigation on the vehicle according to the optimal path point, and publish the next optimal path point to the vehicle when the vehicle reaches a set range of an optimal path point, until all optimal path points are published.

[0013] Another technical solution of the present invention is: a computer storage medium having a computer program stored thereon, wherein when the computer program is executed by a processor, the vehicle path optimization method combining ORB-SLAM and intelligent optimization is implemented.

[0014] Yet another technical solution of the present invention is: a computer program, which, when executed by a processor, implements the vehicle path optimization method combining ORB-SLAM and intelligent optimization.

[0015] The advantages of the present invention compared with the prior art are: The present invention combines ORB-SLAM technology with an intelligent optimization algorithm, realizes the integration of positioning and path planning in a navigation system, and effectively improves the smoothness and driving efficiency of vehicles in various complex environments, which helps to reduce energy consumption and improve the stability of vehicles during passage. The optimal Latin hypercube algorithm is used to preliminarily obtain the best feasible area of ​​the entire space, and then the optimization algorithm is used to perform a detailed search in the best feasible area. The preliminary optimization strategy using the optimal Latin hypercube algorithm can quickly find the best feasible path suitable for the current map, and perform secondary optimization calculations through the optimization algorithm to obtain the shortest vehicle driving path. The ORB-SLAM3 visual navigation technology and dual optimization strategy can effectively solve the rapid design and automated driving of vehicle paths, and can effectively improve the accuracy and computational efficiency of vehicle path planning. The present invention has broad application prospects by combining the environmental perception ability of ORB-SLAM with the path planning ability of the intelligent optimization algorithm. BRIEF DESCRIPTION OF THE DRAWINGS

[0016] Figure 1 It is a flowchart of a vehicle path optimization method combining ORB-SLAM and intelligent optimization according to an embodiment.

[0017] Figure 2 It is the optimal scattered point location result of the vehicle's driving path. DETAILED DESCRIPTION

[0018] The present invention will be further described below in conjunction with the embodiments, but are not intended to be limiting of the present invention.

[0019] The vehicle path optimization system combining ORB-SLAM and intelligent optimization in this embodiment includes the following modules: Initial processing module: used by the ORB-SLAM3 algorithm to create sparse maps and grid maps based on the image data acquired in real time by the vehicle camera and the movement of the vehicle; Scatter point creation module: used to create multiple scattered point coordinate positions of vehicle driving paths through experimental design method based on the established grid map; The best scattered point selection module is used to select the scattered point coordinate position of the vehicle driving path with the shortest distance from the vehicle starting position to the end position as the best scattered point coordinate position; Optimal path point determination module: used to determine the secondary precise optimization range of each optimal scattered point coordinate position based on the optimal scattered point coordinate position, and use the optimization algorithm to optimize the individual coordinate position of the population to obtain the optimal path point; And, the navigation module: is used to perform multi-point navigation for the vehicle according to the optimal path point, and publish the next optimal path point to the vehicle when the vehicle reaches the set range of an optimal path point, until all the optimal path points are published.

[0020] Please combine Figure 1 As shown, the vehicle path optimization method implemented by the system includes the following steps: S1. The ORB-SLAM3 algorithm creates a sparse map and a grid map based on the image data acquired in real time by the vehicle camera and the movement of the vehicle.

[0021] The specific steps include: S101, install the required dependencies, including OpenCV library, Pangolin and Eigen. OpenCV library provides tools for computer vision and image processing, Pangolin is used for SLAM visualization, helping to intuitively display related data; Eigen is a linear algebra calculation library. The installation of these dependencies ensures that ORB-SLAM and intelligent optimization algorithms can run smoothly.

[0022] S102. Enter the working directory and set environment variables to start the simulation environment.

[0023] S103, run ORB-SLAM3, and use the image data obtained in real time by the camera for feature extraction and map construction; the ORB feature points and descriptor data extracted from the image are used for feature matching in the SLAM algorithm; the three-dimensional point cloud data obtained through depth calculation and dense reconstruction represents each point in the environment and its three-dimensional coordinates; the generated dense three-dimensional map contains detailed information such as obstacles and terrain in the environment; and the real-time position information calculated by the SLAM algorithm represents the current position of the vehicle in the environment.

[0024] S104. The system efficiently acquires point cloud data of the environment from various sensors to generate and finely manage 3D grid maps. At the same time, it uses Rviz tools for visualization, presenting abstract data in an intuitive visual form, making it easier for operators to observe and analyze the system operation status in real time.

[0025] S105. Complete the establishment of the sparse map and the grid map by controlling the movement of the vehicle, and avoid pure rotation and rapid rotation in the process.

[0026] S2. Create multiple scattered coordinate positions of vehicle driving paths through experimental design method based on the established grid map.

[0027] Specifically, the process includes: step S201, running ORB-SLAM3 for pure positioning. The image data acquired in real time by the camera is used for pure positioning. The ORB feature points and descriptor data extracted from the image are used for feature matching in the SLAM algorithm. The real-time position information calculated by the SLAM algorithm represents the current position of the vehicle in the environment.

[0028] Step S202: construct multiple scattered point coordinate positions of vehicle driving paths using the optimal Latin hypercube algorithm.

[0029] S3, select the vehicle travel path scatter point coordinate position with the shortest distance from the vehicle current position to the end position as the optimal scatter point coordinate position, and the result is as follows Figure 2 shown.

[0030] Specifically, it includes: judging whether each scattered point coordinate position of the vehicle's driving path can hit an obstacle. If it hits an obstacle, the scattered point coordinate combination is cancelled. If it cannot hit an obstacle, the distance from the vehicle's starting position (starting point) through these corresponding scattered point coordinate positions to the end position is calculated. Compare the driving distances of all the scattered point coordinate positions of the vehicle's driving path (excluding the combination of points that collide with obstacles) to find the scattered point position corresponding to the driving path with the smallest distance. In this embodiment, the optimal scattered point position of the vehicle's driving path is found by calculation. Figure 2 As shown in the figure, the black filled circle represents an obstacle, and the circle represents the extreme position reached by the center position of the vehicle. If the center position of the vehicle is within the circle, the vehicle will hit the obstacle.

[0031] S4, based on the best scatter point coordinate position (ie: Figure 2 The coordinate positions of the four points in the middle are used to determine the second precise optimization range of the coordinate position of each best scattered point, and the optimization algorithm is used to optimize the coordinate position of the individual population to obtain the optimal path point.

[0032] Specifically, it includes: S400, determining the optimization design variable range according to the best scattered point coordinate position, which is usually determined as a certain radius range within the best scattered point coordinate position.

[0033] Step S401: Set the iterative calculation step number s=1 and the maximum number of iterations, and randomly generate a population D(s) within the optimization range. Each individual in D(s) consists of the coordinate position of a scattered point. In this embodiment, it is assumed that there are four optimal scattered point coordinate positions, then D(s)={d(s)1, d(s)2, ..., d(s) n}, where d(s) i ={d(s) i1 , d(s) i2 , d(s) i3 , d(s) i4 , d(s) i5 , d(s) i6 , d(s) i7 , d(s) i8}. d(s) i1 , d(s) i2 are the horizontal and vertical coordinates of the first scattered point in the sth step, d(s) i3 , d(s) i4are the horizontal and vertical coordinates of the second scattered point in the sth step, d(s) i5 , d(s) i6 are the horizontal and vertical coordinates of the third scattered point in the sth step, d(s) i7 , d(s) i8 They are the horizontal and vertical coordinates of the fourth scatter point in the sth step respectively.

[0034] Step S402, calculating the distance traveled by each individual in the population D(s); for , where n≥3.

[0035] In this embodiment, .

[0036] Step S403: Update the individuals in population D(s) to obtain a new population D(s+1) = {d(s+1)1, d(s+1)2,..., d(s+1) n}, where: and Remove individuals from the population D(s) Two random individuals other than the above, μ is the set control parameter, with a value of 0 to 2; For this embodiment, d(s+1) i ={d(s+1) i1 , d(s+1) i2 , d(s+1) i3 , d(s+1) i4 , d(s+1) i5 , d(s+1) i6 , d(s+1) i7 , d(s+1) i8}.

[0037] Step S404, calculating the distance traveled by each individual in the population D(s+1); , where n≥3.

[0038] In this embodiment, .

[0039] Step S405, compare the travel distances of individuals in the new population D(s+1) and the original population D(s). If the travel distance of individuals in the new population D(s+1) is smaller than that of individuals in the original population D(s), individuals in the new population D(s+1) are used to replace individuals in the original population D(s). Otherwise, individuals in the original population D(s) are retained to form a new population D(s+2). Step S406: When s does not reach the maximum number of iterations, set s=s+3 and return to step S402. Otherwise, the optimization ends and the best scatter point position is output as the optimal path point, denoted as opt. i ={opt i1 , opt i2 , opt i3 , opt i4 , opt i5 , opt i6 ,opt i7 , opt i8}.

[0040] S5: Perform multi-point navigation for the vehicle based on the optimal path point, and the publisher publishes the coordinate value of the first optimal path point (opt i1 , opt i2 ), after the subscriber subscribes to the coordinate, the vehicle moves to the point according to the instruction. When the vehicle reaches the set range of the first point, the publisher publishes the coordinate value of the next point to the vehicle (opt i3 , opt i4 ), and so on, the vehicle passes through the point (opt i5 , opt i6 ) and (opt i7 , opt i8 ) until you reach the end of the map.

[0041] Finally, it should be pointed out that the specific methods of the above embodiments can form a computer program product. Therefore, the computer program product implemented in this application can be stored on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.).

Claims

1. A vehicle path optimization method combining ORB-SLAM and intelligent optimization, characterized in that: The following steps are involved: S1, the ORB-SLAM3 algorithm creates a sparse map and a grid map based on the image data acquired in real time by the vehicle camera and the movement of the vehicle; S2, creating multiple scattered coordinate positions of vehicle driving paths through an experimental design method based on the established grid map; S3, selecting the scattered point coordinate position of the vehicle driving path with the shortest distance from the vehicle starting position to the end position as the optimal scattered point coordinate position; S4, based on the best scattered point coordinate position, determine the second precise optimization range of each best scattered point coordinate position, and use the optimization algorithm to optimize the individual coordinate position of the population to obtain the optimal path point; S5. Perform multi-point navigation on the vehicle according to the optimal path points, and publish the next optimal path point to the vehicle when the vehicle reaches a set range of an optimal path point, until all optimal path points are published.

2. The vehicle path optimization method combining ORB-SLAM and intelligent optimization according to claim 1, characterized in that: The current position of the vehicle is determined by pure positioning by ORB-SLAM3 based on the image data acquired in real time by the vehicle camera and the established sparse map.

3. The vehicle path optimization method combining ORB-SLAM and intelligent optimization according to claim 1, characterized in that: The step S3 includes first determining whether each scattered point coordinate position can hit an obstacle, and canceling the scattered point combination that hits the obstacle; calculating the distance from the starting position to the end position through the scattered point combination coordinates that cannot hit the obstacle; comparing the driving distances of all sample vehicle driving paths, and finding the optimal scattered point coordinate position corresponding to the vehicle driving path with the shortest distance.

4. The vehicle path optimization method combining ORB-SLAM and intelligent optimization according to claim 1, characterized in that: In step S4, an optimization algorithm is used to optimize the coordinate positions of the individuals in the population to obtain the optimal path point, which specifically includes: Step S401, set the iterative calculation step number s=1 and the maximum iterative step number, randomly generate a population D(s) within the optimization range, and each individual in D(s) consists of the coordinate position of a scattered point; Step S402, calculating the distance traveled by each individual in the population D(s); Step S403: For individuals in population D(s) Update to get a new population D(s+1), where: and Remove individuals from the population D(s) Two random individuals other than the above, μ is the set control parameter; Step S404, calculating the distance traveled by each individual in the new population D(s+1); Step S405, compare the travel distances of individuals in the new population D(s+1) and the original population D(s). If the travel distance of individuals in the new population D(s+1) is smaller than that of individuals in the original population D(s), individuals in the new population D(s+1) are used to replace individuals in the original population D(s). Otherwise, individuals in the original population D(s) are retained to form a new population D(s+2). Step S406: When s does not reach the maximum number of iterations, set s=s+3 and return to step S402; otherwise, end the optimization.

5. The vehicle path optimization method combining ORB-SLAM and intelligent optimization according to claim 4, characterized in that: μ is 0~2.

6. The vehicle path optimization method combining ORB-SLAM and intelligent optimization according to claim 4, characterized in that: The formula for calculating the travel distance of each individual in the population D(s) in step S402 is: , where n≥3; The formula for calculating the distance traveled by each individual in the population D(s+1) in step S404 is: , where n≥3.

7. The vehicle path optimization method combining ORB-SLAM and intelligent optimization according to claim 1, characterized in that: In the step S2, the optimal Latin hypercube algorithm is used to construct the multiple scattered coordinate positions of the vehicle driving path.

8. A vehicle path optimization system combining ORB-SLAM and intelligent optimization, characterized in that: Includes the following modules: Initial processing module: used by the ORB-SLAM3 algorithm to create sparse maps and grid maps based on the image data acquired in real time by the vehicle camera and the movement of the vehicle; Scatter point creation module: used to create multiple scattered point coordinate positions of vehicle driving paths through experimental design method based on the established grid map; The best scattered point selection module is used to select the scattered point coordinate position of the vehicle driving path with the shortest distance from the vehicle starting position to the end position as the best scattered point coordinate position; Optimal path point determination module: used to determine the secondary precise optimization range of each optimal scattered point coordinate position based on the optimal scattered point coordinate position, and use the optimization algorithm to optimize the individual coordinate position of the population to obtain the optimal path point; And, a navigation module: used to perform multi-point navigation on the vehicle according to the optimal path point, and publish the next optimal path point to the vehicle when the vehicle reaches a set range of an optimal path point, until all optimal path points are published.

9. A computer storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by the processor, the vehicle path optimization method combining ORB-SLAM and intelligent optimization is implemented.

10. A computer program, characterized in that When the computer program is executed by the processor, the vehicle path optimization method combining ORB-SLAM and intelligent optimization is implemented.

Images