CN116380108A - Track planning method and device based on laser radar - Google Patents
Track planning method and device based on laser radar Download PDFInfo
- Publication number
- CN116380108A CN116380108A CN202310644099.8A CN202310644099A CN116380108A CN 116380108 A CN116380108 A CN 116380108A CN 202310644099 A CN202310644099 A CN 202310644099A CN 116380108 A CN116380108 A CN 116380108A
- Authority
- CN
- China
- Prior art keywords
- matrix
- vehicle
- data
- passable
- information
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 47
- 239000011159 matrix material Substances 0.000 claims abstract description 97
- 238000013527 convolutional neural network Methods 0.000 claims abstract description 18
- 238000012545 processing Methods 0.000 claims abstract description 17
- 230000006870 function Effects 0.000 claims description 35
- 239000002689 soil Substances 0.000 claims description 14
- 210000005036 nerve Anatomy 0.000 claims description 9
- 238000011176 pooling Methods 0.000 claims description 7
- 230000001537 neural effect Effects 0.000 claims description 6
- 238000012549 training Methods 0.000 claims description 5
- 238000003825 pressing Methods 0.000 claims description 4
- 238000012216 screening Methods 0.000 claims description 3
- 239000004973 liquid crystal related substance Substances 0.000 claims 2
- 238000010586 diagram Methods 0.000 description 3
- 230000008569 process Effects 0.000 description 3
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000008878 coupling Effects 0.000 description 1
- 238000010168 coupling process Methods 0.000 description 1
- 238000005859 coupling reaction Methods 0.000 description 1
- 238000011478 gradient descent method Methods 0.000 description 1
- 239000000463 material Substances 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 238000007781 pre-processing Methods 0.000 description 1
- 230000000750 progressive effect Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/34—Route searching; Route guidance
- G01C21/3446—Details of route searching algorithms, e.g. Dijkstra, A*, arc-flags, using precalculated routes
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
- G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
- G01S7/4802—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00 using analysis of echo signal for target characterisation; Target signature; Target cross-section
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02A—TECHNOLOGIES FOR ADAPTATION TO CLIMATE CHANGE
- Y02A90/00—Technologies having an indirect contribution to adaptation to climate change
- Y02A90/10—Information and communication technologies [ICT] supporting adaptation to climate change, e.g. for weather forecasting or climate simulation
Abstract
The invention discloses a laser radar-based track planning method and device, belongs to the technical field of route planning navigation, and is used for solving the technical problems that under the existing off-road environment, the off-road map modeling and track planning capability of a vehicle are poor, an off-road optimal route cannot be planned well, and safe and stable navigation and passing of the vehicle are difficult to realize. The method comprises the following steps: performing matrix reconstruction of related data on the pre-acquired laser radar data to obtain a data reconstruction matrix; performing surface fitting processing on the cross-country road surface form according to the height matrix to determine the traffic domain information of the vehicle; carrying out data reconstruction on the echo intensity matrix relative to the one-dimensional convolutional neural network, and determining pavement type information; determining a reference speed based on the vehicle passable domain information and the road surface type information; and carrying out path searching and judging on the reference speed according to a preset heuristic function and a child node passing cost function to obtain a navigation planning path of the off-road vehicle.
Description
Technical Field
The present disclosure relates to the field of route planning navigation, and in particular, to a method and apparatus for planning a trajectory based on a laser radar.
Background
The planning control is used as the core of automatic driving, and the reasonable planning of the track and the stable motion control are the keys for realizing automatic driving. Under the off-road environment, the ground surface types are various, the ground attribute is complex, so that the running of the vehicle is more hindered, meanwhile, the terrain is more fluctuant, if the running path of the off-road vehicle is not planned in advance, the passing speed of the vehicle in a designated area can be seriously influenced, and if experience is summarized in the real vehicle passing exercise process, the consumption of manpower, material resources and financial resources is large, and the running safety of a driver and a test vehicle cannot be guaranteed.
The off-road vehicle running under the complex working condition of the existing off-road environment is influenced by multi-factor coupling such as ground elevation, gradient, slope direction, attachment rate and the like, terrain reconstruction is difficult to carry out, the existing automatic driving vehicle track planning strategy is more suitable for automatic driving vehicle track planning of a structured road surface, and an optimal route from a starting point to a finishing point is difficult to plan in the off-road environment, so that the vehicle can safely and stably pass through the off-road surface.
Disclosure of Invention
The embodiment of the application provides a track planning method and device based on a laser radar, which are used for solving the following technical problems: under the existing off-road environment, the off-road map modeling and track planning capability of the vehicle is poor, an off-road optimal route cannot be well planned, and safe and stable navigation and passing of the vehicle are difficult to realize.
The embodiment of the application adopts the following technical scheme:
in one aspect, an embodiment of the present application provides a track planning method based on a lidar, including: performing matrix reconstruction of related data on the pre-acquired laser radar data to obtain a data reconstruction matrix; wherein the data reconstruction matrix comprises: a height matrix and an echo intensity matrix; performing surface fitting processing on the cross-country road surface form according to the height matrix to determine the traffic domain information of the vehicle; reconstructing data of the echo intensity matrix on a one-dimensional convolutional neural network, and determining pavement type information conforming to the cross-country pavement form; determining a reference speed conforming to the passing of the off-road vehicle based on the passable domain information of the vehicle and the road surface type information; and carrying out path searching and judging on the reference speed according to a preset heuristic function and a child node passing cost function to obtain a navigation planning path of the off-road vehicle.
According to the method and the device for the vehicle speed planning, the cross-country vehicle can consider vehicle path planning and speed planning through the determined vehicle passable domain information and road surface type information, and the running speed of the vehicle is considered on the basis of the path planning. And then, combining the road surface type and the road surface elevation, acquiring road surface information according to a TOF (Time-of-Flight) laser radar, and realizing road surface type reconstruction according to a one-dimensional convolutional neural network. Based on the obtained reference speed, an A algorithm is utilized, and according to a certain heuristic rule and in combination with a cost function algorithm, the resolution is complete, an optimal path can be easily searched, and an optimal navigation planning path of the off-road vehicle is obtained.
In a possible implementation manner, the method for reconstructing the matrix of the related data of the pre-acquired laser radar data to obtain a data reconstruction matrix specifically includes: scanning and acquiring laser radar data according to a TOF (Time-of-Flight) laser radar pre-installed above an off-road vehicle; wherein the TOF (Time-of-Flight) lidar is a 3D TOF laser array radar; according to the scanning range threshold value of the TOF (Time-of-Flight) laser radar, carrying out data reconstruction on the relevant elevation raster image, echo intensity image and corresponding pixel array on the laser radar data to respectively obtain elevation raster data and echo intensity pixel array data; wherein the pixel array is an 8×8 pixel array; generating matrixes of the elevation raster data and the echo intensity pixel array data to obtain the elevation matrixes and the echo intensity matrixes respectively; wherein the data reconstruction matrix is composed of the height matrix and the echo intensity matrix.
In a possible implementation manner, according to the height matrix, performing surface fitting processing on the off-road pavement form to determine vehicle passable domain information, which specifically includes: performing surface fitting on the height matrix according to a least square method to obtain three-dimensional pavement information; wherein the three-dimensional road surface information includes: three-dimensional coordinate information and road elevation information; calculating the elevation value and the curved slope of the elevation points of the adjacent grids in the three-dimensional pavement information to determine the grid elevation difference of the adjacent grids; determining gradient values of adjacent grids based on the grid elevation difference; screening the value of the gradient value; and determining the grid corresponding to the screened gradient value as an elevation passable grid, and obtaining the vehicle passable domain information.
In a possible implementation manner, the data reconstruction of the echo intensity matrix on the one-dimensional convolutional neural network is performed to determine pavement type information conforming to the off-road pavement morphology, and the method specifically includes: normalizing the echo intensity matrix to obtain an echo matrix after data reconstruction; wherein the echo matrix after the data reconstruction is a matrix of 1×64; and according to the one-dimensional convolutional neural network, carrying out model prediction on the echo matrix after the data reconstruction on a time sequence to obtain the pavement type information.
In a possible implementation manner, according to a one-dimensional convolutional neural network, performing model prediction on a time sequence on the echo matrix after the data reconstruction to obtain the pavement type information, which specifically includes: performing arrangement processing on the echo matrix after the data reconstruction in a related time sequence format to obtain dimension data; wherein the dimension data includes: a time step dimension and a road surface type dimension; performing convolution iterative training of relevant parameters on the dimensional data according to a convolution layer, a pooling layer and a full-connection layer of the one-dimensional convolution nerve model to obtain a trained one-dimensional convolution nerve model; performing convolution processing on the echo matrix after data reconstruction according to the trained one-dimensional convolution nerve model, and traversing each data point after the convolution processing based on a convolution kernel to obtain a score matrix; model prediction is carried out on the new time sequence data in the scoring matrix through the trained one-dimensional convolutional neural model, so that pavement type information conforming to the cross-country pavement form is obtained; wherein the road surface type information at least includes: vegetation road surface information, non-vegetation soil information, highway information and wading road surface information.
In a possible implementation manner, the method for determining the reference speed according with the passing of the off-road vehicle based on the passable domain information of the vehicle and the road surface type information specifically includes: according to
Obtaining a reference speed v corresponding to the vehicle passable domain information, wherein +>
The rated power of the engine is determined by the model of the vehicle engine; beta is the terrain gradient and is determined by the grid elevation difference in the vehicle passable domain information; />
The value of (2) is equal to the corresponding ++for pressing a tire of width b vertically into the road surface type information>
Work done when soil is deep; w is the vertical load on the wheel.
In a possible embodiment, according to
Obtaining a vertical load W; wherein b tire width of off-road vehicle, < >>
The corresponding soil sinking depth in the pavement type information is set, and p is vertical pressure; according to
Obtaining the vertical pressure; wherein (1)>
、/>
And n is the Barker soil parameter and is all determined by the road surface type informationAnd (3) determining.
In a possible implementation manner, before the route searching and judging are performed on the reference speed according to a preset heuristic function and a sub-node passing cost function to obtain the navigation planning route of the off-road vehicle, the method further comprises: constructing a closed table (extended node table) conforming to the information of the non-passable domain of the vehicle according to the reference speed; the vehicle non-passable domain information is non-passable ground elevation grid region information, and the maximum speed of the wild vehicle in the vehicle non-passable domain information is 0; transmitting child nodes of a passable grid network into an open table (unexpanded node table) according to the closed table (expanded node table), and adding the passable grid network that the off-road vehicle has traveled into the closed table (expanded node table); wherein the child nodes of the passable grid network and the driven passable grid network are both determined by the vehicle passable domain information, and the closed table (expanded node table) and the open table (unexpanded node table) are search tables in an a-algorithm.
In a possible implementation manner, according to a preset heuristic function and a subnode passing cost function, performing path search and judgment on the reference speed to obtain a navigation planning path of the off-road vehicle, and specifically includes: recording and updating the open (unexpanded node table) table according to the heuristic function of the A-algorithm and the child node passing cost function to obtain a new open table (unexpanded node table); wherein the new open table (unexpanded node table) comprises child nodes and parent nodes of the passable grid network; wherein according to
Obtaining the child node passing cost function +.>
；/>
The parent node in the new open table (unexpanded node table) is passed a cost function,/>
is the maximum value of the reference speed in the passable grid network, +.>
Maximum speed achievable for a child node in the new open table (unexpanded node table); according to the new open table (unexpanded node table), searching and judging the grid network with the minimum total cost value by the child nodes of the passable grid network to obtain the optimal child node in the child nodes of the passable grid network; sequentially connecting the optimal child nodes, and reversely inquiring the parent node grids to obtain an optimal search path; and generating a navigation planning path of the off-road vehicle based on the optimal search path and the reference speed.
On the other hand, the embodiment of the application also provides a track planning device based on the laser radar, which comprises: at least one processor; and a memory communicatively coupled to the at least one processor; the memory stores instructions executable by the at least one processor to enable the at least one processor to perform a laser radar-based trajectory planning method according to any one of the embodiments described above.
The application provides a track planning method and equipment based on a laser radar, which enable an off-road vehicle to consider vehicle path planning and speed planning through determined vehicle passable domain information and road surface type information, and consider the running speed distribution of the vehicle on the basis of path planning. And then, combining the road surface type and the road surface elevation, acquiring road surface information according to a TOF (Time-of-Flight) laser radar, and realizing road surface type reconstruction according to a one-dimensional convolutional neural network. Based on the obtained reference speed, an A algorithm is utilized, and according to a certain heuristic rule and in combination with a cost function algorithm, the resolution is complete, an optimal path can be easily searched, and an optimal navigation planning path of the off-road vehicle is obtained.
Drawings
In order to more clearly illustrate the embodiments of the present application or the technical solutions in the prior art, the drawings that are required to be used in the embodiments or the description of the prior art will be briefly described below, and it is obvious that the drawings in the following description are only some embodiments described in the present application, and other drawings may be obtained according to these drawings without inventive effort for a person skilled in the art. In the drawings:
fig. 1 is a flowchart of a track planning method based on a laser radar according to an embodiment of the present application;
fig. 2 is a schematic diagram of a track navigation planning procedure provided in an embodiment of the present application;
fig. 3 is a schematic structural diagram of a track planning device based on a lidar according to an embodiment of the present application.
Detailed Description
In order to better understand the technical solutions in the present application, the following description will clearly and completely describe the technical solutions in the embodiments of the present application with reference to the drawings in the embodiments of the present application, and it is obvious that the described embodiments are only some embodiments of the present application, not all embodiments. All other embodiments, which can be made by one of ordinary skill in the art based on the embodiments herein without making any inventive effort, shall fall within the scope of the present application.
The embodiment of the application provides a track planning method based on a laser radar, as shown in fig. 1, the track planning method based on the laser radar specifically comprises steps S101-S105:
s101, performing matrix reconstruction of related data on the pre-acquired laser radar data to obtain a data reconstruction matrix. Wherein the data reconstruction matrix comprises: a height matrix and an echo intensity matrix.
Specifically, laser radar data is scanned and acquired according to a TOF (Time-of-Flight) laser radar pre-installed above an off-road vehicle. Wherein the TOF (Time-of-Flight) lidar is a 3D TOF laser array radar. And carrying out data reconstruction on the elevation raster image, the echo intensity image and the corresponding pixel array according to the scanning range threshold value of the TOF (Time-of-Flight) laser radar to obtain elevation raster data and echo intensity pixel array data respectively. Wherein the pixel array is an 8×8 pixel array.
Further, generating a matrix of elevation raster data and echo intensity pixel array data to obtain an elevation matrix and an echo intensity matrix respectively. The data reconstruction matrix consists of a height matrix and an echo intensity matrix.
In an embodiment, fig. 2 is a schematic diagram of a track navigation planning flow provided in the embodiment of the present application, as shown in fig. 2, a TOF (Time-of-Flight) lidar device is installed on the upper portion of an off-road vehicle, and connected to a computer, so as to ensure that the device can work normally. The TOF (Time-of-Flight) laser radar is a 3D TOF laser area array radar, and can scan and obtain an elevation grid graph, an echo intensity graph and a corresponding 8×8 pixel array within a range of 4.5m distance and a view angle of 30 degrees in front of a vehicle body. And respectively constructing a height matrix and an echo intensity matrix according to the elevation raster data corresponding to the elevation raster image and the echo intensity pixel array data corresponding to the echo intensity pixel array, wherein the height matrix and the echo intensity matrix are 8 rows and 8 columns.
S102, performing surface fitting processing on the cross-country road surface form according to the height matrix, and determining the traffic domain information of the vehicle.
Specifically, according to a least square method, performing surface fitting on the height matrix to obtain three-dimensional pavement information. Wherein the three-dimensional road surface information includes: three-dimensional coordinate information and road elevation information. And calculating the elevation value and the curved slope of the elevation points of the adjacent grids in the three-dimensional pavement information to determine the grid elevation difference of the adjacent grids.
Further, determining gradient values of adjacent grids based on the grid elevation difference; and (5) screening the values of the gradient. And determining the grid corresponding to the screened gradient value as an elevation passable grid, and obtaining the vehicle passable domain information.
In one embodiment, as shown in fig. 2, a height matrix is fitted to a Matlab to obtain three-dimensional pavement information containing coordinates and elevation information according to a least square method, then the elevation difference of adjacent grids is determined according to the elevation value of adjacent Gao Chengdian in the three-dimensional pavement information and the slope of the curved surface, namely, the slope value is determined according to the elevation difference of the adjacent grids, and then a grid with the gradient value of the adjacent grids smaller than 30 degrees is selected as an elevation passable grid to obtain vehicle passable domain information.
And S103, reconstructing data of the echo intensity matrix on the one-dimensional convolutional neural network, and determining pavement type information conforming to the cross-country pavement form.
Specifically, the echo intensity matrix is normalized to obtain the echo matrix after data reconstruction. Wherein, the echo matrix after data reconstruction is a matrix of 1×64. And performing arrangement processing on the echo matrix after data reconstruction in a related time sequence format to obtain dimension data. Wherein the dimension data includes: a time step dimension and a road surface type dimension.
Further, according to the convolution layer, the pooling layer and the full-connection layer of the one-dimensional convolution nerve model, carrying out convolution iterative training of relevant parameters on the dimensional data to obtain the trained one-dimensional convolution nerve model.
Further, according to the trained one-dimensional convolution nerve model, carrying out convolution processing on the echo matrix after data reconstruction, and traversing each data point after the convolution processing based on a convolution kernel to obtain a score matrix.
Further, model prediction is carried out on the new time sequence data in the score matrix through the trained one-dimensional convolutional neural model, so that road surface type information conforming to the cross-country road surface morphology is obtained. The road surface type information at least comprises: vegetation road surface information, non-vegetation soil information, highway information and wading road surface information.
In one embodiment, the echo intensity matrix after data reconstruction is a matrix of 1×64, and then the echo intensity matrix after data reconstruction is processed by applying a one-dimensional Convolutional Neural Network (CNN), which will obtain different road surface type information, including:
1) Carrying out data preprocessing on an echo intensity matrix obtained by reconstructing data obtained in vehicle running: firstly, arranging an echo intensity matrix after data reconstruction in a time sequence format to obtain dimension data, wherein one dimension represents a time step and the other dimension represents a road surface type characteristic. The echo intensity matrix time step after data reconstruction is 1, and the road surface type characteristic data dimension (road surface type dimension) is 64.
2) Constructing a one-dimensional CNN model (one-dimensional convolutional neural model): the one-dimensional CNN model consists of a convolution layer, a pooling layer and a full connection layer. The convolution layer is used for extracting features of the time series data, the pooling layer is used for reducing the dimension of the features, and the full connection layer gathers the features learned by the convolution layer for classification. Relevant parameters of the network: the convolution kernel is 3 in size, the moving step length is 1, the pooling domain is 2 in size, the moving step length is 2, the pooling method adopts a maximum value method, the learning rate is 0.09, the network training method adopts a batch gradient descent method, the batch size is 64, the maximum iteration number is 1000, the training sample size is 12, the input layer node number is 64, and the output layer node number is 4.
3) Using a trained one-dimensional convolutional neural model: the output layer is classified into four categories of vegetation road surface, non-vegetation soil, highway and wading road surface, so the number of output layer nodes is set to 4. Specifically, a Conv1d () function is applied to convolve each row of data to obtain a new scoring matrix. The specific process is to multiply each row of data points by one convolution kernel to obtain a number, wherein 3 convolution kernels are 3 numbers, so that 1 column of data of one point is changed into 3 columns. Each point is then traversed row by row, resulting in a new scoring matrix (64 x 3).
4) And carrying out model prediction on the new time sequence data in the score matrix by using the trained one-dimensional convolutional neural model to obtain different pavement type information.
S104, determining the reference speed which accords with the passing of the off-road vehicle based on the passable area information of the vehicle and the road surface type information.
In particular according to
Obtaining a reference speed v corresponding to the vehicle passable domain information, wherein +>
The rated power of the engine is determined by the model of the engine of the vehicle. Beta is the terrain grade and is determined by the grid elevation difference in the vehicle-navigable domain information. />
The value of (2) is equal to the corresponding ++of the vertical pressing of a tire of width b into the road type information>
Work done in deep soil. W is the vertical load on the wheel.
Wherein according to
The vertical load W on the wheel is obtained. Wherein b tire width of off-road vehicle, < >>
And p is vertical pressure, which is the corresponding soil sinking depth in the pavement type information. According to->
Vertical pressure is obtained. Wherein (1)>
、/>
And n is a Barker soil parameter and is determined by road surface type information.
In one implementation, the maximum achievable speed for a vehicle in the non-trafficable region of the road elevation raster pattern is 0. The reference speed of the vehicle in the passable domain information is related to the ground attribute and the terrain gradient under different road conditions, namely
Which is provided withIn (I)>
The rated power of the engine is determined by the model of the engine of the vehicle. Beta is the slope of the terrain, determined by the elevation difference. />
The value is equal to the vertical pressing of a tire of width b into the soil>
Work done at depth, i.e. f= |w|. W is the vertical load on the off-road vehicle wheels.
And S105, carrying out path search judgment on the reference speed according to a preset heuristic function and a sub-node traffic cost function to obtain a navigation planning path of the off-road vehicle.
Specifically, a closed table conforming to the information of the non-passable domain of the vehicle is constructed according to the reference speed. The vehicle non-passable domain information is non-passable ground elevation grid region information, and the maximum speed of the wild vehicle in the vehicle non-passable domain information is 0.
Further, according to the closed table (extended node table), the child nodes of the passable grid network are transferred into the open table (unexpanded node table), and the passable grid network in which the off-road vehicle has traveled is added into the closed table (expanded node table). The child nodes of the trafficable grid network and the trafficable grid network which are driven are determined by the vehicle trafficable domain information, and the closed table (expanded node table) and the open table (unexpanded node table) are search tables in an algorithm A.
Further, according to the heuristic function of the A-algorithm and the child node passing cost function, the open table (unexpanded node table) is recorded and updated, and a new open table (unexpanded node table) is obtained. Wherein the new open table (unexpanded node table) includes child nodes and parent nodes of the passable grid network.
Wherein according to
Obtaining the seedNode passing cost function->
。/>
Pass cost function for parent node in new open table (unexpanded node table), +.>
Maximum value of reference speed in the passable grid, +.>
Maximum speed achievable for child nodes in the new open table (unexpanded node table).
Further, according to the new open table (unexpanded node table), searching and judging the grid network with the minimum total cost value is carried out on the child nodes of the passable grid network, and the optimal child node in the child nodes of the passable grid network is obtained. And connecting the optimal child nodes in sequence, and reversely inquiring the parent node grid to obtain an optimal search path. And generating a navigation planning path of the off-road vehicle based on the optimal search path and the reference speed.
In one embodiment, using a heuristic function of an a-algorithm and a child node traffic cost function, performing a path search decision on a reference speed includes: step one: depending on the reference speed, a closed table (extended node table) is built that cannot be initialized by the ground grid. Step two: referring to the closed table (expanded node table), the child nodes of the passable grid are added to the open table (unexpanded node table), and the passed grid is added to the closed table (expanded node table), i.e., a new open table (unexpanded node table) is generated, and then no more searching is performed. And thirdly, recording and updating a new open table (unexpanded node table) according to a heuristic function and an optimization cost function of the A-algorithm. Step four: taking the grid with the minimum total cost value in all the child nodes of the passable grid, judging whether the grid reaches a destination, if so, ending the search, and entering a step five; otherwise, continuing to carry out the second step and the third step until the search judgment is finished. Step five: and sequentially connecting the optimal child nodes, reversely searching the parent node grid to obtain an optimal search path, and finally generating an optimal navigation planning path of the off-road vehicle based on the optimal search path and the reference speed.
In addition, the embodiment of the application further provides a track planning device based on the laser radar, as shown in fig. 3, the track planning device 300 based on the laser radar specifically includes:
at least one processor 301. And a memory 302 communicatively coupled to the at least one processor 301. Wherein the memory 302 stores instructions executable by the at least one processor 301 to enable the at least one processor 301 to perform:
performing matrix reconstruction of related data on the pre-acquired laser radar data to obtain a data reconstruction matrix; wherein the data reconstruction matrix comprises: a height matrix and an echo intensity matrix;
performing surface fitting processing on the cross-country road surface form according to the height matrix to determine the traffic domain information of the vehicle;
carrying out data reconstruction on the echo intensity matrix relative to the one-dimensional convolutional neural network, and determining pavement type information conforming to the cross-country pavement form;
determining a reference speed conforming to the passing of the off-road vehicle based on the traffic domain information and the road surface type information;
and carrying out path searching and judging on the reference speed according to a preset heuristic function and a child node passing cost function to obtain a navigation planning path of the off-road vehicle.
According to the method and the device, the cross-country vehicle can take vehicle path planning and speed planning into consideration through the determined vehicle passable domain information and road surface type information, and the running speed distribution of the vehicle is considered on the basis of the path planning.
And then, combining the road surface type and the road surface elevation, acquiring road surface information according to a TOF (Time-of-Flight) laser radar, and realizing road surface type reconstruction according to a one-dimensional convolutional neural network. Based on the obtained reference speed, an A algorithm is utilized, and according to a certain heuristic rule and in combination with a cost function algorithm, the resolution is complete, an optimal path can be easily searched, and an optimal navigation planning path of the off-road vehicle is obtained.
All embodiments in the application are described in a progressive manner, and identical and similar parts of all embodiments are mutually referred, so that each embodiment mainly describes differences from other embodiments. In particular, for the apparatus embodiments, since they are substantially similar to the method embodiments, the description is relatively simple, and reference is made to the description of the method embodiments in part.
The foregoing describes specific embodiments of the present application. In some cases, the acts or steps recited in the specification may be performed in a different order than in the embodiments and still achieve desirable results. In addition, the processes depicted in the accompanying figures do not necessarily require the particular order shown, or sequential order, to achieve desirable results. In some embodiments, multitasking and parallel processing are also possible or may be advantageous.
The foregoing is merely exemplary of the present application and is not intended to limit the present application. Various modifications and variations of the embodiments of the present application will be apparent to those skilled in the art. Any modifications, equivalent substitutions, improvements, etc. which are within the spirit and principles of the embodiments of the present application are intended to be included within the scope of the present application.
Claims (10)
1. A method for laser radar-based trajectory planning, the method comprising:
performing matrix reconstruction of related data on the pre-acquired laser radar data to obtain a data reconstruction matrix; wherein the data reconstruction matrix comprises: a height matrix and an echo intensity matrix;
performing surface fitting processing on the cross-country road surface form according to the height matrix to determine the traffic domain information of the vehicle;
reconstructing data of the echo intensity matrix on a one-dimensional convolutional neural network, and determining pavement type information conforming to the cross-country pavement form;
determining a reference speed conforming to the passing of the off-road vehicle based on the passable domain information of the vehicle and the road surface type information;
and carrying out path searching and judging on the reference speed according to a preset heuristic function and a child node passing cost function to obtain a navigation planning path of the off-road vehicle.
2. The track planning method based on laser radar according to claim 1, wherein the method comprises the steps of performing matrix reconstruction of related data on pre-acquired laser radar data to obtain a data reconstruction matrix, and specifically comprises the following steps:
scanning and acquiring laser radar data according to a TOF laser radar preinstalled above an off-road vehicle; the TOF laser radar is a 3D TOF laser array radar;
according to the scanning range threshold value of the TOF laser radar, carrying out data reconstruction on the laser radar data on an elevation raster image, an echo intensity image and a corresponding pixel array to respectively obtain elevation raster data and echo intensity pixel array data; wherein the pixel array is an 8×8 pixel array;
generating matrixes of the elevation raster data and the echo intensity pixel array data to obtain the elevation matrixes and the echo intensity matrixes respectively; wherein the data reconstruction matrix is composed of the height matrix and the echo intensity matrix.
3. The laser radar-based trajectory planning method according to claim 1, wherein the method is characterized in that the off-road pavement morphology is subjected to surface fitting processing according to the height matrix, and vehicle passable domain information is determined, and specifically comprises the following steps:
performing surface fitting on the height matrix according to a least square method to obtain three-dimensional pavement information; wherein the three-dimensional road surface information includes: three-dimensional coordinate information and road elevation information;
calculating the elevation value and the curved slope of the elevation points of the adjacent grids in the three-dimensional pavement information to determine the grid elevation difference of the adjacent grids;
determining gradient values of adjacent grids based on the grid elevation difference;
screening the value of the gradient value; and determining the grid corresponding to the screened gradient value as an elevation passable grid, and obtaining the vehicle passable domain information.
4. The track planning method based on the laser radar according to claim 1, wherein the data reconstruction of the echo intensity matrix on the one-dimensional convolutional neural network is performed to determine road type information conforming to the off-road surface morphology, and specifically comprises:
normalizing the echo intensity matrix to obtain an echo matrix after data reconstruction; wherein the echo matrix after the data reconstruction is a matrix of 1×64;
and according to the one-dimensional convolutional neural network, carrying out model prediction on the echo matrix after the data reconstruction on a time sequence to obtain the pavement type information.
5. The track planning method based on the laser radar according to claim 4, wherein the model prediction of the time series is performed on the echo matrix after the data reconstruction according to a one-dimensional convolutional neural network, so as to obtain the road surface type information, and specifically comprises the following steps:
performing arrangement processing on the echo matrix after the data reconstruction in a related time sequence format to obtain dimension data; wherein the dimension data includes: a time step dimension and a road surface type dimension;
performing convolution iterative training of relevant parameters on the dimensional data according to a convolution layer, a pooling layer and a full-connection layer of the one-dimensional convolution nerve model to obtain a trained one-dimensional convolution nerve model;
performing convolution processing on the echo matrix after data reconstruction according to the trained one-dimensional convolution nerve model, and traversing each data point after the convolution processing based on a convolution kernel to obtain a score matrix;
model prediction is carried out on the new time sequence data in the scoring matrix through the trained one-dimensional convolutional neural model, so that pavement type information conforming to the cross-country pavement form is obtained; wherein the road surface type information at least includes: vegetation road surface information, non-vegetation soil information, highway information and wading road surface information.
6. The laser radar-based trajectory planning method according to claim 1, wherein determining a reference speed that corresponds to an off-road vehicle passing based on the vehicle passable domain information and road surface type information, specifically comprises:
according to
Obtaining a reference speed v corresponding to the vehicle passable domain information, wherein +>
The rated power of the engine is determined by the model of the vehicle engine; beta is the terrain gradient and is determined by the grid elevation difference in the vehicle passable domain information; />
The value of (2) is equal to the corresponding ++for pressing a tire of width b vertically into the road surface type information>
Work done when soil is deep; w is the vertical load on the wheel.
7. A method for lidar-based trajectory planning as claimed in claim 6, wherein,
according to
Obtaining a vertical load W; wherein b tire width of off-road vehicle, < >>
The corresponding soil sinking depth in the pavement type information is set, and p is vertical pressure;
according to
Obtaining the vertical pressure; wherein (1)>
、/>
And n is a Barker soil parameter and is determined by the road surface type information.
8. The laser radar-based trajectory planning method of claim 1, wherein before performing a path search decision on the reference speed according to a preset heuristic function and a sub-node traffic cost function to obtain a navigation planned path of the off-road vehicle, the method further comprises:
constructing a closed table conforming to the information of the non-passable domain of the vehicle according to the reference speed; the vehicle non-passable domain information is non-passable ground elevation grid region information, and the maximum speed of the wild vehicle in the vehicle non-passable domain information is 0;
transmitting the child nodes of the passable grid network to an open table according to the closed table, and adding the passable grid network which the off-road vehicle has driven to the closed table; the child nodes of the passable grid network and the driven passable grid network are determined by the vehicle passable domain information, and the closed table and the open table are search tables in an algorithm A.
9. The laser radar-based trajectory planning method according to claim 8, wherein the path search determination is performed on the reference speed according to a preset heuristic function and a sub-node traffic cost function to obtain a navigation planning path of the off-road vehicle, and the method specifically comprises:
recording and updating the open table according to the heuristic function of the A-algorithm and the child node passing cost function to obtain a new open table; the new open table comprises child nodes and father nodes of the passable grid network;
wherein according to
Obtaining the child node passing cost function +.>
；/>
Passing a cost function for a parent node in said new open table,>
is the maximum value of the reference speed in the passable grid network, +.>
Maximum speed achievable for a child node in the new open table;
according to the new open table, searching and judging the grid network with the minimum total cost value from the child nodes of the passable grid network to obtain the optimal child node in the child nodes of the passable grid network;
sequentially connecting the optimal child nodes, and reversely inquiring the parent node grids to obtain an optimal search path;
and generating a navigation planning path of the off-road vehicle based on the optimal search path and the reference speed.
10. A lidar-based trajectory planning device, the device comprising:
at least one processor; the method comprises the steps of,
a memory communicatively coupled to the at least one processor; wherein, the liquid crystal display device comprises a liquid crystal display device,
the memory stores instructions executable by the at least one processor to enable the at least one processor to perform a lidar-based trajectory planning method of any of claims 1-9.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310644099.8A CN116380108B (en) | 2023-06-02 | 2023-06-02 | Track planning method and device based on laser radar |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310644099.8A CN116380108B (en) | 2023-06-02 | 2023-06-02 | Track planning method and device based on laser radar |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN116380108A true CN116380108A (en) | 2023-07-04 |
| CN116380108B CN116380108B (en) | 2023-08-11 |
Family
ID=86966020
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310644099.8A Active CN116380108B (en) | 2023-06-02 | 2023-06-02 | Track planning method and device based on laser radar |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116380108B (en) |
Citations (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20030057320A1 (en) * | 2001-02-28 | 2003-03-27 | Schneider Arthur J. | Precision-guided hypersonic projectile weapon system |
| CN106054208A (en) * | 2016-08-16 | 2016-10-26 | 长春理工大学 | Multiline laser radar vehicle object recognition method and vehicle anti-collision device |
| CN107632295A (en) * | 2017-09-15 | 2018-01-26 | 广东工业大学 | A kind of Radar Echo Extrapolation method based on sequential convolutional neural networks |
| CN109800773A (en) * | 2019-02-19 | 2019-05-24 | 北京大学 | Cross-country road extracting method based on three-dimensional laser radar |
| CN110244321A (en) * | 2019-04-22 | 2019-09-17 | 武汉理工大学 | A kind of road based on three-dimensional laser radar can traffic areas detection method |
| CN111315599A (en) * | 2017-09-14 | 2020-06-19 | 米其林集团总公司 | Method for evaluating hardness of ground |
| US20200298080A1 (en) * | 2019-03-19 | 2020-09-24 | NEX Team Inc. | Methods and systems for 3d ball trajectory reconstruction |
| US20200393566A1 (en) * | 2019-06-14 | 2020-12-17 | DeepMap Inc. | Segmenting ground points from non-ground points to assist with localization of autonomous vehicles |
| CN112622932A (en) * | 2020-12-23 | 2021-04-09 | 同济大学 | Automatic driving track-changing planning algorithm based on heuristic search of potential energy field |
| CN114580154A (en) * | 2022-02-11 | 2022-06-03 | 盐城工学院 | Calculation method for agricultural vehicle ground mechanics |
| US20220176962A1 (en) * | 2020-12-09 | 2022-06-09 | Waymo Llc | Physics-informed optimization for autonomous driving systems |
-
2023
- 2023-06-02 CN CN202310644099.8A patent/CN116380108B/en active Active
Patent Citations (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20030057320A1 (en) * | 2001-02-28 | 2003-03-27 | Schneider Arthur J. | Precision-guided hypersonic projectile weapon system |
| CN106054208A (en) * | 2016-08-16 | 2016-10-26 | 长春理工大学 | Multiline laser radar vehicle object recognition method and vehicle anti-collision device |
| CN111315599A (en) * | 2017-09-14 | 2020-06-19 | 米其林集团总公司 | Method for evaluating hardness of ground |
| CN107632295A (en) * | 2017-09-15 | 2018-01-26 | 广东工业大学 | A kind of Radar Echo Extrapolation method based on sequential convolutional neural networks |
| CN109800773A (en) * | 2019-02-19 | 2019-05-24 | 北京大学 | Cross-country road extracting method based on three-dimensional laser radar |
| US20200298080A1 (en) * | 2019-03-19 | 2020-09-24 | NEX Team Inc. | Methods and systems for 3d ball trajectory reconstruction |
| CN110244321A (en) * | 2019-04-22 | 2019-09-17 | 武汉理工大学 | A kind of road based on three-dimensional laser radar can traffic areas detection method |
| US20200393566A1 (en) * | 2019-06-14 | 2020-12-17 | DeepMap Inc. | Segmenting ground points from non-ground points to assist with localization of autonomous vehicles |
| US20220176962A1 (en) * | 2020-12-09 | 2022-06-09 | Waymo Llc | Physics-informed optimization for autonomous driving systems |
| CN112622932A (en) * | 2020-12-23 | 2021-04-09 | 同济大学 | Automatic driving track-changing planning algorithm based on heuristic search of potential energy field |
| CN114580154A (en) * | 2022-02-11 | 2022-06-03 | 盐城工学院 | Calculation method for agricultural vehicle ground mechanics |
Non-Patent Citations (4)
| Title |
|---|
| PHANG DARREN REN YEE; NATTAWAT PINRATH; NOBUTO MATSUHIRA: "Autonomous Mobile Robot Navigation Using 2D LiDAR and Inclined Laser Rangefinder to Avoid a Lower Object", 2020 59TH ANNUAL CONFERENCE OF THE SOCIETY OF INSTRUMENT AND CONTROL ENGINEERS OF JAPAN * |
| 吴天羿;许继恒;刘建永;廖伟;: "基于改进A~*算法的越野路径规划研究", 计算机应用研究, vol. 30, no. 06 * |
| 巩绪生;史美萍;李焱;贺汉根;: "越野环境建模与动态路径规划", 计算机应用, vol. 26, no. 12 * |
| 郭延宁，冯振，马广富，郭宇晴，张米令: "行星车视觉导航与自主控制进展与展望", 宇航学报, vol. 39, no. 11 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116380108B (en) | 2023-08-11 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN104392486B (en) | One kind point cloud scene reconstruction method | |
| CN112257597B (en) | Semantic segmentation method for point cloud data | |
| CN109584294B (en) | Pavement point cloud extraction method and device based on laser point cloud | |
| CN106056614A (en) | Building segmentation and contour line extraction method of ground laser point cloud data | |
| CN105512759A (en) | Urban CA model parameter optimization method based on biogeographic optimization algorithm | |
| CN103679655A (en) | LiDAR point cloud filter method based on gradient and area growth | |
| CN104573705A (en) | Clustering method for building laser scan point cloud data | |
| CN114706400B (en) | Path planning method based on improved A-x algorithm in off-road environment | |
| CN104318611B (en) | A kind of Three-dimension Tree method for reconstructing based on laser scanning point cloud | |
| CN106355643A (en) | Method for generating three-dimensional real scene road model of highway | |
| CN113879339A (en) | Decision planning method for automatic driving, electronic device and computer storage medium | |
| CN113343858B (en) | Road network geographic position identification method and device, electronic equipment and storage medium | |
| CN106599848A (en) | Depth visual feature and support vector machine-based terrain texture recognition algorithm | |
| CN115451983A (en) | Dynamic environment mapping and path planning method and device in complex scene | |
| Gruszczyński et al. | Application of convolutional neural networks for low vegetation filtering from data acquired by UAVs | |
| CN116202533A (en) | Vehicle track navigation planning method and device based on terrain reconstruction | |
| CN110726416A (en) | Reinforced learning path planning method based on obstacle area expansion strategy | |
| CN116380108B (en) | Track planning method and device based on laser radar | |
| CN113903173A (en) | Vehicle track feature extraction method based on directed graph structure and LSTM | |
| CN107578447A (en) | A kind of crop ridge location determining method and system based on unmanned plane image | |
| CN107564024A (en) | SAR image aggregation zone extracting method based on unilateral aggregation line segment | |
| Subhashini et al. | An innovative hybrid technique for road extraction from noisy satellite images | |
| CN111310919A (en) | Driving control strategy training method based on scene segmentation and local path planning | |
| Cheng et al. | Local perception-based intelligent building outline aggregation approach with back propagation neural network | |
| CN113378800A (en) | Automatic classification and vectorization method for road sign lines based on vehicle-mounted three-dimensional point cloud |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |