Path planning method

Abstract

The invention relates to a path planning method, which is used for automatically planning a driving path from a departure place to a destination for a vehicle, and comprises the following steps: a data acquisition step: acquiring three-dimensional environment data around the vehicle in a current scene observed by the vehicle, which describes road and traffic conditions in the current environment of the vehicle; a candidate path determination step: taking the position of the vehicle when the vehicle observes the current scene as a starting point of the current scene, and determining at least one target which can be reached from the starting point and one or more candidate paths from the starting point to the target in the current scene according to the three-dimensional environment data; a total path cost calculation step: calculating the total path cost of each candidate path based on the three-dimensional environment data; in the planning path selection step, the candidate path with the lowest total path cost is selected as the planning path in the current scene, and the candidate path determination step, the total path cost calculation step and the planning path selection step are realized in a deep learning network in which a microbial tree group row algorithm is introduced.

Description

Technical Field

[0001] The present invention relates to a path planning method for automatically planning a driving path from a starting point to a destination for a vehicle. The present invention also relates to a method for correcting a three-dimensional environment model reconstructed from a two-dimensional image. Background Art

[0002] Path planning based on multi-vision sensor fusion is a prerequisite for autonomous vehicles in complex environments. For example, in a mine environment, unmanned mining trucks usually use multiple vision sensors, such as two monocular cameras, to detect their surrounding environment. In order to perform path planning based on the two-dimensional image data of the monocular cameras, it has been proposed to perform data fusion on the two-dimensional image data detected by the two monocular cameras, and perform three-dimensional reconstruction based on the fusion data to obtain three-dimensional environment data of the vehicle's surrounding environment, and then perform path planning based on the three-dimensional environment data. Among them, before detection, the monocular cameras need to be calibrated first to determine their external parameters and internal parameters, and adjusted to make the external parameters and internal parameters of the two monocular cameras consistent.

[0003] However, current path planning algorithms generally have problems of poor real-time performance and insufficient accuracy in industrial environments. Summary of the Invention

[0004] The object of the present invention is to provide an adaptive bionic path planning method implemented by a deep learning network, which can overcome at least one defect existing in the prior art mentioned above.

[0005] This object is achieved by a method and a computer-readable storage medium having the following characteristics.

[0006] According to one aspect of the present invention, a path planning method is provided. This path planning method is used to automatically plan a driving path for a vehicle from a starting point to a destination. The path planning method includes the following steps: a data acquisition step, in which three-dimensional environmental data around the vehicle in the current scene observed by the vehicle is acquired. This three-dimensional environmental data describes the roads and traffic conditions in the vehicle's current environment, and the three-dimensional environmental data includes information related to obstacles on the road and information related to the vehicle ahead, especially congestion information; a candidate path determination step, in which the position where the vehicle is located when observing the current scene is used as the starting point of the current scene. According to the three-dimensional environmental data, at least one target that can be reached from this starting point is determined in the current scene, and one or more candidate paths from this starting point to the at least one target are determined; a total path cost calculation step, in which the total path cost of each candidate path is calculated based on the environmental data, especially based on the information related to obstacles and congestion information; a planned path selection step, in which the candidate path with the lowest total path cost is selected as the planned path within the current scene. Among them, the candidate path determination step, the total path cost calculation step, and the planned path selection step are implemented in a deep learning network introducing a microbial slime mold algorithm. In particular, after driving along the planned path to the corresponding target, this corresponding target is used as a new starting point, the scene observed by the vehicle at this new starting point is used as the new current scene, new three-dimensional environmental data is acquired, and a planned path within the new current scene is determined based on the new environmental data, and so on in a loop until the destination is reached. The whole process is carried out automatically. That is to say, based on the three-dimensional environmental data of each scene, especially the obstacles and congestion information on the candidate path, the deep learning network imitates the microbial slime mold network to select the planned path with the lowest total path cost as the goal, and the segments of the planned path are connected to form the entire planned path from the starting point to the destination.

[0007] Therefore, the present invention proposes an improved path planning method for a complex industrial environment. This path planning method is a highly adaptive bionic mathematical model, which can automatically plan complex trajectories, improve the real-time performance of the path planning algorithm and the accuracy of environmental reconstruction, while improving the vehicle environmental positioning accuracy and realizing automatic navigation based on the vehicle system.

[0008] In particular, when the three-dimensional environmental data is obtained through three-dimensional reconstruction by fusing two-dimensional data from multiple vision sensors, such as two monocular cameras, the bionic path planning method can also be combined with the adaptive loop detection algorithm implemented by using the image control point function of the present invention. The three-dimensional environmental model reconstructed is corrected through image control points, the environmental positioning accuracy is improved, and the environmental model is generated more accurately, thereby providing a basis for more accurate path planning. In the adaptive loop detection method of the present invention, a control point is identified in the images detected by multiple vision sensors by using an RBF (Radial Basis Function) classifier. The three-dimensional environmental model is corrected by adjusting the model coordinates of the control point in the three-dimensional environmental model to the true geographical coordinates (Ground Truth) of the control point. In particular, the RBF classifier is a radial basis function neural network based on Fourier transform, the integral kernel of the radial basis function neural network is parameterized in the Fourier space, and / or the RBF classifier extracts the normalized Fourier coefficients of the image as feature vectors to learn the target image features.

[0009] According to another aspect of the present invention, a method for correcting a three-dimensional environmental model reconstructed from a two-dimensional image is proposed. The method includes the following steps: identifying a control point in the image detected by a sensor by using an RBF classifier; and adjusting the model coordinates of the control point in the three-dimensional environmental model to the true geographical coordinates of the control point to correct the three-dimensional environmental model, where the RBF classifier is a radial basis function neural network based on Fourier transform, the integral kernel of the radial basis function neural network is parameterized in the Fourier space, and / or the RBF classifier extracts the normalized Fourier coefficients of the image as feature vectors to learn the target image features.

[0010] According to another aspect of the present invention, a computer-readable storage medium is proposed, on which a computer program is stored. The computer program includes executable instructions that, when executed by a processor, implement the path planning method described above or the method for correcting a three-dimensional environmental model reconstructed from a two-dimensional image.

[0011] It should be understood that the above general description and the following detailed description are only exemplary and explanatory, and do not limit the protection scope of the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

[0012] The present invention will be described in detail below with reference to the accompanying drawings. In the drawings,

[0013] Figure 1 A road network is schematically shown.

[0014] Figure 2A flowchart showing a path planning method based on three-dimensional environmental data according to the present invention.

[0015] Figure 3 Shows the three-dimensional environmental model reconstruction process based on multi-vision sensor data fusion.

[0016] Figure 4 Shows the projection of point P in the world coordinate system onto the image coordinate system.

[0017] Figure 5 Shows the conversion between camera coordinates and image coordinates. Detailed implementation manners

[0018] In an urban road system or a mine environment, each road is connected and intersects with each other, usually forming a network with multiple nodes, that is, a so-called road network. Vehicles or vehicle fleets need to periodically travel back and forth between multiple locations in the road network to transport, for example, goods or mineral resources. In Figure 1 a road network is schematically shown, which includes multiple nodes S, A, B, C, D, Q, E. Each node is an intersection point of two or more road segments, and a road segment is a road part between every two adjacent nodes. Therefore, there may be multiple paths available to travel from one location to another in the road network (which can be a node or a point on a road segment). Each path may include one road segment or multiple mutually connected road segments. For example, assuming node S is the starting point and node E is the destination, there may be multiple alternative paths from the starting point S to the destination E, such as the path SCE composed of road segments SC and CE, the path SACE composed of road segments SA, AC, and CE, the path SDQE composed of road segments SD, DQ, and QE, the path SBDQE composed of road segments SB, BD, DQ, and QE, and so on. In an urban road system, a node can be various intersections, such as crossroads, T-intersections, roundabouts, etc. In a mine environment, a node can be a working point (such as a loading point, an unloading point, a refueling point) or an intersection point of mine roads. Additionally, it should be noted that in Figure 1 although each road segment is shown as a straight line, this does not mean that the actual extension direction of the road segment must be straight, but it can also have a winding extension direction.

[0019] Figure 2An embodiment of a method for automatically planning a driving route from a starting point to a destination for a vehicle according to the present invention is shown. This method mimics the network composed of slime molds. The microorganism is a slime mold called "Physarum polycephalum", and the network composed of slime molds can achieve nutrient transport extremely efficiently through cytoplasmic transport. If the nutrients are extremely sufficient, the bacterial population will enter a wild growth state and quickly occupy space, and this growth mode is called "dendritic swarming". The bacterial population usually forms a thin film structure called a "biofilm". When the biofilm expands, the number of single cells increases, the number of paths also increases, the walking cost also increases, and a complex regular structure will be generated to achieve optimization. This path selection of slime molds is spontaneous in real time and dynamically according to the current situation. To mimic this regular structure, it is assumed that each slime mold has its own memory, which includes using a network parameter array A to store the nodes that the slime mold has visited, and the slime mold will not be able to visit these nodes in subsequent searches. This memory also includes using another network parameter array B to store the nodes that the slime mold can still visit. When the bacterial population explores in a strange environment, if a path has extremely scarce nutrients, it will leave a transparent chemical substance behind it to warn not to take this route again. This chemical substance can be represented by a matrix L. At this time, the activation function softmax of the connection network outputs 0 to warn not to take this route again, and outputs 1 to indicate that this route can be taken. In addition, a matrix D is used to store the transparent chemical substance L left behind by the slime molds passing through in a cycle (or iteration). The calculation process of the dendritic swarming algorithm is as follows: Step (1), perform initialization. Step (2), select the next node for each slime mold. Step (3), update the matrix L of the transparent chemical substance left behind by the slime mold. Step (4), check the termination condition. If the maximum number of iterations is reached, the algorithm terminates and goes to Step (5) to output the optimal value. Otherwise, re-initialize the D matrix of all slime molds, initialize all elements to 0, clear the A table, add all nodes to the B table. Randomly select or manually specify their starting positions, add the starting node to the A table, and remove the starting node from the B table, and repeat the above steps (2)-(4).

[0020] The present invention mimics this method of slime molds that can quickly find a suitable path, and introduces the "dendritic swarming" algorithm of microorganisms to use a deep learning network to optimize path planning. Among them, the vehicle is regarded as a slime mold, and always preferentially selects a low-cost path with less fuel consumption, shorter time and / or shorter distance for the vehicle to go to the destination. Especially for the situation where a vehicle or a fleet needs to travel back and forth between two fixed locations in the road network periodically, this deep learning network gradually learns and "remembers" the obstacle conditions of each section of the road network, the driving difficulty of the road surface, and the congestion conditions on each section, so as to better and better plan the optimal driving path.

[0021] In the path planning method according to the present invention, first, three-dimensional environment data around the vehicle in the current scene observed by the vehicle is acquired (S201). The current scene refers to the area of the environment around the vehicle that can be detected by the sensors on the vehicle at present, especially the area in front of the vehicle. The sensors may include, for example, vision sensors (such as monocular cameras, stereo cameras), lidar, microwave radars, etc. The three-dimensional environment data describes the road and traffic conditions in the current environment of the vehicle, including information about obstacles on the road and the vehicle in front. That is to say, the three-dimensional environment data constructs a three-dimensional model of the environment around the vehicle. This model gives, for example, the boundaries and directions of the road in the current scene, the shape, size, and position of the obstacles on the road, and the shape, size, and position of the vehicle in front.

[0022] The three-dimensional environment data can be acquired by any known method. For example, the three-dimensional environment data can be directly detected by a stereo camera or lidar. Alternatively, the three-dimensional environment data can also be obtained through data fusion and three-dimensional reconstruction based on two-dimensional image data detected by, for example, a monocular camera.

[0023] Next, in step S202, based on the three-dimensional environment data, one or more candidate paths in the current scene are determined. Here, the position where the vehicle is located when observing the current scene, especially the position of the sensors on the vehicle, is used as the starting point of the current scene. Then, according to the three-dimensional environment data, at least one target that can be reached from this starting point in the current scene is determined, and one or more candidate paths from this starting point to the at least one target are determined. Here, there may be multiple paths starting from the starting point, and these paths do not cross or converge with each other. Therefore, there are correspondingly multiple different targets and thus multiple candidate paths. Of course, it is also possible that at least a part of these paths converge with each other. In this case, there are also multiple candidate paths from the starting point to the same target. The target of the corresponding candidate path can be selected as the point farthest from the starting point on each candidate path starting from the starting point in the current scene, and / or selected as the position of the last vehicle in front on each candidate path in the current scene. In another embodiment, the target can also be determined such that the candidate path from the starting point to the target extends in the direction towards the destination.

[0024] In step S203, the total path cost of each candidate path is calculated based on the environment data. The factors affecting the total path cost can be, for example, whether there are obstacles to be avoided on the road and the number thereof, whether there are other vehicles in front of the road and whether it is congested, whether the road has a bend, the number of bends and the radius of curvature of the bends, whether the road has an uphill or downhill and the size of the slope, whether the road surface is slippery or muddy and difficult to travel, etc. For each factor, a corresponding cost function Gn can be given in advance. iCalculate the path cost of the vehicle driving on the road affected by this factor according to (i = 1, 2, 3,..., m,...). For example, the cost function Gn i can be represented by the fuel consumption, the required time, or the distance traveled by the vehicle on the section with the corresponding factor. Therefore, configure the corresponding weight γ according to the obtained three-dimensional environmental data i , and the corresponding factor can be incorporated into the calculation of the total path cost. Therefore, the total path cost can be calculated as:

[0025] Fn = γ1Gn1 + γ2Gn2 + … + γ m Gn m + … = ∑ i γ i Gn i (1),

[0026] where Fn is the total path cost of each candidate path, Gn1, Gn2, … Gn m , … are the cost functions for the corresponding factors, and γ1, γ2, … γ m , … are the weight coefficients for the corresponding factors. The weight coefficients γ1, γ2, … γ m , … can take values in the range [0, 1]. The values of the weight coefficients γ1, γ2, … γ m , … are determined by the environmental data. For example, taking the cost function used to calculate the path cost caused by obstacles as an example, if there are no obstacles on the candidate path, or the obstacles are small and close to the roadside and do not need to be avoided, the weight coefficient configured for this cost function can be 0, which means that the path cost caused by obstacles is zero; if there are many obstacles to be avoided in the middle of the road, a larger weight coefficient such as 0.8, 0.9 or 1 is configured for this cost function. Therefore, the contribution of the corresponding factor to the total path cost can also be reflected by the magnitude of the value of the weight coefficient. Generally speaking, for a path with fewer obstacles, less congestion, no sharp turns, and a non-muddy road surface, the corresponding weight coefficient is configured to be smaller. On the contrary, the more obstacles there are, the more congested it is, the more sharp turns there are, and the muddier the road surface is, the larger the corresponding weight coefficient is configured.

[0027] For example, assume that only three cost functions are considered. Among them, Gn1 is used to calculate the path cost of driving on a road when there are many obstacles to avoid on the road, Gn2 is used to calculate the path cost of driving on a road when there is serious traffic congestion, and Gn3 is used to calculate the path cost of driving on a road when there are sharp turns or the road surface is muddy and difficult to drive. If for a certain candidate path, it can be known from the obtained environmental data that this path has no turns and the road surface is flat and easy to drive, but there are many obstacles and the congestion is very serious, the weight coefficients can be configured as follows: γ1 = 0.9, γ2 = 1, γ3 = 0. Thus, the total path cost of this candidate path is Fn = 0.9 * Gn1 + Gn2. In this way, according to the actual road and traffic conditions on each candidate path, corresponding weight coefficients are configured for each cost function to calculate the total path cost of each candidate path.

[0028] In step S204, select the candidate path with the minimum total path cost among all candidate paths in the current scenario as the planned path within the current scenario. Here, compare the total path costs of all candidate paths and select the candidate path with the minimum total path cost as the optimal path.

[0029] In step S205, after driving along the planned path to its corresponding target, take this corresponding target as the new starting point, take the scenario observed by the vehicle at this new starting point as the new current scenario, and repeat steps S201 to S204 to obtain new three-dimensional environmental data, and determine the planned path within the new current scenario based on the new three-dimensional environmental data. Repeat this cycle until the destination is reached. Thus, the driving path from the starting point to the destination is composed of the planned paths in multiple interconnected scenarios.

[0030] The following takes Figure 1Taking the road network shown as an example, an example of implementing the path planning method of the present invention is given. Suppose a vehicle or a fleet of vehicles often needs to drive from the starting point S to the destination E. When the first vehicle departs from the starting point S for the first time, taking the starting point S as the starting point of the current scenario, the three-dimensional environmental data of the current scenario is detected, and it is determined from the three-dimensional environmental data that there are four paths SA, SC, SD, and SB starting from the starting point S in the current scenario, and along these four paths, four targets A′, C′, D′, and B′ can be reached. Among them, the targets A′, C′, D′, and B′ are respectively the farthest points that can be reached along the corresponding paths within the current scenario. Therefore, there are four candidate paths SA′, SC′, SD′, and SB′ from the starting point S to these four targets. Then, the total path costs of these four candidate paths are calculated respectively. Suppose there are more obstacles on the candidate path SA′, and fewer obstacles on the other candidate paths SC′, SD′, and SB′, and there are no other vehicle congestions and the road surface is flat and easy to travel on each candidate path. Therefore, the total path costs of the candidate paths SC′, SD′, and SB′ are equal and the lowest. Therefore, a candidate path is randomly selected, assumed to be SC′, as the planned path in the current scenario. Alternatively, according to the principle that the shorter the path and the less time-consuming, the lower the total path cost from the starting point S to the destination E. Since the target C′ is the closest to the destination E, the candidate path SC′ is selected as the planned path in the current scenario.

[0031] When the vehicle arrives at the target C′ along the planned path SC′, taking the target C′ as the new starting point, the three-dimensional environmental data of the current scenario is detected again. It can be determined from the new three-dimensional environmental data that there is only one path C′C starting from the starting point C′ in the current scenario, but there are three paths CA, CE, and CQ starting from C, and along these three paths, three targets R, E′, and Q′ can be reached. Among them, the targets R, E′, and Q′ are respectively the farthest points that can be reached along the corresponding paths within the current scenario. Therefore, there are three candidate paths C′CR, C′CE′, and C′CQ′ from the starting point C′ to these three targets. Here, the candidate path does not necessarily extend in the direction of the destination E, because the road may also turn back diagonally backward for a certain distance and then turn into a forward-extending section, as Figure 1 shown by the midpoint line path. Then, the total path costs of these three candidate paths are calculated respectively. Suppose there are no other vehicle congestions on these three candidate paths C′CR, C′CE′, and C′CQ′, but there are the fewest obstacles on the candidate path C′CE′. Therefore, the candidate path C′CE′ with the lowest total path cost in the current scenario is selected as the planned path.

[0032] At the new starting point E′, there is only one candidate path E′E available for selection, and its total path cost must be the smallest in the current scenario with E′ as the starting point. Therefore, the vehicle selects this candidate path E′E as the planned path and reaches the destination E.

[0033] Thus, by means of the best paths SC′, C′CE′, and E′E (i.e., road segments SC and CE) selected each time in the current scenario, the driving path SCE from the starting point S to the destination E is planned.

[0034] Suppose that the subsequent second vehicle and third vehicle also choose the path SCE. Thus, when the first vehicle wants to return from the destination E to the starting point S, it may detect that there are other vehicles (i.e., the second vehicle and the third vehicle) coming on the EC path, and the total path cost on this path increases due to traffic congestion. Therefore, the first vehicle may choose the path EQ instead of returning along the original path EC.

[0035] When the vehicle travels between the starting point S and the destination E multiple times, it will gradually "learn" the connectivity of all road segments in the road network, the road surface conditions, and the possible congestion conditions on each road segment. For example, in another possible implementation, if the vehicle chooses the path C′CR in the scenario with C′ as the starting point, it will find that it returns to the starting point S along the path CAS. Thus, the vehicle will "learn" and "remember" this point and exclude the candidate path C′CR when passing through point C next time. Therefore, with the continuous learning of the deep learning network of the present invention, when the vehicle is at the starting point S, it can pre-plan the overall path from the starting point S to the destination E, determine the optimal path from the starting point S to the destination E, and ensure that its total path cost is minimized. To this end, given the path cost from the starting point to the farthest point I(x, y) reached in the current scenario and the path cost from this farthest point to the destination, the total path cost is calculated using the following formula: Fn = Qn + Hn, where Fn is the total path cost, Qn is the total path cost from the starting point S to the farthest point I(x, y) in the current scenario, and Hn is the path cost from the farthest point I(x, y) in the current scenario to the destination E. Qn and Hn are also calculated according to the previous formula (1). When the total path cost Fn reaches the lowest, the path can be optimized.

[0036] When driving along the pre-planned optimal path, if an unexpected situation is detected on the way, local path planning can also be performed in real time to bypass the road segments or scenarios where the unexpected situation occurs. Therefore, the method of the present invention is essentially a local + overall path planning method.

[0037] The above method of the present invention, especially steps S202 to S205, is implemented by a deep learning network. The three-dimensional environmental data obtained in step S201 is input into the deep learning network. The deep learning network introduces the microbial "tree-shaped group movement" algorithm, uses the calculated path cost as a hyperparameter for judging the optimal path trajectory, and obtains the optimal planned path based on the three-dimensional environmental data. Corresponding to the tree-shaped group movement algorithm described above, the specific steps are as follows:

[0038] 1. Initialize relevant parameters, such as the number of vehicles \(m\), the importance factor \(\alpha\) of pheromone, the importance factor \(\beta\) of heuristic function, the pheromone evaporation factor \(\rho\), the pheromone constant \(Q\), the maximum number of iterations \(max\), etc.;

[0039] 2. Construct the solution space, randomly place each vehicle at different starting points, determine the current candidate road set for each path, and input all data into the tree - shaped group - line deep learning network;

[0040] 3. Given the total distance from the starting point to the current analysis pixel \(I(x,y)\) (the farthest pixel point reached by the current 3D reconstruction) and the distance from the analysis pixel to the target, calculate the total path cost \(F_n\). When the cost reaches the minimum, the path can be optimized;

[0041] 4. Update the pheromone, calculate the path length passed by each vehicle, record the optimal solution (the shortest path) in the current iteration. At the same time, update the pheromone on the paths connecting each node;

[0042] 5. Judge whether to terminate. If the maximum number of iterations is not reached, clear the record table of the paths passed by the vehicles and return to step 2. Otherwise, terminate the calculation and output the optimal solution.

[0043] Thus, input the 3D environmental data or the 3D environmental model detected by the sensor or obtained by 3D reconstruction into the deep learning network. This deep learning network can identify the paths around the vehicle and select the optimal path from them as the planned path, and the whole process is automatically realized in real - time. The deep learning network can be implemented as a convolutional neural network, including an input layer, a convolutional layer, and a pooling layer. Among them, each cost function is introduced into the convolutional layer as a hyper - parameter through an activation function.

[0044] Therefore, the deep learning network is a highly adaptive bionic mathematical model, which can automatically plan complex trajectories and extract trajectories from them to be used as training samples for parameter adjustment of the deep learning network, and train and learn the intelligent bionic deep network model in the above - mentioned way. At the beginning, this deep learning network may only be able to calculate the total path cost according to the current surrounding environment of the vehicle. However, when the vehicle drives in the same road network multiple times, and even traverses all the nodes and sections of the road network, this deep learning network learns and remembers the roads and traffic conditions of each section, so as to accurately plan the optimal path from the current position to the destination that the sensor cannot currently detect. Due to the high adaptability of the deep learning network, it can achieve a smooth trajectory for the vehicle when changing the starting and target positions and output the optimal path in real - time.

[0045] The path planning method of the present invention is based on three-dimensional environmental data. The more accurate the three-dimensional environmental data is, the better the planned path will be. As mentioned before, the three-dimensional environmental data can be detected by a stereo camera or lidar, or can be obtained through data fusion and three-dimensional reconstruction based on two-dimensional image data detected by multiple monocular cameras. Currently, due to the advantages of simple structure and high cost performance of monocular cameras, two monocular cameras are widely used to detect two-dimensional image data of the vehicle environment and perform data fusion on the sensor data to three-dimensionally reconstruct the environmental model based on the fused data. This method of obtaining three-dimensional environmental data generally includes steps such as calibration, data preprocessing, data fusion, feature extraction, inter-frame pose estimation, map registration, three-dimensional reconstruction, etc., as Figure 3 shown.

[0046] Before detection, the two monocular cameras need to be calibrated first to make their external parameters and internal parameters consistent. Calibration is carried out by photographing a calibration board with a known geometric structure, which is used to determine the internal and external parameters of the camera, so as to achieve accurate image measurement and three-dimensional reconstruction. The external parameters may include, for example: a rotation matrix, which describes the rotation relationship between the camera coordinate system and the world coordinate system; a translation vector, which describes the translation relationship between the camera coordinate system and the world coordinate system; an image offset, that is, the offset of the image plane relative to the camera coordinate system, etc. The internal parameters may include: the focal length of the camera lens, the distortion coefficients of the camera lens (such as radial distortion and tangential distortion) or distortion models (such as Brown model or Fish-eye model), the optical center point (i.e., the principal point) on the camera imaging plane, the pixel size, etc.

[0047] Then, data preprocessing is performed on the two-dimensional image data detected by the monocular cameras. At this stage, mainly operations such as denoising, distortion removal, and color correction are performed on the input image data to improve the accuracy and stability of subsequent processing. These operations can eliminate noise and distortion in the image, making subsequent feature extraction and matching more reliable.

[0048] After data preprocessing, the image data from the two monocular cameras are fused to obtain data containing depth information, that is, fused data. Here, data from different sensors or multiple perspectives are integrated to obtain more complete and accurate three-dimensional information. This can be achieved by performing registration, fusion, and smoothing processing on multiple images.

[0049] In addition, feature extraction is also performed on the preprocessed data, that is, feature points or feature descriptors with significant information and distinctiveness are extracted from the image data. These features can be used for key steps in the matching, tracking, and reconstruction processes, such as establishing feature point correspondences or three-dimensional coordinates of feature points.

[0050] After feature extraction, inter-frame pose estimation is performed. Among them, by analyzing the motion relationship between adjacent images, the pose change of the camera is deduced. This can be achieved through motion recovery algorithms (such as visual odometry).

[0051] Then map registration is carried out. Map registration is to align and integrate the three-dimensional data of multiple local maps or perspectives to construct a globally consistent three-dimensional map. This can be achieved by matching the corresponding relationships of feature points, planes or voxels, so as to achieve the consistency and integrity of multiple perspectives.

[0052] Then, based on the map and the fused data, a three-dimensional environmental model of the vehicle's surrounding environment is reconstructed, that is, the position and size data of surrounding objects, especially obstacles on the road and the vehicle ahead are obtained. On the basis of completing the reconstruction of the three-dimensional environmental model, the driving path of the vehicle can be planned, for example, using the real-time deep learning algorithm based on "tree-shaped group driving" of the present invention. Obviously, the more accurate the reconstructed three-dimensional environmental model is, the better the planned path will be.

[0053] Therefore, in order to improve the accuracy of three-dimensional environmental reconstruction and achieve precise positioning, the present invention proposes an adaptive loop detection method based on image control points. During the SLAM (simultaneous localization and mapping) mapping process, visual odometry only considers the key frames at adjacent times, and the errors generated during this period will gradually accumulate to form cumulative errors. In this way, the long-term estimation results will be unreliable. Therefore, the loop detection method can correct the drift error by detecting possible map loops, so as to construct a globally consistent trajectory and map.

[0054] The adaptive loop detection method of the present invention is implemented by means of image control points. Herein, an RBF (Radial Basis Function) classifier for identifying control points from images is trained first. For this purpose, a plurality of control points are preset in the vehicle surrounding environment, and the true three-dimensional geographical coordinates (Ground Truth) of each control point are obtained. Herein, each control point is a point in the world coordinate system, that is, a real point in life. The setting of the control points can be carried out, for example, in the following way: manually use a positioning device to punch points at the selected positions, and the punched positions are the control points, and the absolute geographical coordinates at the punched positions are obtained. The positioning device can be, for example, a known positioning device such as a GPS positioning system or a Beidou positioning system. Then, the punched positions are found on multiple images taken by the monocular camera on the vehicle from various directions and distances, and the images are manually labeled. The true three-dimensional geographical coordinates of the control points and these labeled images are input into the RBF classifier to train the RBF classifier, associating the true geographical coordinates of the punched positions with their image features on the images, and realizing the three-dimensional-two-dimensional mapping relationship under multiple images.

[0055] Herein, different from the usual RBF classifier, the RBF classifier of the present invention is a radial basis function (RBF) neural network based on Fourier transform, which uses Fourier transform to normalize data. The RBF neural network is a single-hidden-layer feedforward neural network, which includes an input layer, a hidden layer, and an output layer, and the hidden layer includes a plurality of radial basis nerve operators. Different from the Gaussian kernel function in the prior art, the present invention formulates a new nerve operator by directly parameterizing the integral kernel in the Fourier space, which extracts the normalized Fourier coefficients of the image as feature vectors and learns the target image features. Therefore, Fourier transform is performed on each image point of an image, and the color values 0-255 of the image points are normalized into Fourier coefficients between 0 and 1. Among them, the Fourier coefficient of the image point corresponding to the control point is 1, and the farther away from the control point, the smaller the Fourier coefficient. Therefore, each image can be represented by a feature vector composed of the Fourier coefficients of each image point. Using this feature vector as the input data of the RBF neural network can learn the image features of the control points. Through Fourier transform, the image features can be better learned, so that the RBF classifier is more excellent.

[0056] After training the RBF classifier, the RBF classifier can be used to identify control points from the images captured by a monocular camera, and then the true 3D geographical coordinates of the image points corresponding to the control points on the images can be known. Comparing this true geographical coordinate with the model geographical coordinates of the 3D environmental model reconstructed from the corresponding image points in 3D, and adjusting the model geographical coordinates of the control points in the 3D environmental model towards the true geographical coordinates of the control points, the environmental model obtained by 3D reconstruction can be corrected.

[0057] When making corrections, for example, the control point P in the world coordinate system Ow-XwYwZw can be projected onto the image captured by the camera, that is, projected onto the image coordinate system o-xy. Thus, the image coordinates (x, y) of the projection point p of the control point P on the image can be calculated from the true geographical coordinates (Xw, Yw, Zw) of the control point P, and this image coordinate is the true image coordinate. Here, the coordinate transformation from the world coordinate system Ow-XwYwZw to the image coordinate system o-xy is carried out with the help of the camera coordinate system Oc-XcYcZc. First, the world coordinate system can be converted into the camera coordinate system through rotation and translation (as Figure 4 shown):

[0058]

[0059] where R is the rotation matrix and T is the translation vector.

[0060] And the camera coordinates (Xc, Yc, Zc) and the image coordinates (x, y) have the following proportional relationship (see Figure 5 ):

[0061]

[0062] where f is the focal length of the camera lens.

[0063] Therefore, through the above coordinate transformation formulas among the world coordinates, camera coordinates, and image coordinates, using the internal and external parameters of the camera, the true image coordinates (x, y) of the projection point p of the control point P can be obtained from the true geographical coordinates (Xw, Yw, Zw) of the control point P. Subtracting the true image coordinate from the image coordinate of the image point representing the control point P on the captured image to obtain the projection error, and using the ceres optimization library to optimize this projection error to make it as close to zero as possible, the precise adjustment of the model position can be achieved.

[0064] In an exemplary embodiment of the present invention, there is also provided a computer-readable storage medium, on which a computer program is stored. The program includes executable instructions, and when the executable instructions are executed by, for example, a processor, the steps of the path planning method and the model correction method described in the above embodiments can be implemented. In some possible implementation manners, various aspects of the present invention can also be implemented in the form of a computer program product, which includes program code. When the computer program product runs on a terminal device, the program code is used to cause the terminal device to execute the steps according to various exemplary embodiments of the present invention described in the path planning method and the model correction method in this specification.

Claims

1. A path planning method, which is used to automatically plan a driving path for a vehicle from a starting point to a destination, characterized in that, The path planning method includes the following steps: A data acquisition step, in which three-dimensional environmental data around the vehicle in the currently observed scene of the vehicle is acquired. The three-dimensional environmental data describes the roads and traffic conditions in the current environment of the vehicle. The three-dimensional environmental data includes information related to obstacles on the road and information related to the vehicle ahead, especially congestion information. A candidate path determination step, in which the position where the vehicle is located when observing the current scene is used as the starting point of the current scene. At least one target reachable from the starting point is determined in the current scene according to the three-dimensional environmental data, and one or more candidate paths from the starting point to the at least one target are determined. A total path cost calculation step, in which the total path cost of each candidate path is calculated based on the environmental data, especially based on the information related to obstacles and congestion information. A planned path selection step, in which the candidate path with the lowest total path cost is selected as the planned path within the current scene. Among them, the candidate path determination step, the total path cost calculation step, and the planned path selection step are implemented in a deep learning network introducing a microbial tree swarm algorithm.

2. The path planning method according to claim 1, wherein After driving along the planned path to the corresponding target, the corresponding target is used as a new starting point, the scene observed by the vehicle at the new starting point is used as the new current scene, new three-dimensional environmental data is acquired, and a planned path within the new current scene is determined based on the new environmental data, and so on in a loop until the destination is reached.

3. The path planning method according to claim 1 or 2, characterized in that, The total path cost is calculated according to the following formula: Fn = γ1Gn1 + γ2Gn2 + … + γ m Gn m + … = ∑ i γ i Gn i , Among them, Fn is the total path cost of each candidate path, and Gn i (i = 1, 2, …, m, …) is the cost function for various factors affecting the total path cost, and γ i (i = 1, 2, …, m, …) is the weight coefficient for the corresponding factors. In particular, γ i (i = 1, 2, …, m, …) can respectively take values in the interval [0, 1].

4. The path planning method according to claim 3, characterized in that The factors affecting the total path cost include whether there are obstacles to be avoided on the road and their quantity, whether there are other vehicles ahead on the road and whether it is congested, whether there are turns on the road, the number of turns and the curvature radius of the curve, whether there are uphill and downhill sections on the road and the size of the slope, and whether the road surface is slippery or muddy and difficult to travel. and / or Weight coefficient γ i (The values of (i = 1, 2,..., m,...) are determined by environmental data. For example, for a path with fewer obstacles, less congestion, no sharp turns, and a non-muddy road surface, the weight coefficients of the corresponding factors are small.) 5. The path planning method according to any one of claims 1 to 4, characterized in that The information related to obstacles includes the quantity, position, and size of the obstacles to be avoided on the road; and / or the information related to the vehicle ahead includes the quantity and position of the vehicle ahead.

6. The path planning method according to any one of claims 1 to 5, characterized in that, The point farthest from the starting point on each candidate path starting from the starting point within the current scene is determined as the target of the corresponding candidate path, and / or the position of the last vehicle ahead on each candidate path within the current scene is determined as the target of the corresponding candidate path.

7. The path planning method according to any one of claims 1 to 6, characterized in that, The three-dimensional environmental data is detected by a lidar.

8. The path planning method according to any one of claims 1 to 6, characterized in that, The three-dimensional environmental data is obtained by reconstructing a three-dimensional environmental model from the two-dimensional image data of multiple visual sensors, such as two monocular cameras.

9. The path planning method according to claim 8, wherein The reconstructed three-dimensional environmental model is corrected by image control points. In particular, control points are identified in the images detected by the visual sensors using an RBF classifier, and the three-dimensional environmental model is corrected by adjusting the model coordinates of the control points in the three-dimensional environmental model to the true geographical coordinates of the control points.

10. The path planning method according to claim 9, characterized in that, The RBF classifier is a radial basis function neural network based on the Fourier transform. The integral kernel of the radial basis function neural network is parameterized in the Fourier space, and / or the RBF classifier extracts the normalized Fourier coefficients of the image as feature vectors to learn the target image features.

11. The path planning method according to claim 9 or 10, characterized in that, Project the control points in the world coordinate system onto the images detected by the vision sensor. Calculate the true image coordinates of the projected points of the control points on the images through coordinate transformation using the true geographic coordinates of the control points. Subtract the true image coordinates from the image coordinates of the image points representing the control points on the images to obtain the projection error, and use the Ceres optimization library to optimize this projection error.

12. The path planning method according to any one of claims 9 to 11, characterized in that, The RBF classifier is trained in the following manner: preset control points in the vehicle's surrounding environment and obtain the true three-dimensional geographic coordinates of the control points. Specifically, manually use a positioning device to mark points at selected locations and obtain the absolute geographic coordinates of the marked locations; find the marked locations in multiple images and manually label the images; input the true three-dimensional geographic coordinates of the control points and the labeled images into the RBF classifier to train the RBF classifier.

13. A method for correcting a three-dimensional environment model reconstructed from two-dimensional images, the method comprising the following steps: Use the RBF classifier to identify control points in the images detected by the sensor; And Adjust the model coordinates of the control points in the three-dimensional environment model towards the true geographic coordinates of the control points to correct the three-dimensional environment model. It is characterized in that the RBF classifier is a radial basis function neural network based on Fourier transform, the integral kernel of the radial basis function neural network is parameterized in the Fourier space, and / or the RBF classifier extracts the normalized Fourier coefficients of the images as feature vectors to learn the target image features.

14. A computer-readable storage medium, on which a computer program is stored, the computer program comprising executable instructions, which when executed by a processor, implement the path planning method according to any one of claims 1 to 12 or implement the method for correcting a three-dimensional environment model reconstructed from two-dimensional images according to claim 13.

Images