CN109886877B - Method for fitting waypoints and splicing segmented routes - Google Patents
Method for fitting waypoints and splicing segmented routes Download PDFInfo
- Publication number
- CN109886877B CN109886877B CN201910147367.9A CN201910147367A CN109886877B CN 109886877 B CN109886877 B CN 109886877B CN 201910147367 A CN201910147367 A CN 201910147367A CN 109886877 B CN109886877 B CN 109886877B
- Authority
- CN
- China
- Prior art keywords
- point
- points
- waypoints
- route
- routes
- 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
Images
Abstract
The invention discloses a method for fitting waypoints and splicing sectional routes, which is technically characterized by comprising the following steps of: the uploaded route data are usually the coordinates of waypoints recorded while the vehicle is driving, and routes stored in the route library are classified according to road sections. The invention clusters the uploaded waypoints, fits the waypoints of each road section into a smooth route with continuous second derivative, and can also identify and remove noise points deviating from the route generated during scanning or uploading. The fitted segmented routes are stored in a route library. When the navigation line combination method needs to be called, the navigation line combination method generates the navigation line at the interval of the sectional navigation line according to the coordinates and the slope of the head point and the tail point of each navigation line, and the navigation line does not need to be re-fitted by calling all navigation point data. By adopting the technical scheme, the denoising of the waypoints and the sectional fitting and connection of the route can be efficiently realized.
Description
Technical Field
The invention relates to the field of image processing, in particular to a method for waypoint fitting and sectional route splicing.
Background
With the rapid development of the field of intelligent driving in recent years, a large number of automobile enterprises begin to research intelligent driving. The uploaded airlines can be managed and called again by establishing an airline management system, and when the airlines are called, the segmented airlines need to be combined into a complete airline, but at present, no good method is available for connecting the segmented airlines. In addition, the coordinates of waypoints are usually collected at fixed time intervals during the driving process of the vehicle, and the data need to be converted into a segmented route to be stored in a route database during uploading, so that a method for classifying and fitting the waypoints into the route is needed to be researched.
The cubic spline interpolation curve belongs to piecewise smooth interpolation, and is a mathematical model of a mathematical curve cubic spline curve applied to a two-dimensional graph application program, and is shown as a formula (1).
When x belongs to [ x ] j-1 ,x j ]The expression of s (x) is:
wherein x j-1 、x j 、y j-1 And y j As coordinates of two control points, h j ＝x j -x j-1 Therefore, only M is determined 0 ，M 1 …，M n The n +1 undetermined parameters can obtain the expression s of the cubic spline interpolation function in each subinterval j (x)。
M of natural boundary conditions 0 ＝M n =0, unknown element M 1 ，M 2 …，M n-1 This can be solved by the following equation:
wherein
In the formula f [ x ] j-1 ,x j ,x j+1 ]To relate to x j-1 、x j 、x j+1 The second order difference quotient of the three points.
A bezier curve is a mathematical curve applied to a two-dimensional graphics application, and a mathematical model of a cubic bezier curve is shown in equation (4):
B(t)＝(1-t 2 )P 0 +2t(1-t)P 1 +t 2 P 2 ,t∈[0,1] (4)
in the formula P 0 、P 1 And P 2 Respectively representing a starting point, a control point and an end point.
Disclosure of Invention
The invention aims to solve the technical problem of providing a method for classifying, denoising and fitting waypoints into a segmented flight path for storage, and simultaneously solving the splicing problem of the segmented flight path during calling.
The technical scheme adopted by the invention for solving the technical problems comprises the following steps:
1) Converting the collected waypoint data into a point set { P) of a two-dimensional plane 1 、P 2 ……P n }, the point set contains the coordinates of the respective points;
2) According to the distance between adjacent waypoints, the waypoints are classified by a clustering algorithm 1 、L 2 ……L m G, wherein L 1 、L 2 ……L m The flight point sets from 1 st to m-th road sections are provided, G is an isolated point set and does not participate in the fitting of subsequent flight paths;
3) For any waypoint set L i All the navigation points in the navigation system are sorted according to the position relation, and the navigation point coordinates from the starting point to the end point are respectively recorded as P' i1 、P' i2 ……P' ij Update of waypoint set to L 'after completion of sorting' i ；
4) Fitting navigation point set L 'by utilizing cubic spline interpolation function' i Middle P' i1 To P' ij Selecting natural boundary conditions for the routes between the two paths;
5) When splicing the route, fitting two-segment navigation by using Bezier curveThe gap of the wire. Terminal point P 'of previous flight line' (i-1)j Is the starting point of the Bessel curve and the starting point P 'of the following route segment' i1 The point of intersection of tangent lines at the two points is the end point of the bezier curve, and the point of intersection of tangent lines at the two points is a control point to form the bezier curve; the clustering algorithm in the step 2 comprises the following specific steps:
1) Number of initialization classes n =0, i =0, isolated set
2) Judging whether an unclassified sampling point exists or not, if so, executing the step 3, and if not, ending the algorithm;
3) Searching unclassified sample points P i Neighborhood U of ε (P i ) Whether there are other unclassified sample points within, wherein
Clustering threshold epsilon =3 · min { d (P) i ,P j ) }. If yes, continuing to execute the step 4; if not, then P is added i Classified as a set of isolated points G, let G = G & { P } i I = i +1, and returning to the step 2 for searching unclassified sampling points;
4) n = n +1, let P n1 ＝P i Establishing a new waypoint set L n ＝{P n1 The number of points in the waypoint set is initialized to be m =1;
5) Counting the number k of other unclassified sampling points in the neighborhood of the newly added sampling point, and establishing a point set { P n(m+1) ,P n(m+2) …,P n(m+k) }. Respectively connect the sampling points P i Assigning k points in the neighborhood to P n(m+1) ，P n(m+2) …，P n(m+k) Let L n ＝L n ∪{P n(m+1) ,P n(m+2) …,P n(m+k) }, and update points m = m + k;
6) Searching for newly added sampling point P n(m-k+1) ，P n(m-k+2) …，P nm If there are any unclassified sampling points in the neighborhood, go back to step 5 and add the points into the navigation point set L n If not, the classification is complete, i = i +1, go backGo to step 2; the sorting algorithm in the step 3 comprises the following specific steps:
1) Initialization i =1
2) Searching a certain navigation point set L n Waypoint P in ni Neighborhood U of ε (P ni ) All waypoints belonging to the waypoint set are contained;
3) If the waypoint P ni Neighborhood U of ε (P ni ) If only one other waypoint exists in the navigation system, the waypoint is taken as an end point, and the step 5 is carried out; if the waypoint P ni Neighborhood U of ε (P ni ) Turning to the step 4 when the number of other waypoints is more than 1;
4) Determine whether the pair is satisfied
Is provided with
If yes, the point is an end point, and the step 5 is carried out; if not, the point is not an end point, i = i +1, and step 2 is switched to search the next point;
5) Judging P' n1 If it is already present, let P 'if it is not present' n1 ＝P ni I = i +1 and goes to step 1; if P 'is already present' n1 Then let P' nm ＝P ni And the end point searching process is finished and the step 6 is carried out. Wherein m is the number of points of the navigation point set, P' n1 And P' nm Respectively representing the starting point and the end point of the navigation point set;
6) The initialization j =1 and the initialization j is carried out,
7) Search for P' nj Neighborhood U of ε (P' nj ) All other unclassified sample points in the sample, if point P k Satisfy to
Is d (P' ni ,P k )≤d(P' ni ,P x ) Then P is k Let j = j +1,P 'as the next point' nj ＝P k ；
8) Judging whether j = m-1 exists, if so, finishing the sorting, turning to the step 9, and if not, returning to the step 7 to continue searching for the next point;
9) Establishing ordered complete waypoint set L' n ＝{P' n1 ,P' n2 …,P' nm }；
Any navigation point set L 'is drawn by a cubic spline interpolation curve in step 4' i Selecting natural boundary conditions, namely at a starting point P' i1 And terminal point P' im The second derivative of the curve is 0. P 'is calculated according to equations (1), (2) and (3), respectively' i1 And P' i2 、P' i2 And P' i3 ……P' i(m-1) And P' im Cubic spline function in between. And storing the curve to an air route database after the calculation is finished.
And when needing to call the air route, selecting a corresponding air route segment from the air route library, and fitting the clearance between the air route segments through the Bezier curve in the step 5.
The method has the advantages that the navigation points can be classified in segments through a clustering algorithm, and noise points generated in the scanning or uploading process can be removed through the method; the curve fitted by the cubic spline under the natural boundary condition has good mathematical properties, and the curve has continuous second derivative; when the route is called, the coordinate and tangent slope of the splicing point are only needed by splicing two routes by using the Bezier curve, and the original route point information is not needed to be called, so that the calculated amount can be effectively reduced, and the efficiency is improved.
Drawings
FIG. 1 illustrates the steps of the waypoint fitting and segmented course splicing method of the present invention;
FIG. 2 is a flow chart of a waypoint clustering algorithm in accordance with the present invention;
FIG. 3 is a flow chart of the waypoint set ordering algorithm of the present invention.
Detailed Description
The following description is provided for illustrative purposes and is not intended to limit the invention to the particular embodiments disclosed.
The invention is described in detail below with reference to the figures and the specific embodiments.
As shown in fig. 1:
1. converting the collected waypoint data into a two-dimensional plane point set { P } 1 、P 2 ......P n }；
2. Classifying waypoints L using clustering algorithm 1 、L 2 ……L m G, wherein L 1 、L 2 ……L m The method is a flight point set on the 1 st to the mth road sections, and G is an isolated point set and does not participate in the fitting of subsequent flight paths. The specific algorithm is as follows (as shown in fig. 2):
1) Number of initialization classes n =0, isolated set
2) Judging whether an unclassified sampling point exists or not, if so, executing the step 3, and if not, ending the algorithm;
3) Searching unclassified sample points P i Neighborhood U of ε (P i ) Whether there are other unclassified sample points within, wherein
Clustering threshold epsilon =3 · min { d (P) i ,P j ) }. If yes, continuing to execute the step 4; if not, then P is added i Classified as a set of isolated points G, let G = G & { P } i Returning to the step 2 to search an unclassified sampling point;
4) n = n +1, let P n1 ＝P i Establishing a new waypoint set L n ＝{P n1 The number of points in the waypoint set is initialized to be m =1;
5) Counting the number k of other unclassified sampling points in the neighborhood of the newly added sampling point, and establishing a point set
{P n(m+1) ,P n(m+2) …,P n(m+k) }. Respectively connect the sampling points P i Assigning k points in the neighborhood to P n(m+1) ，P n(m+2) …，P n(m+k) Let L n ＝L n ∪{P n(m+1) ,P n(m+2) …,P n(m+k) }, and update points m = m + k;
6) Searching for newly added sampling point P n(m-k+1) ，P n(m-k+2) …，P nm If there are unclassified sampling points in the neighborhood, returning to the step 5 to add the points into the waypoint set L n If not, the classification is finished, and the step 2 is returned to;
3. for any waypoint set L i All the navigation points in (1) are sorted according to the position relation, and the navigation point coordinates from the starting point to the end point are recorded as P' i1 、P' i2 ……P' ij Update of waypoint set to L 'after completion of sorting' i . The specific algorithm is as follows (as shown in fig. 3):
1) Initializing i =1;
2) Searching a certain waypoint set L n Waypoint P in ni Neighborhood U of ε (P ni ) All waypoints within the waypoint set;
3) If the waypoint P ni Neighborhood U of ε (P ni ) If only one other waypoint exists in the navigation system, the waypoint is taken as an end point, and the step 5 is carried out; if the waypoint P ni Neighborhood U of ε (P ni ) Turning to the step 4 when the number of other waypoints is more than 1;
4) Determine whether the pair is satisfied
Is provided with
If yes, the point is an end point, and the step 5 is carried out; if not, the point is not an end point, i = i +1, and step 2 is switched to search the next point;
5) Judging P' n1 If it is not present, let P' n1 ＝P ni I = i +1 and goes to step 1; if P 'is already present' n1 Let P' nm ＝P ni And the end point searching process is finished and the step 6 is carried out. Wherein m is the number of points of the navigation point set, P' n1 And P' nm Respectively representing the starting point and the end point of the navigation point set;
6) Initializing j =1;
7) Search for P' nj Neighborhood U of ε (P' nj ) All other unclassified sample points in the sample, if point P k Satisfy to
There is d (P' ni ,P k )≤d(P' ni ,P x ) Then P is k Let j = j +1,P 'as the next point' nj ＝P k ；
8) Judging whether j = m-1 is true, if so, finishing the sorting, turning to the step 9, and if not, returning to the step 7 to continue searching for the next point;
9) Establishing ordered complete waypoint set L' n ＝{P' n1 ,P' n2 …,P' nm }。
4. And interpolating the waypoints in the waypoint set after finishing the sequencing by using a cubic spline function, and selecting a natural boundary condition. E.g. set of fitted waypoints L' i At waypoint in (1), let starting point P' i1 And terminal point P' im The second derivative of the curve is 0, and P 'is calculated according to equations (1), (2) and (3) respectively' i1 And P' i2 、P' i2 And P' i3 ……P' i(m-1) And P' im And integrating the piecewise functions into a complete function after the calculation of the cubic spline functions and storing the complete function in the route database.
5. When needing to call the air route, selecting a corresponding air route section from an air route library, and selecting the terminal point P 'of the previous air route section' (i-1)j Is the starting point of a Bessel curve and the starting point P 'of the following section of the flight path' i1 The point of intersection of the tangents at the two points is a control point, and the gap between the two route segments is fitted by referring to a mathematical model of the bezier curve in a formula 4.
The above description is intended to be illustrative of the preferred embodiment of the present invention and should not be taken as limiting the invention, but rather, the intention is to cover all modifications, equivalents, and alternatives falling within the spirit and scope of the invention.
Claims (2)
1. A method for fitting a waypoint and splicing a segmented route is characterized by comprising the following steps:
1) Converting collected waypoint data into two-dimensional planeSet of points for a surface { P 1 、P 2 ……P n -the point set contains the coordinates of the respective points;
2) According to the distance between adjacent waypoints, the waypoints are classified by a clustering algorithm 1 、L 2 ……L m G, wherein L 1 、L 2 ……L m The method is characterized in that the method is a navigation point set on 1 st to mth road sections, G is an isolated point set and does not participate in the fitting of subsequent navigation lines;
3) For any waypoint set L i All the navigation points in (1) are sorted according to the position relation, and the navigation point coordinates from the starting point to the end point are recorded as P' i1 、P′ i2 ……P′ ij Update of waypoint set to L 'after completion of sorting' i ；
4) Fitting navigation point set L 'by utilizing cubic spline interpolation function' i Middle P' i1 To P' ij Selecting natural boundary conditions for the routes between the two paths;
5) When the routes are spliced, a Bezier curve is used for fitting the clearance of two routes, and the terminal point P 'of the previous route' (i-1)j Is the starting point of a Bessel curve and the starting point P 'of the following section of the flight path' i1 The point of intersection of tangent lines at the two points is the end point of the bezier curve, and the point of intersection of tangent lines at the two points is a control point to form the bezier curve; the classification of waypoints in step 2 is based on the fact that the distance between adjacent waypoints does not exceed a threshold epsilon, and the threshold is specified to be 3 times the minimum distance between waypoints, namely epsilon =3min i ,P j ) Wherein (0 < i ≠ j < n); the clustering algorithm of the step 2 is as follows: searching unclassified sample points P i Whether there are other unclassified sampling points, P, in the neighborhood of (A) i Neighborhood of (2)
If present, P is added i And the searched sample points are classified as a new class L n+1 (ii) a If not, then P is added i Classifying the points into an isolated point set G, and repeating the steps for the points which are newly added into the classification until no unclassified sampling points exist; the sequencing algorithm of the step 3 comprises the following specific steps: judging a sampling point P from i =1 i Whether it is an endpoint, if P i Neighborhood U of ε (P i ) Only one other sample point in it, then P i Is an end point; if the sampling point P i More than one other sampling point in the neighborhood of (A) to determine whether any two sampling points and P are satisfied i The included angle between the formed vectors is less than 30 degrees, if the included angle is satisfied, P is i Is endpoint, if not satisfied, P i Not end points and verify the next point until two end points are found, respectively noting the found start and end points as P' i1 、P′ ij From origin P' i1 Searching a sampling point closest to the point in the neighborhood, recording the sampling point as a next point, and repeating the steps until the sorting is finished; step 4, cubic spline curve interpolation of a natural boundary is utilized, namely, the second derivatives of the flight line at the starting point and the ending point are 0, and the second derivatives of the curve drawn by the cubic spline interpolation are continuous; in the step 5, the route at the clearance can be fitted only by coordinates and tangent slopes at two end points of the route segment, and the original route point data does not need to be called for re-interpolation; and the second-order Bezier curve with the tangent intersection point as the control point can ensure the continuity of the first-order derivatives at the starting point and the ending point.
2. The method of claim 1 for waypoint fitting and piecewise course stitching, wherein the method comprises the steps of: and storing the segmented routes into a route database after the step 4, and splicing the routes by using the algorithm in the step 5 when the routes need to be called.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910147367.9A CN109886877B (en) | 2019-02-27 | 2019-02-27 | Method for fitting waypoints and splicing segmented routes |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910147367.9A CN109886877B (en) | 2019-02-27 | 2019-02-27 | Method for fitting waypoints and splicing segmented routes |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN109886877A CN109886877A (en) | 2019-06-14 |
| CN109886877B true CN109886877B (en) | 2023-01-13 |
Family
ID=66929785
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910147367.9A Active CN109886877B (en) | 2019-02-27 | 2019-02-27 | Method for fitting waypoints and splicing segmented routes |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109886877B (en) |
Families Citing this family (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110706356B (en) * | 2019-09-19 | 2023-06-16 | 阿波罗智联(北京)科技有限公司 | Path drawing method, path drawing device, electronic equipment and storage medium |
| CN110617817B (en) * | 2019-09-29 | 2022-04-08 | 阿波罗智联(北京)科技有限公司 | Navigation route determining method, device, equipment and storage medium |
| CN111145599B (en) * | 2019-12-17 | 2021-01-12 | 中国民用航空总局第二研究所 | Curve flight segment and error distribution establishing method |
| CN112394743B (en) * | 2020-10-12 | 2021-12-10 | 天津航天中为数据系统科技有限公司 | Method for detecting dangerous points of power tower inspection route |
| CN112836851A (en) * | 2020-12-30 | 2021-05-25 | 成都方位导向科技开发有限公司 | Hierarchical database building method for airlines |
| CN113257288B (en) * | 2021-04-29 | 2022-12-16 | 北京凯视达信息技术有限公司 | PCM audio sampling rate conversion method |
| CN113269670B (en) * | 2021-05-26 | 2023-04-18 | 海南太美航空股份有限公司 | Merging rendering method, system and device for airline and storage medium |
| CN113269669B (en) * | 2021-05-26 | 2023-05-09 | 海南太美航空股份有限公司 | Hierarchical rendering method and system for airlines |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104732588A (en) * | 2015-03-30 | 2015-06-24 | 中国测绘科学研究院 | Power line three-dimensional reconstructing method based on airborne laser radar point cloud |
| CN104951655A (en) * | 2015-06-08 | 2015-09-30 | 浙江大学 | Vehicle movement pattern mining method based on frequent pattern tree |
| CN108845569A (en) * | 2018-04-27 | 2018-11-20 | 厦门维斯云景信息科技有限公司 | Generate semi-automatic cloud method of the horizontal bend lane of three-dimensional high-definition mileage chart |
-
2019
- 2019-02-27 CN CN201910147367.9A patent/CN109886877B/en active Active
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104732588A (en) * | 2015-03-30 | 2015-06-24 | 中国测绘科学研究院 | Power line three-dimensional reconstructing method based on airborne laser radar point cloud |
| CN104951655A (en) * | 2015-06-08 | 2015-09-30 | 浙江大学 | Vehicle movement pattern mining method based on frequent pattern tree |
| CN108845569A (en) * | 2018-04-27 | 2018-11-20 | 厦门维斯云景信息科技有限公司 | Generate semi-automatic cloud method of the horizontal bend lane of three-dimensional high-definition mileage chart |
Non-Patent Citations (8)
| Title |
|---|
| 一种基于插值拟合的复杂刀具轨迹在线平滑压缩算法;陶浩等;《中国机械工程》;20180709(第13期);全文 * |
| 一种精度可控制的有理曲线拟合算法;齐树义等;《天津大学学报》;19920527(第03期);全文 * |
| 保持移动速度特征的轨迹线化简方法;杨敏等;《测绘学报》;20171215(第12期);全文 * |
| 反求工程中点云数据的曲线拟合技术;程萍等;《现代制造工程》;20060618(第06期);全文 * |
| 基于平面曲率提取产品特征线的散乱数据建模;梁佳洪等;《机床与液压》;20040930(第09期);全文 * |
| 基于放牧轨迹数据的采食区域挖掘及可视化化析;李琦等;《内蒙古科技大学学报》;20180315(第01期);全文 * |
| 基于浮动车轨迹数据的城市路网提取;王冬等;《中国科技论文》;20190215(第02期);全文 * |
| 数据拟合的Toric Bézier曲面方法;孙兰银等;《数值计算与计算机应用》;20141214(第04期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN109886877A (en) | 2019-06-14 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109886877B (en) | Method for fitting waypoints and splicing segmented routes | |
| US10627241B2 (en) | Map-centric map matching method and apparatus | |
| CN108205813B (en) | Learning network based image stylization | |
| US10019440B2 (en) | Methods and apparatus for three-dimensional (3D) sketching | |
| JP2018109621A (en) | Navigation for moving body | |
| CN108711194B (en) | Three-dimensional grid model splicing method based on cubic B spline interpolation | |
| CN108827335B (en) | Shortest path planning method based on one-way search model | |
| CN104200523A (en) | Large-scale scene three-dimensional reconstruction method for fusion of additional information | |
| CN110647693B (en) | Path recommendation method and device | |
| CN102346912A (en) | Image processing apparatus, image processing method, and program | |
| Karagiorgou et al. | A layered approach for more robust generation of road network maps from vehicle tracking data | |
| CN104794494B (en) | A kind of expansible quick method of trajectory clustering | |
| EP3794312A1 (en) | Indoor location-based service | |
| CN113190636A (en) | Offshore road network construction method and system | |
| CN115731355A (en) | SuperPoint-NeRF-based three-dimensional building reconstruction method | |
| US20220261512A1 (en) | Segmenting a 3d modeled object representing a mechanical part | |
| Garcia-Fidalgo et al. | Probabilistic appearance-based mapping and localization using visual features | |
| Regmi et al. | Video geo-localization employing geo-temporal feature learning and gps trajectory smoothing | |
| Yu et al. | Scalablemap: Scalable map learning for online long-range vectorized hd map construction | |
| Dørum | Deriving double-digitized road network geometry from probe data | |
| CN110992481B (en) | Building white mold consistency merging method based on nearest connecting line | |
| JP2013045152A (en) | Dynamic body tracker | |
| CN116363856A (en) | Method and device for generating road intersection topological structure | |
| JP2011210160A (en) | Image processing method, image processor, program and program storage medium | |
| CN114677508A (en) | Point cloud instance semantic segmentation method based on dynamic filtering and point-by-point correlation |
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 |