CN117351449B - Polar coordinate weighting-based road passable region boundary optimization method - Google Patents
Polar coordinate weighting-based road passable region boundary optimization method Download PDFInfo
- Publication number
- CN117351449B CN117351449B CN202311646355.3A CN202311646355A CN117351449B CN 117351449 B CN117351449 B CN 117351449B CN 202311646355 A CN202311646355 A CN 202311646355A CN 117351449 B CN117351449 B CN 117351449B
- Authority
- CN
- China
- Prior art keywords
- fitting
- point
- boundary
- angle
- polar
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 63
- 238000005457 optimization Methods 0.000 title claims abstract description 38
- 238000001914 filtration Methods 0.000 claims abstract description 18
- 238000013500 data storage Methods 0.000 claims abstract description 12
- 125000004122 cyclic group Chemical group 0.000 claims abstract description 10
- 230000006870 function Effects 0.000 claims description 42
- 238000004590 computer program Methods 0.000 claims description 4
- 238000012216 screening Methods 0.000 claims description 4
- 238000001514 detection method Methods 0.000 description 4
- 239000011159 matrix material Substances 0.000 description 4
- 238000007500 overflow downdraw method Methods 0.000 description 3
- 230000007613 environmental effect Effects 0.000 description 2
- 238000009499 grossing Methods 0.000 description 2
- 239000000463 material Substances 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 description 1
- 230000015556 catabolic process Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000006731 degradation reaction Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000009191 jumping Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 238000007781 pre-processing Methods 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 230000001360 synchronised effect Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V20/00—Scenes; Scene-specific elements
- G06V20/50—Context or environment of the image
- G06V20/56—Context or environment of the image exterior to a vehicle by using sensors mounted on the vehicle
- G06V20/588—Recognition of the road, e.g. of lane markings; Recognition of the vehicle driving pattern in relation to the road
-
- 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
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Abstract
The invention relates to a polar coordinate weighting-based road passable region boundary optimization method, which comprises the following steps: inputting a boundary point set of a passable area of a road; converting from Cartesian coordinates to polar coordinates and setting a data storage structure for each point; storing the radius of the boundary point obtained at each moment point into a structure body history polar coordinate container corresponding to the angle, and carrying out Kalman filtering prediction on the corresponding angle history polar coordinate point at each angle; performing polar coordinate polynomial cyclic fitting on the obtained polar coordinates; voting weighting treatment is carried out on the results subjected to the multiple fitting treatment, and a fitting polar coordinate weighting result is obtained; and setting a limiting condition of the road boundary points, and outputting the optimized point set. The invention also relates to a corresponding device, a processor and a storage medium thereof. By adopting the method, the device, the processor and the storage medium thereof, the reliability of boundary fitting is greatly improved while boundary noise points can be removed.
Description
Technical Field
The invention relates to the technical field of automatic driving, in particular to the technical field of road passable area detection, and specifically relates to a method and a device for optimizing a road passable area boundary based on polar coordinate weighting, a processor and a computer readable storage medium thereof.
Background
In the unmanned field, the perception module needs to identify the environmental elements. The road passable area detection can remove environmental elements such as dynamic and static obstacles and road boundaries, and provides a safe passable area for vehicles. The existing road passable area representation mode mainly comprises polar coordinates, and effective edge points of the passable road are provided in a 360-degree range of looking around. However, whether the vision-based or radar-based passable area detection method is greatly affected by noise of the perceived result, frequent boundary jumping phenomenon exists at the road edge, which makes the final passable area detection result not directly usable for the subsequent task.
Based on this, a technical solution is needed that can significantly optimize the boundary of the drivable area, and eliminate the boundary jump problem caused by noise while preserving the boundary details, thereby improving the automatic driving safety guarantee.
Disclosure of Invention
It is an object of the present invention to overcome the above-mentioned drawbacks of the prior art by providing a method, apparatus, processor and computer readable storage medium for optimizing the boundary of a road passable area based on polar coordinate weighting.
In order to achieve the above object, the method for optimizing the boundary of the passable road area based on polar coordinate weighting according to the present invention comprises the following steps:
the method for optimizing the boundary of the passable road area based on polar coordinate weighting is mainly characterized by comprising the following steps:
(1) Inputting a boundary point set of a passable area of a road;
(2) Converting the input points from Cartesian coordinates to polar coordinates, and setting a data storage structure body for each point;
(3) Storing the radius of the corresponding boundary point acquired by each moment point into a structure body history polar coordinate container corresponding to the angle, and carrying out Kalman filtering prediction processing on the corresponding angle history polar coordinate point on each angle;
(4) Performing polar coordinate polynomial cyclic fitting processing on each acquired polar coordinate, so as to update the angle range of each boundary point;
(5) Voting weighting treatment is carried out on the results subjected to the multiple fitting treatment, and a fitting polar coordinate weighting result is obtained;
(6) And setting a limiting condition of the road boundary points, and outputting a boundary point set of the drivable area after the optimization treatment.
Preferably, the step (1) specifically includes:
setting a road boundary point as a three-dimensional point with z=0, wherein an input point set is set as S, the point number is m, and the ith point is set asThe set of road passable area boundary points is expressed as +.>。
Preferably, the step (2) specifically includes the following steps:
(2.1) extracting the input point in the step (1), converting the corresponding coordinate point from Cartesian coordinates to polar coordinates, and reserving the angle and the radius of the point under the polar coordinate system;
(2.2) setting a data storage structure body for each point, wherein the data storage structure body comprises Cartesian coordinates of the point, polar coordinates of the point, a historical polar coordinate container of the point is initialized to be empty, a fitting data container is initialized to be empty, key value pair data are { error, fitting required radius }, a weighted result of the polar coordinates of the fitting is initialized to be 0, and an accumulated fitting error is initialized to be 0.
Preferably, the step (3) specifically includes the following steps:
(3.1) storing the radius of each moment point into a structural body history polar coordinate container corresponding to the corresponding angle at each moment t, wherein the container only retains the first 10 pieces of history data at the current moment, and dynamically deletes the data when the data quantity is exceeded;
(3.2) subjecting the angle history polar coordinate points to a kalman filter process at each angle:
the Kalman filtering observation input is the radius of the current point, the prediction state is the radial value and the radial change speed of the polar coordinate of the point, the absolute value of the difference value between the Kalman filtering result and the radial value of the polar coordinate of the current point is used for setting the implicit attribute of the point fitting, wherein the point fitting attribute is dominant if the absolute value of the difference value is larger than a set threshold value, and the point fitting attribute is recessive if the absolute value of the difference value is smaller than the set threshold value.
Preferably, the step (4) includes splitting the obtained polar coordinates into the set of road passable area boundary points according to an angle range, specifically:
(4.1) setting three parameters of a single fitting angle range, an angle span between each fitting and error precision limitation, splitting a point set into small subsets, and circularly performing multiple fitting on all angles, wherein the single fitting angle range is [0 degrees, 360 degrees ], and setting the upper limit of the angle fitting end as a 360 degrees+fitting angle range, and the point selection rule of which the angle is larger than 360 degrees is as follows: the number of degrees% 360 corresponds to the angular point.
More preferably, the step (4) further includes performing a polar polynomial fitting process on the polar coordinates after the angle splitting process, specifically:
(4.2.1) fitting the basis function module using the following formula as a polynomial：
；
Wherein,for the power function with the highest term number of j, the value range of j is determined by the fitted highest term number;
(4.2.2) setting the polynomial function f (x) in the above formula:
；
wherein the formula comprisesUnknown number of->Is a polynomial coefficient, x is the radius of a point under a polar coordinate system, and n is the highest term number;
(4.2.3) setting fitting conditions: the square of the error of the fitted curve with the original data point in the y direction is minimized, i.e., expressed by the following formula:
；
wherein,as an error function +.>Values fitting for the i points, +.>The true value of the radius of the point i, m is the number of points, and n is the highest number of times;
(4.2.4) solving the above equation for the minimum value in such a way that the derivative for a is equal to 0:
；
；
；
wherein,is the value of the function of the power of k at the ith point, < >>Is the j power function value of the ith point,>and (3) obtaining a fitting function f (x) by solving the equation set for the i-point radius true value, and rewriting the formula (6) into a vector format:
；
wherein m is the number of points, n is the number of times of fitting the highest term, and the formula (7) is rewritten as:
；
wherein,
；
where m is the number of points, n is the number of times the highest term is fitted,is an nth power function of the radius of the mth point, and has Y dimension of m x 1, U dimension of m x (n+1), K dimension of (n+1) x 1, when m>n+1, the overdetermined equation is solved as follows:
。
preferably, the step (4) further comprises performing point structure status update in the following manner:
screening the single fitting of each subset according to a fitting error threshold value, not counting the fitting result when the fitting error threshold value is larger than the threshold value, storing the fitting error and the value of the angle in the current fitting function in a point structure corresponding to the angle when the fitting error is smaller than the threshold value and the fitting attribute of the point is dominant, and adding the accumulated error of the point and the fitting error of the time to finish updating;
after the updating is completed, further judging whether the angle to which the next step length belongs exceeds an angle upper limit, if not, continuing to split the point set according to the new angle range, otherwise, ending the polar coordinate polynomial circular fitting process.
Preferably, the step (5) specifically includes:
traversing all points within 0-360 degrees, judging whether the number of parameters in a fitting data container is more than 1, and if the number of parameters is less than or equal to 1, determining that the weighted result of fitting polar coordinates of the points is an original radius r; if the number of parameters is greater than 1, the fit polar weighting result is calculated according to the following formula:
；
wherein r represents the weighted result of the fitting polar coordinates, n represents the parameter number of the fitting result in the container, E is the accumulated error,error for kth fitting result, +.>The kth fitting result is shown.
Preferably, the step (6) specifically includes the following steps:
(6.1) setting a limit condition of a road boundary point: the maximum boundary radius of the road passable area is max_r, boundary points larger than the radius are filtered, and the maximum radius max_r is adopted for replacing the fitting result of the fitting boundary points larger than the maximum radius;
and (6.2) outputting the boundary point set of the driving area after the optimization treatment.
The device for realizing the boundary optimization of the passable road area based on polar coordinate weighting is mainly characterized by comprising the following components:
a processor configured to execute computer-executable instructions;
and a memory storing one or more computer-executable instructions which, when executed by the processor, implement the steps of the polar-weighting-based road passable region boundary optimization method described above.
The processor for realizing the road passable region boundary optimization based on the polar coordinate weighting is mainly characterized in that the processor is configured to execute computer executable instructions, and the steps of the road passable region boundary optimization method based on the polar coordinate weighting are realized when the computer executable instructions are executed by the processor.
The computer readable storage medium is characterized in that the computer program is stored thereon, and the computer program can be executed by a processor to implement the steps of the road passable region boundary optimization method based on polar coordinate weighting.
The polar coordinate weighting-based road passable region boundary optimization method, the polar coordinate weighting-based road passable region boundary optimization device, the polar coordinate weighting-based road passable region boundary optimization processor and the polar coordinate weighting-based road passable region boundary optimization computer are adopted, the Kalman filtering and error voting weighting fusion method is used, robust filtering can be carried out on noise points in boundary point sets, better smoothing can be carried out on continuous points with large boundary fluctuation changes, more original boundary details can be kept on the basis of optimization, and the accuracy of the road passable boundary can be remarkably improved; meanwhile, the fitting function used in the technical scheme and the solving process are deduced again, the least square fitting process is converted into multiplication of a parameter matrix, the improved function is more attached to the drivable boundary of the actual road, and the solving process is low in time complexity and higher in efficiency. In addition, control parameters such as an angle range, a step length, precision and the like are added in the setting of the point structure body, and the self-adaption can be performed according to an actual fitting point set, so that the fitting process is controllable.
Drawings
Fig. 1 is a flowchart of the polar weighting-based road passable region boundary optimization method of the present invention.
Detailed Description
In order to more clearly describe the technical contents of the present invention, a further description will be made below in connection with specific embodiments.
Before describing in detail embodiments that are in accordance with the present invention, it should be observed that the terms "comprises," "comprising," or any other variation thereof, are intended to cover a non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements does not include only those elements but may include other elements not expressly listed or inherent to such process, method, article, or apparatus.
The polar coordinate weighting-based road passable region boundary optimization method of the technical scheme has the following basic processing ideas: the set of input boundary points is transformed to a polar representation, creating a point structure at each angular interval. At each angle, a Kalman filter is performed to predict the punctiform state according to the historical radius of the angle. And performing multiple cyclic fitting according to the single fitting angle range and the span step length of the next fitting angle in the interval of 0+n degrees (n=360+angle range). And combining and judging the Kalman prediction state and the current point state, setting the implicit attribute of the single fitting result, and voting and weighting all fitting results of the point according to the fitting error under the same angle after the fitting is completed, wherein the fused result is used as the optimization result of the current point.
Referring to fig. 1, the method for optimizing the boundary of the passable road area based on polar coordinate weighting includes the following steps:
(1) Inputting a boundary point set of a passable area of a road;
(2) Converting the input points from Cartesian coordinates to polar coordinates, and setting a data storage structure body for each point;
(3) Storing the radius of the corresponding boundary point acquired by each moment point into a structure body history polar coordinate container corresponding to the angle, and carrying out Kalman filtering prediction processing on the corresponding angle history polar coordinate point on each angle;
(4) Performing polar coordinate polynomial cyclic fitting processing on each acquired polar coordinate, so as to update the angle range of each boundary point;
(5) Voting weighting treatment is carried out on the results subjected to the multiple fitting treatment, and a fitting polar coordinate weighting result is obtained;
(6) And setting a limiting condition of the road boundary points, and outputting a boundary point set of the drivable area after the optimization treatment.
As a preferred embodiment of the present invention, the step (1) specifically includes:
setting a road boundary point as a three-dimensional point with z=0, wherein an input point set is set as S, the point number is m, and the ith point is set asThe set of road passable area boundary points is expressed as +.>。
As a preferred embodiment of the present invention, the step (2) specifically includes the following steps:
(2.1) extracting the input point in the step (1), converting the corresponding coordinate point from Cartesian coordinates to polar coordinates, and reserving the angle and the radius of the point under the polar coordinate system;
(2.2) setting a data storage structure body for each point, wherein the data storage structure body comprises Cartesian coordinates of the point, polar coordinates of the point, a historical polar coordinate container of the point is initialized to be empty, a fitting data container is initialized to be empty, key value pair data are { error, fitting required radius }, a weighted result of the polar coordinates of the fitting is initialized to be 0, and an accumulated fitting error is initialized to be 0.
As a preferred embodiment of the present invention, the step (3) specifically includes the following steps:
(3.1) storing the radius of each moment point into a structural body history polar coordinate container corresponding to the corresponding angle at each moment t, wherein the container only retains the first 10 pieces of history data at the current moment, and dynamically deletes the data when the data quantity is exceeded;
(3.2) subjecting the angle history polar coordinate points to a kalman filter process at each angle:
the Kalman filtering observation input is the radius of the current point, the prediction state is the radial value and the radial change speed of the polar coordinate of the point, the absolute value of the difference value between the Kalman filtering result and the radial value of the polar coordinate of the current point is used for setting the implicit attribute of the point fitting, wherein the point fitting attribute is dominant if the absolute value of the difference value is larger than a set threshold value, and the point fitting attribute is recessive if the absolute value of the difference value is smaller than the set threshold value.
As a preferred embodiment of the present invention, the step (4) includes splitting the obtained polar coordinates into the set of boundary points of the passable road area according to an angle range, specifically:
(4.1) setting three parameters of a single fitting angle range, an angle span between each fitting and error precision limitation, splitting a point set into small subsets, and circularly performing multiple fitting on all angles, wherein the single fitting angle range is [0 degrees, 360 degrees ], and setting the upper limit of the angle fitting end as a 360 degrees+fitting angle range, and the point selection rule of which the angle is larger than 360 degrees is as follows: the number of degrees% 360 corresponds to the angular point.
As a preferred embodiment of the present invention, the step (4) further includes performing a polar polynomial fitting process on the polar coordinates after the angle splitting process, specifically:
(4.2.1) fitting the basis function module using the following formula as a polynomial：
；
Wherein,for the power function with the highest term number of j, the value range of j is determined by the fitted highest term number;
(4.2.2) setting the polynomial function f (x) in the above formula:
；
wherein the formula comprisesUnknown number of->Is a polynomial coefficient, x is the radius of a point under a polar coordinate system, and n is the highest term number;
(4.2.3) setting fitting conditions: the square of the error of the fitted curve with the original data point in the y direction is minimized, i.e., expressed by the following formula:
；
wherein,as an error function +.>Values fitting for the i points, +.>The true value of the radius of the point i, m is the number of points, and n is the highest number of times;
(4.2.4) solving the above equation for the minimum value in such a way that the derivative for a is equal to 0:
；
；
；
wherein,is the value of the function of the power of k at the ith point, < >>Is the j power function value of the ith point,>and (3) obtaining a fitting function by solving the equation set for the i-point radius true value, and rewriting the formula (6) into a vector format:
；
wherein m is the number of points, n is the number of times of fitting the highest term, and the formula (7) is rewritten as:
；
wherein,
；
where m is the number of points, n is the number of times the highest term is fitted,is an nth power function of the radius of the mth point, and has Y dimension of m x 1, U dimension of m x (n+1), K dimension of (n+1) x 1, when m>n+1, the overdetermined equation is solved as follows:
。
as a preferred embodiment of the present invention, the step (4) further includes performing a point structure state update in the following manner:
screening the single fitting of each subset according to a fitting error threshold value, not counting the fitting result when the fitting error threshold value is larger than the threshold value, storing the fitting error and the value of the angle in the current fitting function in a point structure corresponding to the angle when the fitting error is smaller than the threshold value and the fitting attribute of the point is dominant, and adding the accumulated error of the point and the fitting error of the time to finish updating;
after the updating is completed, further judging whether the angle to which the next step length belongs exceeds an angle upper limit, if not, continuing to split the point set according to the new angle range, otherwise, ending the polar coordinate polynomial circular fitting process.
As a preferred embodiment of the present invention, the step (5) specifically includes:
traversing all points within 0-360 degrees, judging whether the number of parameters in a fitting data container is more than 1, and if the number of parameters is less than or equal to 1, determining that the weighted result of fitting polar coordinates of the points is an original radius r; if the number of parameters is greater than 1, the fit polar weighting result is calculated according to the following formula:
；
wherein r represents the weighted result of the fitting polar coordinates, n represents the parameter number of the fitting result in the container, E is the accumulated error,error for kth fitting result, +.>The kth fitting result is shown.
As a preferred embodiment of the present invention, the step (6) specifically includes the steps of:
(6.1) setting a limit condition of a road boundary point: the maximum boundary radius of the road passable area is max_r, boundary points larger than the radius are filtered, and the maximum radius max_r is adopted for replacing the fitting result of the fitting boundary points larger than the maximum radius;
and (6.2) outputting the boundary point set of the driving area after the optimization treatment.
In practical application, the method for optimizing the boundary of the passable road area based on polar coordinate weighting mainly comprises the following processing steps:
1) And (5) inputting a road boundary point set.
Inputting the extracted travelable regionA set of spatial points of the boundary. In the present technical solution, the road boundary point is a three-dimensional point with z=0, the input point set is S, the point number is m, and the i-th point isThe point set can be expressed as +.>。
2) Cartesian coordinates are transformed into polar coordinates.
And extracting the input points, and converting xy coordinates of the points into a polar coordinate system for representation. Finally, the angle (unit degree) and the radius (unit m) of the point under the polar coordinate system are reserved. In the conversion, a data storage structure is set for each point, and the data storage structure comprises Cartesian coordinates of the point, polar coordinates of the point, a historical polar coordinate container of the point is initialized to be empty, a fitting data container is initialized to be empty (key value data { error, fitting required radius }), a fitting polar coordinate weighting result is initialized to be 0, and an accumulated fitting error is initialized to be 0.
3) History point kalman filtering.
And storing the radius of the moment point into a structural body history polar coordinate container corresponding to the angle at each moment t, wherein the container only keeps the first 10 pieces of history data at the current moment, and dynamically deleting the data when the data quantity is exceeded. The angular history polar coordinate points are Kalman filtered at each angle. In the technical scheme, kalman filtering observation input is the radius of a point, and the prediction state is the radial value and radial change speed of the polar coordinate of the point. The absolute value of the difference value between the Kalman filtering result and the polar coordinate radius of the current point is used for setting the implicit attribute of the point fitting, and if the absolute value of the difference value is larger than the set threshold value, the point fitting attribute is explicit, otherwise, the point fitting attribute is implicit.
4) And (5) polar polynomial cyclic fitting.
a. Splitting the point set according to the angle range.
In order to obtain the optimization result in a robust way, the optimization performance degradation caused by single fitting misalignment is avoided. According to the invention, three parameters including a single fitting angle range, angle spans among fitting and error precision limit are set, a point set is split into small subsets, and fitting is circularly carried out for multiple times on all angles. In order to avoid data truncation caused by crossing 360 degrees, a closed loop of 360 degrees and 0 degrees is arranged; in order to also include multiple fits at 360 ° and nearby angles, the upper limit of the end of the angle fit is set to 360 ° + the fit angle range, and the point selection rule of which is greater than 360 ° is as follows: the result of the number of degrees% 360 corresponds to the angle point. The angular span set by the invention should be less than half of the fitting angular range.
b. And (5) polar coordinate polynomial fitting.
The polynomial fitting basis function module used in the invention is as follows:
；
in the formula (1),is a power function with the highest term number j. The value range of j is determined by the highest number of times of fitting.
The polynomial function is:
；
the formula (2) containsUnknown number of->And x is the radius of a point under a polar coordinate system, and n is the highest term degree.
Fitting conditions: the square of the error in the y-direction of the fitted curve to the original data point is minimal. Namely:
；
in the formula (3)As an error function +.>Values fitting for the i points, +.>The true value of the radius of the point i, m is the number of points, and n is the highest number of times;
the minimum problem is solved for equation (3), i.e., the derivative for a equals 0:
；
；
；
is the value of the function of the power of k at the ith point, < >>Is the j power function value of the ith point,>is the i-point radius true value.
Solving the equation set to obtain the coefficient a0 a1 a 2. Equation (6) is written in vector format:
；
in the formula (7), m is the number of points, and n is the number of times of fitting the highest term.
For convenience of representation, the above formula is written as:
；
wherein:
；
in the formula (9), m is the number of points, and n is the number of times of fitting the highest term.Is an nth power function of the radius of the mth point.
In formula (9), the Y dimension is m×1, the u dimension is m×1 (n+1), and the K dimension is (n+1) ×1. When m > n+1, solve for the over-determined equation:
；
from the above deductions, the function fitting process of the present technical solution finally becomes the operation of matrix multiplication, i.e. formula (10).
c. And updating the point structure state.
And screening the single fitting of each subset according to a fitting error threshold value, not considering the fitting result when the fitting error threshold value is larger than the threshold value, and storing the fitting error and the value of the angle in the current fitting function in a point structure corresponding to the angle when the error is smaller than the threshold value and the fitting attribute of the point is dominant. And adding the accumulated error of the point and the fitting error of the time to update.
d. And judging the maximum upper angle limit.
And c, after finishing the subset fitting once, judging whether the angle to which the next step length belongs exceeds an angle upper limit, if not, continuing to split the point set according to a new angle range, and repeating the steps b and c. And otherwise, finishing the fitting process.
5) Voting weights are carried out on the fitting results.
All points of 0-360 degrees are traversed, and the fitting data container in each angle point structure body has a new state through the fitting process. Firstly judging whether the number of parameters in the container is more than 1, if so, determining that the fitted polar coordinate weighting result of the point is the original radius r; if the number of parameters is greater than 1, then the fit polar weighting result is calculated using equation (11).
；
In the above formula, r represents the weighted result of the fitting polar coordinates, n represents the number of parameters of the fitting result in the container, E is the accumulated error,error for kth fitting result, +.>The kth fitting result is shown.
6) Boundary constraints.
In the technical scheme, the maximum boundary radius max_r of the passable road area is set, and the boundary point larger than the radius is considered to be accurate in practice and not high, so that the filtering is performed. For fitting results where the fitting boundary points are greater than the maximum radius, the maximum radius max_r is used instead.
7) And outputting the optimized point set.
In practical application, compared with the existing traditional fitting method, the polar coordinate weighting-based road passable region boundary optimization method of the technical scheme can remove boundary noise points and greatly increase the reliability of boundary fitting; meanwhile, the boundary fitting result of the technical scheme adopts a cyclic voting weighted fusion method, so that the accuracy of the fitting boundary can be effectively improved, and the fitting accuracy can be still maintained under the condition of poor single fitting effect; besides, the self-adaptive fitting process can be realized, boundary fitting adjustment can be automatically carried out according to the set fitting angle range and the circulation step length, and the time complexity of the whole fitting process is low due to matrix multiplication operation in the fitting process.
In practice, it will be appreciated that the "Kalman filter prediction" and the "polar polynomial cyclic fit" are essentially synchronized, as shown in FIG. 1. Only the prediction result of the kalman filter participates in updating the state of the point structure, functionally, the kalman filter predicts the state of the points at each angle in the time dimension, and the polar polynomial circular fitting optimizes the states in the space dimension by fitting the states of the points at each angle at each moment. In the task of polar coordinate polynomial cyclic fitting, the pre-processing step of splitting according to the angle range point set is split in detail.
The device for realizing the optimization of the boundary of the passable road area based on the polar coordinate weighting comprises the following components:
a processor configured to execute computer-executable instructions;
and a memory storing one or more computer-executable instructions which, when executed by the processor, implement the steps of the polar-weighting-based road passable region boundary optimization method described above.
The processor is configured to execute computer executable instructions, and the computer executable instructions, when executed by the processor, implement the steps of the road passable region boundary optimization method based on polar coordinates.
The computer readable storage medium having stored thereon a computer program executable by a processor to perform the steps of the polar weighting based road passable region boundary optimization method described above.
Any process or method descriptions in flow charts or otherwise described herein may be understood as representing modules, segments, or portions of code which include one or more executable instructions for implementing specific logical functions or steps of the process, and further implementations are included within the scope of the preferred embodiment of the present invention in which functions may be executed out of order from that shown or discussed, including substantially concurrently or in reverse order, depending on the functionality involved, as would be understood by those reasonably skilled in the art of the present invention.
It is to be understood that portions of the present invention may be implemented in hardware, software, firmware, or a combination thereof. In the above embodiments, the various steps or methods may be implemented in software or firmware stored in a memory and executed by a suitable instruction execution device.
Those of ordinary skill in the art will appreciate that all or a portion of the steps carried out in the method of the above-described embodiments may be implemented by a program to instruct related hardware, and the program may be stored in a computer readable storage medium, where the program when executed includes one or a combination of the steps of the method embodiments.
The above-mentioned storage medium may be a read-only memory, a magnetic disk or an optical disk, or the like.
In the description of the present specification, reference to the terms "one embodiment," "some embodiments," "examples," "specific examples," or "embodiments," etc., means that a particular feature, structure, material, or characteristic described in connection with the embodiment or example is included in at least one embodiment or example of the invention. In this specification, schematic representations of the above terms do not necessarily refer to the same embodiments or examples. Furthermore, the particular features, structures, materials, or characteristics described may be combined in any suitable manner in any one or more embodiments or examples.
While embodiments of the present invention have been shown and described above, it will be understood that the above embodiments are illustrative and not to be construed as limiting the invention, and that variations, modifications, alternatives and variations may be made to the above embodiments by one of ordinary skill in the art within the scope of the invention.
The polar coordinate weighting-based road passable region boundary optimization method, the polar coordinate weighting-based road passable region boundary optimization device, the polar coordinate weighting-based road passable region boundary optimization processor and the polar coordinate weighting-based road passable region boundary optimization computer are adopted, the Kalman filtering and error voting weighting fusion method is used, robust filtering can be carried out on noise points in boundary point sets, better smoothing can be carried out on continuous points with large boundary fluctuation changes, more original boundary details can be kept on the basis of optimization, and the accuracy of the road passable boundary can be remarkably improved; meanwhile, the fitting function used in the technical scheme and the solving process are deduced again, the least square fitting process is converted into multiplication of a parameter matrix, the improved function is more attached to the drivable boundary of the actual road, and the solving process is low in time complexity and higher in efficiency. In addition, control parameters such as an angle range, a step length, precision and the like are added in the setting of the point structure body, and the self-adaption can be performed according to an actual fitting point set, so that the fitting process is controllable.
In this specification, the invention has been described with reference to specific embodiments thereof. It will be apparent, however, that various modifications and changes may be made without departing from the spirit and scope of the invention. The specification and drawings are, accordingly, to be regarded in an illustrative rather than a restrictive sense.
Claims (7)
1. The method for optimizing the boundary of the passable road area based on the polar coordinate weighting is characterized by comprising the following steps:
(1) Inputting a boundary point set of a passable area of a road;
(2) Converting the input points from Cartesian coordinates to polar coordinates, and setting a data storage structure body for each point;
(3) Storing the radius of the corresponding boundary point acquired by each moment point into a structure body historical polar coordinate container corresponding to the current angle, and carrying out Kalman filtering prediction processing on the corresponding angle historical polar coordinate point on each angle;
(4) Performing polar coordinate polynomial cyclic fitting processing on each acquired polar coordinate, so as to update the angle range of each boundary point;
(5) Voting weighting treatment is carried out on the results subjected to the multiple fitting treatment, and a fitting polar coordinate weighting result is obtained;
(6) Setting a limiting condition of a road boundary point, and outputting a boundary point set of the drivable area after optimization treatment;
the step (3) specifically comprises the following steps:
(3.1) storing the radius of each moment point into a structural body history polar coordinate container corresponding to the corresponding angle at each moment t, wherein the container only keeps the first 10 pieces of history data at the current moment, and the container is dynamically deleted when the current data quantity is exceeded;
(3.2) subjecting the angle history polar coordinate points to a kalman filter process at each angle:
the Kalman filtering observation input is the radius of the current point, the prediction state is the radial value and the radial change speed of the polar coordinate of the point, the absolute value of the difference value between the Kalman filtering result and the radial value of the polar coordinate of the current point is used for setting the fitting implicit attribute of the point, wherein the fitting attribute of the point is dominant if the absolute value of the difference value is larger than a set threshold value, and the fitting attribute of the point is recessive if the absolute value of the difference value is smaller than the set threshold value;
the step (4) includes splitting the acquired polar coordinates into the road passable area boundary point sets according to the angle range, specifically:
(4.1) setting three parameters of a single fitting angle range, an angle span between each fitting and error precision limitation, splitting a point set into small subsets, and circularly performing multiple fitting on all angles, wherein the single fitting angle range is [0 degrees, 360 degrees ], and setting the upper limit of the angle fitting end as a 360 degrees+fitting angle range, and the point selection rule of which the angle is larger than 360 degrees is as follows: the current degree is%360 degrees of the angle point corresponding to the result;
the step (4) further comprises the step of performing polar coordinate polynomial fitting processing on the polar coordinates subjected to angle splitting processing in the following manner, wherein the polar coordinate polynomial fitting processing specifically comprises the following steps:
(4.2.1) fitting the basis function module using the following formula as a polynomial：
；
Wherein,for the power function with the highest term number of j, the value range of j is determined by the fitted highest term number;
(4.2.2) setting the polynomial function f (x) in the above formula:
；
wherein the formula comprisesUnknown number of->Is a polynomial coefficient, x is the radius of a point under a polar coordinate system, and n is the highest term number;
(4.2.3) setting fitting conditions: the square of the error of the fitted curve with the original data point in the y direction is minimized, i.e., expressed by the following formula:
；
wherein,as an error function +.>Values fitting for the i points, +.>The true value of the radius of the point i, m is the number of points, and n is the highest number of times;
(4.2.4) solving the above equation for the minimum value in such a way that the derivative for a is equal to 0:
；
；
；
wherein,is the value of the function of the power of k at the ith point, < >>Is the j power function value of the ith point,>and (3) obtaining a fitting function f (x) by solving the equation set for the i-point radius true value, and rewriting the formula (6) into a vector format:
；
wherein m is the number of points, n is the number of times of fitting the highest term, and the formula (7) is rewritten as:
；
wherein,
；
where m is the number of points, n is the number of times the highest term is fitted,is an nth power function of the m-th point radius, and the Y dimension is m x 1, U dimensionThe degree is m× (n+1), the K dimension is (n+1) ×1, when m>n+1, the overdetermined equation is solved as follows:
；
the step (4) further comprises the following steps of updating the point structure state:
screening the single fitting of each subset according to a fitting error threshold value, not counting the fitting result when the fitting error threshold value is larger than the threshold value, storing the fitting error and the value of the angle in the current fitting function in a point structure corresponding to the angle when the fitting error is smaller than the threshold value and the fitting attribute of the point is dominant, and adding the accumulated error of the point and the fitting error of the time to finish updating;
after the updating is finished, further judging whether the angle to which the next step length belongs exceeds an angle upper limit, if not, continuing to split the point set according to a new angle range, otherwise, ending the polar coordinate polynomial cyclic fitting process;
the step (5) is specifically as follows:
traversing all points within 0-360 degrees, judging whether the number of parameters in a fitting data container is more than 1, and if the number of parameters is less than or equal to 1, determining that the weighted result of fitting polar coordinates of the points is an original radius r; if the number of parameters is greater than 1, the fit polar weighting result is calculated according to the following formula:
；
wherein r represents the weighted result of the fitting polar coordinates, n represents the parameter number of the fitting result in the container, E is the accumulated error,error for kth fitting result, +.>The kth fitting result is shown.
2. The method for optimizing the boundary of a passable road area based on polar coordinates according to claim 1, wherein the step (1) specifically comprises:
setting a road boundary point as a three-dimensional point with z=0, wherein an input point set is set as S, the point number is m, and the ith point is set asThe set of boundary points of the passable area of the road is expressed as +.>。
3. The method for optimizing the boundary of a passable road area based on polar weighting according to claim 2, wherein said step (2) specifically comprises the steps of:
(2.1) extracting the input point in the step (1), converting the corresponding coordinate point from Cartesian coordinates to polar coordinates, and reserving the angle and the radius of the point under the polar coordinate system;
(2.2) setting a data storage structure body for each point, wherein the data storage structure body comprises Cartesian coordinates of the point, polar coordinates of the point, a historical polar coordinate container of the point is initialized to be empty, a fitting data container is initialized to be empty, key value pair data are { error, fitting required radius }, a weighted result of the polar coordinates of the fitting is initialized to be 0, and an accumulated fitting error is initialized to be 0.
4. The method for optimizing the boundary of a passable road area based on polar weighting according to claim 1, wherein said step (6) specifically comprises the steps of:
(6.1) setting a limit condition of a road boundary point: the maximum boundary radius of the road passable area is max_r, boundary points larger than the radius are filtered, and the maximum radius max_r is adopted for replacing the fitting result of the fitting boundary points larger than the maximum radius;
and (6.2) outputting the boundary point set of the driving area after the optimization treatment.
5. An apparatus for optimizing the boundary of a passable road area based on polar weighting, said apparatus comprising:
a processor configured to execute computer-executable instructions;
a memory storing one or more computer-executable instructions which, when executed by the processor, perform the steps of the polar-weighted-based road passable region boundary optimization method of any one of claims 1-4.
6. A processor for performing polar weighting based road-passable zone boundary optimization, wherein the processor is configured to execute computer-executable instructions that, when executed by the processor, perform the steps of the polar weighting based road-passable zone boundary optimization method of any one of claims 1-4.
7. A computer readable storage medium having stored thereon a computer program executable by a processor to implement the steps of the polar weighting based road passable region boundary optimization method of any one of claims 1-4.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311646355.3A CN117351449B (en) | 2023-12-04 | 2023-12-04 | Polar coordinate weighting-based road passable region boundary optimization method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311646355.3A CN117351449B (en) | 2023-12-04 | 2023-12-04 | Polar coordinate weighting-based road passable region boundary optimization method |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN117351449A CN117351449A (en) | 2024-01-05 |
| CN117351449B true CN117351449B (en) | 2024-02-09 |
Family
ID=89357873
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311646355.3A Active CN117351449B (en) | 2023-12-04 | 2023-12-04 | Polar coordinate weighting-based road passable region boundary optimization method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117351449B (en) |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104866823A (en) * | 2015-05-11 | 2015-08-26 | 重庆邮电大学 | Vehicle detection and tracking method based on monocular vision |
| CN106842231A (en) * | 2016-11-08 | 2017-06-13 | 长安大学 | A kind of road edge identification and tracking |
| CN109684921A (en) * | 2018-11-20 | 2019-04-26 | 吉林大学 | A kind of road edge identification and tracking based on three-dimensional laser radar |
| JP2021086555A (en) * | 2019-11-29 | 2021-06-03 | 日本電産株式会社 | Automatic driving support device, program and automatic driving system |
| KR20220063433A (en) * | 2020-11-10 | 2022-05-17 | 성균관대학교산학협력단 | Method and apparatus for recognizing and tracking lane using avm image, vehicle data and map information |
| WO2023216470A1 (en) * | 2022-05-11 | 2023-11-16 | 合众新能源汽车股份有限公司 | Drivable area detection method and apparatus, and device |
Family Cites Families (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| GB2591332B (en) * | 2019-12-19 | 2024-02-14 | Motional Ad Llc | Foreground extraction using surface fitting |
| US20220408019A1 (en) * | 2021-06-17 | 2022-12-22 | Fyusion, Inc. | Viewpoint path modeling |
-
2023
- 2023-12-04 CN CN202311646355.3A patent/CN117351449B/en active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104866823A (en) * | 2015-05-11 | 2015-08-26 | 重庆邮电大学 | Vehicle detection and tracking method based on monocular vision |
| CN106842231A (en) * | 2016-11-08 | 2017-06-13 | 长安大学 | A kind of road edge identification and tracking |
| CN109684921A (en) * | 2018-11-20 | 2019-04-26 | 吉林大学 | A kind of road edge identification and tracking based on three-dimensional laser radar |
| JP2021086555A (en) * | 2019-11-29 | 2021-06-03 | 日本電産株式会社 | Automatic driving support device, program and automatic driving system |
| KR20220063433A (en) * | 2020-11-10 | 2022-05-17 | 성균관대학교산학협력단 | Method and apparatus for recognizing and tracking lane using avm image, vehicle data and map information |
| WO2023216470A1 (en) * | 2022-05-11 | 2023-11-16 | 合众新能源汽车股份有限公司 | Drivable area detection method and apparatus, and device |
Non-Patent Citations (3)
| Title |
|---|
| Polynomial Based Kalman Filter Result Fitting to Data;Georgiana MAGU 等;《2020 43rd International Conference on Telecommunications and Signal Processing (TSP)》;全文 * |
| Real-time extraction method of road boundary based on three-dimensional lidar;Kunlei Hu 等;《Journal of Physics: Conference Series》;全文 * |
| 空间直线拟合的加权极坐标法;史翔 等;《测绘地理信息》;第44卷(第2期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN117351449A (en) | 2024-01-05 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108803321B (en) | Autonomous underwater vehicle track tracking control method based on deep reinforcement learning | |
| US20220363259A1 (en) | Method for generating lane changing decision-making model, method for lane changing decision-making of unmanned vehicle and electronic device | |
| US20220004191A1 (en) | Usv formation path-following method based on deep reinforcement learning | |
| CN111142522B (en) | Method for controlling agent of hierarchical reinforcement learning | |
| CN109885891A (en) | A kind of intelligent vehicle GPU accelerates method for planning track parallel | |
| CN109176532B (en) | Method, system and device for planning path of mechanical arm | |
| EP3791324A1 (en) | Sample-efficient reinforcement learning | |
| EP3699698B1 (en) | Method and device for processing control parameter, and storage medium | |
| KR102229859B1 (en) | Operation management method, apparatus and system using machine learning and transfer learning | |
| WO2021227536A1 (en) | Methods and systems for support policy learning | |
| US20210263526A1 (en) | Method and device for supporting maneuver planning for an automated driving vehicle or a robot | |
| CN109556609B (en) | Artificial intelligence-based collision avoidance method and device | |
| CN115298668A (en) | Reinforcement learning using adaptive reward calculation scheme | |
| CN113721645A (en) | Unmanned aerial vehicle continuous maneuvering control method based on distributed reinforcement learning | |
| CN113625753B (en) | Method for guiding neural network to learn unmanned aerial vehicle maneuver flight by expert rules | |
| CN113419424B (en) | Modeling reinforcement learning robot control method and system for reducing overestimation | |
| CN117351449B (en) | Polar coordinate weighting-based road passable region boundary optimization method | |
| CN113503885B (en) | Robot path navigation method and system based on sampling optimization DDPG algorithm | |
| JP2022189799A (en) | Demonstration-conditioned reinforcement learning for few-shot imitation | |
| CN109840921B (en) | Method and device for determining result of unmanned task and unmanned equipment | |
| CN110533158B (en) | Model construction method, system and non-volatile computer readable recording medium | |
| CN114529010A (en) | Robot autonomous learning method, device, equipment and storage medium | |
| CN114167872A (en) | Robot obstacle avoidance method and system, computer and robot | |
| CN111290118A (en) | Decoupling control method and device for deformable mirror | |
| CN116700015B (en) | Active stability augmentation control method and device for underwater vehicle |
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 |