CN109084801B - Moving body path planning method under multi-station relay navigation based on space compression - Google Patents

Moving body path planning method under multi-station relay navigation based on space compression Download PDF

Info

Publication number
CN109084801B
CN109084801B CN201811182099.6A CN201811182099A CN109084801B CN 109084801 B CN109084801 B CN 109084801B CN 201811182099 A CN201811182099 A CN 201811182099A CN 109084801 B CN109084801 B CN 109084801B
Authority
CN
China
Prior art keywords
point
navigation
intersection point
arc
polar angle
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
Application number
CN201811182099.6A
Other languages
Chinese (zh)
Other versions
CN109084801A (en
Inventor
辛斌
漆鸣凤
窦丽华
陈杰
方浩
彭志红
陈晨
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beijing Institute of Technology BIT
Original Assignee
Beijing Institute of Technology BIT
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Beijing Institute of Technology BIT filed Critical Beijing Institute of Technology BIT
Priority to CN201811182099.6A priority Critical patent/CN109084801B/en
Publication of CN109084801A publication Critical patent/CN109084801A/en
Application granted granted Critical
Publication of CN109084801B publication Critical patent/CN109084801B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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/3407—Route searching; Route guidance specially adapted for specific applications
    • G01C21/3415—Dynamic re-routing, e.g. recalculating the route when the user deviates from calculated route or after detecting real-time traffic data or accidents
    • 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

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)

Abstract

The invention discloses a moving body path planning method under multi-station relay navigation based on space compression, which adopts an angle coding mode to code intermediate waypoints, can use polar angle coordinates (one variable) under a local polar coordinate system to replace two-dimensional plane coordinates (two variables) to uniquely represent the position of any point on the boundary of a handover area, and can obviously reduce problem dimensionality and calculated amount; according to the geometric relation between the navigation handover constraint threshold value and the position and effective acting radius of the navigation station, the value range of the polar-angular coordinate of the road point is compressed, partial infeasible solutions violating the navigation handover constraint are directly eliminated before the path planning algorithm is operated, the path planning space is reduced, and the path planning algorithm is facilitated to find high-quality feasible solutions more quickly.

Description

Moving body path planning method under multi-station relay navigation based on space compression
Technical Field
The invention belongs to the field of moving body path planning research, and particularly relates to a moving body path planning method under multi-station relay navigation based on space compression.
Background
The multi-station relay navigation is a joint navigation method for successively navigating a moving body by a plurality of navigation stations which are scattered at different spatial positions and can cover a larger space in a navigation action range. The moving body relies on an external navigation station to provide it with position information or control commands. Compared with other navigation modes, the self navigation mode (such as inertial navigation) of the moving body has limited precision, the satellite navigation mode is easy to damage and has high manufacturing cost, and the multi-station relay navigation mode not only has strong reliability, but also can provide path guidance for the moving body to execute remote tasks in the multi-station combined relay navigation mode, thereby effectively extending the controllable moving range of the moving body.
The problem of moving body path planning under multi-station relay navigation is a key problem to be solved for realizing the advanced navigation mode with frontier performance. The multi-station relay navigation technology imposes two special constraint conditions for path planning of a moving body: 1) the navigation range constraint is that the whole path of the motion body from the starting point to the end point is required to be positioned in the effective action range of the navigation station; 2) the navigation handover constraint, i.e. the shortest path length of the moving body in the handover area (the overlapping area of the effective action areas of the two navigation stations) must be greater than a certain value to ensure that the navigation right of the moving body is successfully handed over from one station to the other.
The first step in solving the problem of moving body path planning under multi-station relay navigation is coding. The existing coding mode is an angle coding mode, a middle waypoint is limited on a boundary arc of a handover area, a local polar coordinate system is reasonably established, and a polar angle coordinate (one variable) is adopted to replace a two-dimensional plane rectangular coordinate (two variables) to represent the position of the middle waypoint, so that the space is compressed and known, and path representation and constraint processing are facilitated.
Disclosure of Invention
The invention provides a moving body path planning method under multi-station relay navigation based on space compression, which can obviously reduce problem dimensionality and calculated amount, reduce the path planning space and be beneficial to a path planning algorithm to find a feasible solution with high quality more quickly.
A moving body path planning method under multi-station relay navigation based on space compression comprises the following steps:
step 1, inputting a starting point position, an end point position, a navigation handover constraint threshold value, positions of a plurality of navigation stations and effective acting radiuses of a moving body; the effective action range of the navigation station is a circular area; the navigation handover constraint means that in order to ensure successful navigation handover, the length of a road section of a moving body in a handover area is not less than a set threshold value, and the handover area is an overlapping area of effective action ranges of two navigation stations;
one path is formed by sequentially connecting waypoints comprising a starting point, a plurality of intermediate waypoints and a terminal point, and the waypoints are connected by adopting straight line segments; defining an intermediate waypoint comprising a starting junction and an ending junction of all the junction areas; the starting intersection point represents the position where the next navigation station starts to provide navigation information for the moving body, the ending intersection point represents the position where the previous navigation station stops providing navigation information for the moving body, and the navigation information of the moving body is provided by the two navigation stations together between the starting intersection point and the ending intersection point and the transfer of the navigation right of the moving body is completed; one path is jointly represented by the positions of all intermediate waypoints;
step 2, encoding the intermediate waypoints by adopting an angle encoding mode, wherein the starting intersection point and the ending intersection point are respectively limited on an arc inlet and an arc outlet of the intersection area; the boundary arcs of the joint area comprise an inlet arc and an outlet arc, wherein the inlet arc is the boundary arc when the moving body enters the joint area, and the outlet arc is the boundary arc when the moving body leaves the joint area;
the angle coding method for the starting junction point comprises the following steps: establishing a local polar coordinate system by taking the position of the next navigation station in the handover area as a pole, the right direction in the horizontal direction as a polar axis and the counterclockwise direction as a positive direction; the starting intersection point is limited on the arc entering of the intersection area, the polar diameter of the starting intersection point under a local polar coordinate system is the effective acting radius of the next navigation station, and the value range of the polar angle coordinate is the polar angle closed interval corresponding to the arc entering of the intersection area;
the angle coding method for ending the intersection point comprises the following steps: establishing a local polar coordinate system by taking the position of a navigation station on the handover area as a pole, the right direction of the horizontal direction as a polar axis and the counterclockwise direction as a positive direction; the ending intersection point is limited on an arc outlet of the intersection area, the polar diameter of the ending intersection point under a local polar coordinate system is the effective acting radius of the last navigation station, and the polar angle coordinate value range is a polar angle closed interval corresponding to the arc outlet of the intersection area;
step 3, compressing the value ranges of the polar angle coordinate of the starting junction and the polar angle coordinate of the ending junction according to the geometric relation between the navigation handover constraint threshold and the position and effective acting radius of the navigation station;
compressing the value range of the polar angle coordinate of the starting intersection point according to the following conditions:
setting the positions of any two adjacent navigation stations as OjAnd Oj+1Radius is R respectivelyjAnd Rj+1Cross over to point AjAnd point Bj(ii) a Point CjIs the mid-point of the incoming arc; with Oj+1Establishing a local polar coordinate system for a pole, a polar axis at the right side in the horizontal direction and a positive direction at the counterclockwise direction, and expressing a polar angle coordinate theta of a point on an arc under the local polar coordinate system; the polar angle coordinate of the starting intersection point has a value range of
Figure BDA0001824424580000031
Wherein
Figure BDA0001824424580000032
Is point AjThe polar-angle coordinate of (a) is,
Figure BDA0001824424580000033
is point BjPolar angle coordinates of (a);
case 1: the arc entry and the arc exit of the handover area are minor arcs:
case 1-A: the mid point of the incoming arc is on the line segment OjOj+1The method comprises the following steps:
if the navigation handover constraint threshold value is satisfied
Figure BDA0001824424580000034
The value range of the polar angle coordinate of the intersection point is unchanged;
if the navigation handover constraint threshold value is satisfied
Figure BDA0001824424580000035
The value range of the polar angle coordinate of the intersection point is compressed into
Figure BDA0001824424580000036
Wherein
Figure BDA0001824424580000037
Is point Fj,1The polar-angle coordinate of (a) is,
Figure BDA0001824424580000038
is a pointFj,2Polar angle coordinates of (a); at point BjAs a circle center, LjMaking a circle with a radius, the intersection point of which with the arc is point Fj,1(ii) a At point AjAs a circle center, LjMaking a circle with a radius, the intersection point of which with the arc is point Fj,2；
If the navigation handover constraint threshold value is satisfied
Figure BDA0001824424580000039
Compressing the value range of the polar angle coordinate of the intersection point into an empty set;
case 1-B: the midpoint of the arc is not in the line segment OjOj+1The method comprises the following steps:
if navigation cross-over constraint threshold value LjSatisfy the requirement of
Figure BDA0001824424580000041
The value range of the polar angle coordinate of the intersection point is unchanged; wherein the navigation handover constraint threshold LjThe shortest path length of the moving body in the cross connection area is used for ensuring the success of navigation cross connection;
if the navigation handover constraint threshold value is satisfied
Figure BDA0001824424580000042
The value range of the polar angle coordinate of the intersection point is compressed into
Figure BDA0001824424580000043
Wherein, point Gj,1Is ray BjOjThe point of intersection with the incoming arc,
Figure BDA0001824424580000044
is point Hj,1The polar-angle coordinate of (a) is,
Figure BDA0001824424580000045
is point Hj,2Polar angle coordinates of (a); at point OjAs a circle center, Lj-RjMaking a circle with a radius, the intersection point of the circle and the arc is a point Hj,1And point Hj,2；
If the navigation handover constraint threshold value is satisfied
Figure BDA0001824424580000046
The value range of the polar angle coordinate of the intersection point is compressed into
Figure BDA0001824424580000047
If the navigation handover constraint threshold value is satisfied
Figure BDA0001824424580000048
Compressing the value range of the polar angle coordinate of the intersection point into an empty set;
case 2: the arc is inferior arc, and the arc is superior arc:
if the navigation handover constraint threshold value is satisfied
Figure BDA0001824424580000049
The value range of the polar angle coordinate of the intersection point is unchanged;
if the navigation handover constraint threshold value is satisfied
Figure BDA00018244245800000410
The value range of the polar angle coordinate of the intersection point is compressed into
Figure BDA00018244245800000411
If the navigation handover constraint threshold value meets Lj>2RjThen compressing the value range of the polar angle coordinate of the intersection point into an empty set;
case 3: the arc is a major arc, and the arc is a minor arc:
case 3-A:
Figure BDA00018244245800000412
if the navigation handover constraint threshold value is satisfied
Figure BDA00018244245800000413
The value range of the polar angle coordinate of the intersection point is unchanged;
② if leadHandover constraint threshold satisfaction
Figure BDA00018244245800000414
The value range of the polar angle coordinate of the intersection point is compressed into
Figure BDA0001824424580000051
If the navigation handover constraint threshold value is satisfied
Figure BDA0001824424580000052
The value range of the polar angle coordinate of the intersection point is compressed into
Figure BDA0001824424580000053
Wherein
Figure BDA0001824424580000054
Is point Jj,2The polar-angle coordinate of (a) is,
Figure BDA0001824424580000055
is point Jj,3Polar angle coordinates of (a); at point BjAs a circle center, LjIs a circle with a radius, and the intersection point of the circle and the arc is a point Jj,1(distance point A)jNearer) and point Jj,2(distance point A)jFarther); at point AjAs a circle center, LjIs a circle with a radius, and the intersection point of the circle and the arc is a point Jj,3(distance point A)jNearer) and point Jj,4(distance point A)jFarther);
if the navigation handover constraint threshold value is satisfied
Figure BDA0001824424580000056
The value range of the polar angle coordinate of the intersection point is compressed into
Figure BDA0001824424580000057
Wherein
Figure BDA0001824424580000058
Is point Jj,1The polar-angle coordinate of (a) is,
Figure BDA0001824424580000059
is point Jj,4Polar angle coordinates of (a);
if the navigation handover constraint threshold value meets Lj>2Rj+1Then compressing the value range of the polar angle coordinate of the intersection point into an empty set;
case 3-B:
Figure BDA00018244245800000510
if the navigation handover constraint threshold value is satisfied
Figure BDA00018244245800000511
The value range of the polar angle coordinate of the intersection point is unchanged;
if the navigation handover constraint threshold value is satisfied
Figure BDA00018244245800000512
The value range of the polar angle coordinate of the intersection point is compressed into
Figure BDA00018244245800000513
If the navigation handover constraint threshold value is satisfied
Figure BDA00018244245800000514
The value range of the polar angle coordinate of the intersection point is compressed into
Figure BDA00018244245800000515
If the navigation handover constraint threshold value is satisfied
Figure BDA00018244245800000516
The value range of the polar angle coordinate of the intersection point is compressed into
Figure BDA00018244245800000517
If the navigation handover constraint threshold value meets Lj>2Rj+1Then, the junction point is startedCompressing the value range of the polar angle coordinate into an empty set;
case 3-C:
Figure BDA0001824424580000061
if the navigation handover constraint threshold value is satisfied
Figure BDA0001824424580000062
The value range of the polar angle coordinate of the intersection point is unchanged;
if the navigation handover constraint threshold value is satisfied
Figure BDA0001824424580000063
The value range of the polar angle coordinate of the intersection point is compressed into
Figure BDA0001824424580000064
If the navigation handover constraint threshold value is satisfied
Figure BDA0001824424580000065
The value range of the polar angle coordinate of the intersection point is compressed into
Figure BDA0001824424580000066
If the navigation handover constraint threshold value is satisfied
Figure BDA0001824424580000067
The value range of the polar angle coordinate of the intersection point is compressed into
Figure BDA0001824424580000068
If the navigation handover constraint threshold value meets Lj>2Rj+1Then compressing the value range of the polar angle coordinate of the intersection point into an empty set;
and compressing the value range of the polar angle coordinate of the intersection point according to the following conditions:
at point DjIs the arc-out midpoint; with OjIs extreme and horizontally to the rightEstablishing a local polar coordinate system by taking the polar axis and the anticlockwise direction as positive directions; the point on the arc is uniquely represented by a polar angle coordinate theta under the local polar coordinate system; the value range of the polar angle coordinate of the ending intersection point is
Figure BDA0001824424580000069
Case 1: the arc outlet and the arc inlet of the cross-over area are both minor arcs:
case 1-A: the midpoint of the arc is on the line segment OjOj+1The method comprises the following steps:
if the navigation handover constraint threshold value is satisfied
Figure BDA00018244245800000610
Ending the value range of the polar angle coordinate of the intersection point unchanged;
if the navigation handover constraint threshold value is satisfied
Figure BDA00018244245800000611
The value range of the polar angle coordinate of the intersection point is compressed into
Figure BDA00018244245800000612
Wherein
Figure BDA00018244245800000613
Is of point F'j,1The polar-angle coordinate of (a) is,
Figure BDA00018244245800000614
is of point F'j,2Polar angle coordinates of (a); at point AjAs a circle center, LjIs a circle with a radius, and the intersection point of the circle and the arc is point F'j,1(ii) a At point BjAs a circle center, LjIs a circle with a radius, and the intersection point of the circle and the arc is point F'j,2；
If the navigation handover constraint threshold value is satisfied
Figure BDA0001824424580000071
The value range of the polar angle coordinate of the intersection point is compressed into an empty set;
case 1-B: midpoint of arc dischargeOn line segment OjOj+1The method comprises the following steps:
if the navigation handover constraint threshold value is satisfied
Figure BDA0001824424580000072
Ending the value range of the polar angle coordinate of the intersection point unchanged;
if the navigation handover constraint threshold value is satisfied
Figure BDA0001824424580000073
The value range of the polar angle coordinate of the intersection point is compressed into
Figure BDA0001824424580000074
Wherein, point G'j,1Is ray AjOj+1The point of intersection with the outgoing arc,
Figure BDA0001824424580000075
is H'j,1The polar-angle coordinate of (a) is,
Figure BDA0001824424580000076
is H'j,2Polar angle coordinates of (a); at point Oj+1As a circle center, Lj-Rj+1Is a circle with a radius, and the intersection point of the circle and the arc is a point H'j,1And point H'j,2；
If the navigation handover constraint threshold value is satisfied
Figure BDA0001824424580000077
The value range of the polar angle coordinate of the intersection point is compressed into
Figure BDA0001824424580000078
If the navigation handover constraint threshold value is satisfied
Figure BDA0001824424580000079
The value range of the polar angle coordinate of the intersection point is compressed into an empty set;
case 2: the arc outlet is a minor arc, and the arc inlet is a major arc:
if navigationHandover constraint threshold satisfaction
Figure BDA00018244245800000710
Ending the value range of the polar angle coordinate of the intersection point unchanged;
if the navigation handover constraint threshold value is satisfied
Figure BDA00018244245800000711
The value range of the polar angle coordinate of the intersection point is compressed into
Figure BDA00018244245800000712
If the navigation handover constraint threshold value meets Lj>2Rj+1Then, the value range of the polar angle coordinate of the intersection point is compressed into an empty set;
case 3: the outgoing arc is a major arc, and the incoming arc is a minor arc:
case 3-A:
Figure BDA00018244245800000713
if the navigation handover constraint threshold value is satisfied
Figure BDA0001824424580000081
Ending the value range of the polar angle coordinate of the intersection point unchanged;
if the navigation handover constraint threshold value is satisfied
Figure BDA0001824424580000082
The value range of the polar angle coordinate of the intersection point is compressed into
Figure BDA0001824424580000083
If the navigation handover constraint threshold value is satisfied
Figure BDA0001824424580000084
The value range of the polar angle coordinate of the intersection point is compressed intoWherein
Figure BDA0001824424580000086
Is Point J'j,2The polar-angle coordinate of (a) is,
Figure BDA0001824424580000087
is Point J'j,3Polar angle coordinates of (a); at point AjAs a circle center, LjIs a circle with a radius, and the intersection point of the circle and the arc is point J'j,1And point J'j,2(ii) a At point BjAs a circle center, LjIs a circle with a radius, and the intersection point of the circle and the arc is point J'j,3And point J'j,4；
If the navigation handover constraint threshold value is satisfied
Figure BDA0001824424580000088
The value range of the polar angle coordinate of the intersection point is compressed into
Figure BDA0001824424580000089
Wherein
Figure BDA00018244245800000810
Is Point J'j,1The polar-angle coordinate of (a) is,
Figure BDA00018244245800000811
is Point J'j,4Polar angle coordinates of (a);
if the navigation handover constraint threshold value meets Lj>2RjThen, the value range of the polar angle coordinate of the intersection point is compressed into an empty set;
case 3-B:
Figure BDA00018244245800000812
if the navigation handover constraint threshold value is satisfied
Figure BDA00018244245800000813
Ending the value range of the polar angle coordinate of the intersection point unchanged;
if the navigation handover constraint threshold value is satisfied
Figure BDA00018244245800000814
The value range of the polar angle coordinate of the intersection point is compressed into
Figure BDA00018244245800000815
If navigation handover constraint threshold value LjSatisfy the requirement of
Figure BDA00018244245800000816
The value range of the polar angle coordinate of the intersection point is compressed into
Figure BDA00018244245800000817
If the navigation handover constraint threshold value is satisfied
Figure BDA00018244245800000818
The value range of the polar angle coordinate of the intersection point is compressed into
Figure BDA00018244245800000819
If the navigation handover constraint threshold value meets Lj>2RjThen, the value range of the polar angle coordinate of the intersection point is compressed into an empty set;
case 3-C:
Figure BDA0001824424580000091
if the navigation handover constraint threshold value is satisfied
Figure BDA0001824424580000092
Ending the value range of the polar angle coordinate of the intersection point unchanged;
if the navigation handover constraint threshold value is satisfied
Figure BDA0001824424580000093
The value range of the polar angle coordinate of the intersection point is compressed into
Figure BDA0001824424580000094
If the navigation handover constraint threshold value is satisfied
Figure BDA0001824424580000095
The value range of the polar angle coordinate of the intersection point is compressed into
Figure BDA0001824424580000096
If the navigation handover constraint threshold value is satisfied
Figure BDA0001824424580000097
The value range of the polar angle coordinate of the intersection point is compressed into
Figure BDA0001824424580000098
If the navigation handover constraint threshold value meets Lj>2RjThen, the value range of the polar angle coordinate of the intersection point is compressed into an empty set;
and 4, planning a path based on the compressed angle coding path planning space in the step 3, and optimizing the position of the middle waypoint.
Preferably, in the step 4, a differential evolution algorithm is adopted to perform path optimization.
The invention has the beneficial effects that:
according to the moving body path planning method under the multi-station relay navigation based on the planning space compression, an angle coding mode is adopted to code the intermediate waypoints, the polar angle coordinate (one variable) under a local polar coordinate system can be used for replacing a two-dimensional plane coordinate (two variables) to uniquely represent the position of any point on the boundary of a handover area, and the problem dimension and the calculated amount can be remarkably reduced; according to the geometric relation between the navigation handover constraint threshold value and the position and effective acting radius of the navigation station, the value range of the polar-angular coordinate of the road point is compressed, partial infeasible solutions violating the navigation handover constraint are directly eliminated before the path planning algorithm is operated, the path planning space is reduced, and the path planning algorithm is facilitated to find high-quality feasible solutions more quickly.
Drawings
FIG. 1 is a flow chart of a path planning method of the present invention;
FIG. 2 is an exemplary diagram of a path;
FIG. 3 is a schematic diagram of an angle encoding method;
FIG. 4 is a schematic view of a compression start intersection polar angle coordinate span as shown in case 1-A;
FIG. 5 is a schematic view of the compression start intersection polar angle coordinate span shown in FIG. 1-B;
FIG. 6 is a schematic diagram of a case 2 of a range of polar angle coordinates of a compression start intersection;
FIG. 7 is a schematic view of a compression start intersection polar angle coordinate span as shown in FIG. 3-A;
FIG. 8 is a schematic view of a compression start intersection polar angle coordinate span as shown in FIG. 3-B;
FIG. 9 is a schematic view of the compression start intersection polar angle coordinate span as shown in FIG. 3-C;
FIG. 10 is a schematic view of the cross point polar angle coordinate span after compression is completed 1-A;
FIG. 11 is a schematic view of the cross point polar angle coordinate span after compression is completed 1-B;
FIG. 12 is a schematic view of a case 2 of the range of polar angle coordinates of the intersection point after compression is completed;
FIG. 13 is a schematic view of the cross point polar angle coordinate span after compression is completed in case 3-A;
FIG. 14 is a schematic view of the cross point polar angle coordinate span after compression is completed in case 3-B;
FIG. 15 is a schematic view of the cross point polar angle coordinate span after compression is completed in case 3-C;
fig. 16 is a schematic view of an embodiment.
Detailed Description
The present invention will be described in detail below with reference to the accompanying drawings and examples.
The invention is realized by the following technical scheme, as shown in figure 1, comprising the following steps:
step 1, inputting the starting point position W of the moving bodysAnd finallyPoint position WdAnd a navigation handover constraint threshold value L ═ L1,L2,...,LN-1](N is the number of navigation stations), positions of a plurality of navigation stations Oi(i ═ 1,2,. cndot., N) and effective radius Ri(i ═ 1,2,. cndot., N); circular area S for effective range of navigation stationi(i ═ 1, 2.., N); the navigation handover constraint means that the moving object is in the handover area H to ensure the successful navigation handoverjThe link length in (j-1, 2.. N-1) must not be less than a set threshold L ═ L1,L2,...,LN-1]The area of the handover is the overlapping area of the effective ranges of the two navigation stations, i.e. Hj＝Sj∩Sj+1；
One path is formed by sequentially connecting waypoints comprising a starting point, a plurality of intermediate waypoints and an end point, the waypoints are connected by straight line segments, and an exemplary path is shown in figure 2; defining an intermediate waypoint including the starting point W of all handover areas2j-1(j ═ 1, 2.., N-1) and end intersection point W2j(j ═ 1,2,. N-1); the starting intersection point represents the position where the next navigation station starts to provide navigation information for the moving body, the ending intersection point represents the position where the previous navigation station stops providing navigation information for the moving body, and the navigation information of the moving body is provided by the two navigation stations together between the starting intersection point and the ending intersection point and the transfer of the navigation right of the moving body is completed; since the starting and ending positions are known, a path can be represented by the position union of all intermediate waypoints, i.e., p ═ W1,W2,...,W2N-2]；
Step 2, encoding the intermediate waypoints by adopting an angle encoding mode, wherein the starting intersection point and the ending intersection point are respectively limited on an arc inlet and an arc outlet of the intersection area; the boundary arcs of the joint area comprise an inlet arc and an outlet arc, wherein the inlet arc is the boundary arc when the moving body enters the joint area, and the outlet arc is the boundary arc when the moving body leaves the joint area; the schematic diagram of the angle encoding method is shown in FIG. 3, and the circular region SjAnd a circular region Sj+1Cross over at point AjAnd point BjThe centers of circles are respectively OjAnd Oj+1Radius is R respectivelyjAnd Rj+1；
For handover area HjStart of intersection W2j-1The angle coding method comprises the following steps: with Oj+1Establishing a local polar coordinate system for the pole, the polar axis at the right side in the horizontal direction and the positive direction at the counterclockwise direction, and W2j-1Can be expressed as
Figure BDA0001824424580000111
Due to W2j-1Is limited to arc-in the handover area
Figure BDA0001824424580000112
In the above, then
Figure BDA0001824424580000113
Thus, it is possible to provide
Figure BDA0001824424580000114
Can be located by
Figure BDA0001824424580000115
Unique representation, whose value range is the arc of the cross-over area
Figure BDA0001824424580000116
The closed polar angle interval corresponding to the local polar coordinate system, i.e. the closed polar angle interval
Figure BDA0001824424580000117
For handover area HjEnd the junction W2jThe angle coding method comprises the following steps: with OjEstablishing a local polar coordinate system for the pole, the polar axis at the right side in the horizontal direction and the positive direction at the counterclockwise direction, and W2jCan be expressed as
Figure BDA0001824424580000121
Due to W2jIs limited to arcing in the handover area
Figure BDA0001824424580000122
In the above, then
Figure BDA0001824424580000123
Thus W2jCan be located by
Figure BDA0001824424580000124
Unique representation, the value range of which is the arc of the cross-over area
Figure BDA0001824424580000125
The closed polar angle interval corresponding to the local polar coordinate system, i.e. the closed polar angle interval
Figure BDA0001824424580000126
Step 3, according to the navigation handover constraint threshold value L ═ L1,L2,...,LN-1]Compressing the value ranges of the polar angle coordinate of the starting intersection point and the polar angle coordinate of the ending intersection point according to the geometric relation between the navigation station position and the effective acting radius, and directly eliminating part of infeasible solutions violating navigation intersection constraints;
compressing the value range of the polar angle coordinate of the starting intersection point according to the following conditions:
setting the positions of any two adjacent navigation stations as OjAnd Oj+1Radius is R respectivelyjAnd Rj+1Cross over to point Aj(vector quantity)
Figure BDA0001824424580000127
Left side) and point Bj(vector quantity)
Figure BDA0001824424580000128
Right side); point CjIs an arc
Figure BDA0001824424580000129
A midpoint of (a); with Oj+1Establishing a local polar coordinate system for a pole, a polar axis at the right side in the horizontal direction and a positive direction in the counterclockwise direction, wherein points on an arc can be uniquely represented by polar angle coordinates theta under the local polar coordinate system; the polar angle coordinate of the starting intersection point has a value range of
Figure BDA00018244245800001210
Wherein
Figure BDA00018244245800001211
Is point AjThe polar-angle coordinate of (a) is,
Figure BDA00018244245800001212
is point BjPolar angle coordinates of (a);
case 1: arc entry
Figure BDA00018244245800001213
And out of arc
Figure BDA00018244245800001214
Are minor arcs:
case 1-A: cj∈OjOj+1As shown in fig. 4:
if
Figure BDA00018244245800001215
Then
Figure BDA00018244245800001216
② if
Figure BDA00018244245800001217
Then
Figure BDA00018244245800001218
Wherein
Figure BDA00018244245800001219
Is point Fj,1The polar-angle coordinate of (a) is,
Figure BDA00018244245800001220
is point Fj,2Polar angle coordinates of (a); at point BjAs a circle center, LjMaking a circle with a radius, the intersection point of which with the arc is point Fj,1(ii) a At point AjAs a circle center, LjMaking a circle with a radius, the intersection point of which with the arc is point Fj,2；
③ if
Figure BDA00018244245800001221
Then
Figure BDA00018244245800001222
Case 1-B:
Figure BDA00018244245800001223
as shown in fig. 5:
if
Figure BDA0001824424580000131
Then
Figure BDA0001824424580000132
② if
Figure BDA0001824424580000133
Then
Figure BDA0001824424580000134
Wherein, point Gj,1Is ray BjOjThe point of intersection with the incoming arc,
Figure BDA0001824424580000135
is point Hj,1The polar-angle coordinate of (a) is,
Figure BDA0001824424580000136
is point Hj,2Polar angle coordinates of (a); at point OjAs a circle center, Lj-RjMaking a circle with a radius, the intersection point of the circle and the arc is a point Hj,1(distance point A)jNearer) and point Hj,2(distance point A)jFarther);
③ if
Figure BDA0001824424580000137
Then
Figure BDA0001824424580000138
Fourthly if
Figure BDA0001824424580000139
Then
Figure BDA00018244245800001310
Case 2: arc entry
Figure BDA00018244245800001311
Is minor arc and discharge arc
Figure BDA00018244245800001312
For the major arc, as shown in fig. 6:
if
Figure BDA00018244245800001313
Then
Figure BDA00018244245800001314
② if
Figure BDA00018244245800001315
Then
Figure BDA00018244245800001316
③ if Lj>2RjThen, then
Figure BDA00018244245800001317
Case 3: arc entry
Figure BDA00018244245800001318
Is a major arc and a minor arc
Figure BDA00018244245800001319
Is a minor arc:
case 3-A:
Figure BDA00018244245800001320
as shown in fig. 7:
if
Figure BDA00018244245800001321
Then
Figure BDA00018244245800001322
② if
Figure BDA00018244245800001323
Then
Figure BDA00018244245800001324
③ if
Figure BDA00018244245800001325
Then
Figure BDA00018244245800001326
Wherein
Figure BDA00018244245800001327
Is point Jj,2The polar-angle coordinate of (a) is,
Figure BDA00018244245800001328
is point Jj,3Polar angle coordinates of (a); at point BjAs a circle center, LjIs a circle with a radius, and the intersection point of the circle and the arc is a point Jj,1(distance point A)jNearer) and point Jj,2(distance point A)jFarther); at point AjAs a circle center, LjIs a circle with a radius, and the intersection point of the circle and the arc is a point Jj,3(distance point A)jNearer) and point Jj,4(distance point A)jFarther);
fourthly if
Figure BDA00018244245800001329
Then
Figure BDA00018244245800001330
Wherein
Figure BDA00018244245800001331
Is point Jj,1The polar-angle coordinate of (a) is,
Figure BDA00018244245800001332
is point Jj,4Polar angle coordinates of (a);
wu Ruo Lj>2Rj+1Then, then
Figure BDA00018244245800001333
Case 3-B:
Figure BDA0001824424580000141
as shown in fig. 8:
if
Figure BDA0001824424580000142
Then
Figure BDA0001824424580000143
② if
Figure BDA0001824424580000144
Then
Figure BDA0001824424580000145
③ if
Figure BDA0001824424580000146
Then
Figure BDA0001824424580000147
Fourthly if
Figure BDA0001824424580000148
Then
Figure BDA0001824424580000149
Wu Ruo Lj>2Rj+1Then, then
Figure BDA00018244245800001410
Case 3-C:
Figure BDA00018244245800001411
as shown in fig. 9:
if
Figure BDA00018244245800001412
Then
Figure BDA00018244245800001413
② if
Figure BDA00018244245800001414
Then
Figure BDA00018244245800001415
③ if
Figure BDA00018244245800001416
Then
Figure BDA00018244245800001417
Fourthly if
Figure BDA00018244245800001418
Then
Figure BDA00018244245800001419
Wu Ruo Lj>2Rj+1Then, then
Figure BDA00018244245800001420
And compressing the value range of the polar angle coordinate of the intersection point according to the following conditions:
at point DjIs out of arc
Figure BDA00018244245800001421
A midpoint of (a); with OjIs established for the pole, the horizontal direction is the polar axis to the right, and the counterclockwise direction is the positive directionA local polar coordinate system is established, and points on an arc can be uniquely represented by polar angle coordinates theta under the local polar coordinate system; the value range of the polar angle coordinate of the ending intersection point is
Figure BDA00018244245800001422
Case 1: discharge arc
Figure BDA00018244245800001423
And arc
Figure BDA00018244245800001424
Are minor arcs:
case 1-A: dj∈OjOj+1As shown in fig. 10:
if
Figure BDA00018244245800001425
Then
Figure BDA00018244245800001426
② if
Figure BDA00018244245800001427
Then
Figure BDA00018244245800001428
Wherein
Figure BDA00018244245800001429
Is of point F'j,1The polar-angle coordinate of (a) is,
Figure BDA00018244245800001430
is of point F'j,2Polar angle coordinates of (a); at point AjAs a circle center, LjIs a circle with a radius, and the intersection point of the circle and the arc is point F'j,1(ii) a At point BjAs a circle center, LjIs a circle with a radius, and the intersection point of the circle and the arc is point F'j,2；
③ if
Figure BDA0001824424580000151
Then
Figure BDA0001824424580000152
Case 1-B:
Figure BDA00018244245800001534
as shown in fig. 11:
if
Figure BDA0001824424580000153
Then
Figure BDA0001824424580000154
② if
Figure BDA0001824424580000155
Then
Figure BDA0001824424580000156
Wherein, point G'j,1Is ray AjOj+1The point of intersection with the outgoing arc,
Figure BDA0001824424580000157
is H'j,1The polar-angle coordinate of (a) is,
Figure BDA0001824424580000158
is H'j,2Polar angle coordinates of (a); at point Oj+1As a circle center, Lj-Rj+1Is a circle with a radius, and the intersection point of the circle and the arc is a point H'j,1(distance point B)jNearer) and point H'j,2(distance point B)jFarther);
③ if
Figure BDA0001824424580000159
Then
Figure BDA00018244245800001510
Fourthly if
Figure BDA00018244245800001511
Then
Figure BDA00018244245800001512
Case 2: discharge arc
Figure BDA00018244245800001513
Is minor arc, entry arc
Figure BDA00018244245800001514
For the major arc, as shown in fig. 12:
if
Figure BDA00018244245800001515
Then
Figure BDA00018244245800001516
② if
Figure BDA00018244245800001517
Then
Figure BDA00018244245800001518
③ if Lj>2Rj+1Then, then
Figure BDA00018244245800001519
Case 3: discharge arc
Figure BDA00018244245800001520
Is a major arc and a minor arc
Figure BDA00018244245800001521
Is a minor arc:
case 3-A:
Figure BDA00018244245800001522
as shown in fig. 13:
if
Figure BDA00018244245800001523
Then
Figure BDA00018244245800001524
② if
Figure BDA00018244245800001525
Then
Figure BDA00018244245800001526
③ if
Figure BDA00018244245800001527
Then
Figure BDA00018244245800001528
Wherein
Figure BDA00018244245800001529
Is Point J'j,2The polar-angle coordinate of (a) is,
Figure BDA00018244245800001530
is Point J'j,3Polar angle coordinates of (a); at point AjAs a circle center, LjIs a circle with a radius, and the intersection point of the circle and the arc is point J'j,1(distance point B)jNearer) and point J'j,2(distance point B)jFarther); at point BjAs a circle center, LjIs a circle with a radius, and the intersection point of the circle and the arc is point J'j,3(distance point B)jNearer) and point J'j,4(distance point B)jFarther);
fourthly if
Figure BDA00018244245800001531
Then
Figure BDA00018244245800001532
Wherein
Figure BDA00018244245800001533
Is Point J'j,1The polar-angle coordinate of (a) is,
Figure BDA0001824424580000161
is Point J'j,4Polar angle coordinates of (a);
wu Ruo Lj>2RjThen, then
Figure BDA0001824424580000162
Case 3-B:
Figure BDA0001824424580000163
as shown in fig. 14:
if
Figure BDA0001824424580000164
Then
Figure BDA0001824424580000165
② if
Figure BDA0001824424580000166
Then
Figure BDA0001824424580000167
③ if
Figure BDA0001824424580000168
Then
Figure BDA0001824424580000169
Fourthly if
Figure BDA00018244245800001610
Then
Figure BDA00018244245800001611
Wu Ruo Lj>2RjThen, then
Figure BDA00018244245800001612
Case 3-C:
Figure BDA00018244245800001613
as shown in fig. 15:
if
Figure BDA00018244245800001614
Then
Figure BDA00018244245800001615
② if
Figure BDA00018244245800001616
Then
Figure BDA00018244245800001617
③ if
Figure BDA00018244245800001618
Then
Figure BDA00018244245800001619
Fourthly if
Figure BDA00018244245800001620
Then
Figure BDA00018244245800001621
Wu Ruo Lj>2RjThen, then
Figure BDA00018244245800001622
And 4, planning a path by adopting a Differential Evolution (DE) algorithm based on the compressed angle coding path planning space, taking the minimized total path length as a target function, and requiring to meet navigation handover constraint and optimize the position of a middle waypoint.
The following describes a space compression method for the angle coding path planning of a moving body under multi-station relay navigation with reference to an embodiment. As shown in fig. 16, the starting position of the moving body is (-0.5,0.6), the ending position is (6.5, -0.5), the moving body provides navigation information by six navigation stations to reach the ending position, and the positions of the navigation stations are, in order of navigation sequence: (0,0), (1.5,0.5), (2.5,0), (4, -0.3), (5, -0.2) and (6.3,0.2), the radii of the effective ranges are 1, 1.1, 1.3, 1.2, 0.9 and 1, respectively, and the navigation handover constraint threshold is L ═ 1.2,1.8,1.3,1.3,1], in units.
The angle-coding-based path can be represented as:
Figure BDA0001824424580000171
the value range before compression and the value range after compression of the road point polar angle coordinate are as follows:
Figure BDA0001824424580000172
in the experiment, DE-A represents a path planning algorithm based on differential evolution and angle coding, and DE-A-K represents a path planning algorithm based on differential evolution, angle coding and space compression. Firstly, setting the algorithm termination condition of DE-A and DE-A-K as the running time of 60 seconds to make them fully evolved, and using the smaller value of the objective function values of optimum solution found by DE-A and DE-A-K as the objective function value f of optimum solution*The value of the objective function is defined to be 1.05 xf or less*The solution of (2) is a better solution. Then, setting the algorithm termination conditions of DE-A and DE-A-K to find a better solution or the running time reaches 60 seconds, repeating the running for 30 times, and counting the probability of successfully finding the better solution and the average time of successfully finding the better solution of the two algorithms.
The experimental results are as follows: in 30 repeated experiments, the probability of successfully finding the better solution by DE-A is 43.33%, the average time of successfully finding the better solution is 2.2284 seconds, the probability of successfully finding the better solution by DE-A is 100%, and the average time of successfully finding the better solution is 0.9996 seconds. Obviously, DE-A-K is superior to DE-A, the angle coding path planning space compression method of the moving body under the multi-station relay navigation greatly compresses the value range of the polar angular coordinate of the road point, reduces the planning space of the path, and enables the optimization algorithm to find a feasible solution with high quality more quickly.
The embodiments disclosed above are implemented on the premise of the technical solution of the present invention, and detailed embodiments and specific operation procedures are given, but the scope of the present invention is not limited to the embodiments. From the above description, it should be understood that many modifications and substitutions may be made in the present invention, and all such modifications, equivalents, improvements and the like that are made on the basis of the technical solutions of the present invention are intended to be included in the scope of the present invention.

Claims (2)

1. A moving body path planning method under multi-station relay navigation based on space compression is characterized by comprising the following steps:
step 1, inputting a starting point position, an end point position, a navigation handover constraint threshold value, positions of a plurality of navigation stations and effective acting radiuses of a moving body; the effective action range of the navigation station is a circular area; the navigation handover constraint means that in order to ensure successful navigation handover, the length of a road section of a moving body in a handover area is not less than a set threshold value, and the handover area is an overlapping area of effective action ranges of two navigation stations;
one path is formed by sequentially connecting waypoints comprising a starting point, a plurality of intermediate waypoints and a terminal point, and the waypoints are connected by adopting straight line segments; defining an intermediate waypoint comprising a starting junction and an ending junction of all the junction areas; the starting intersection point represents the position where the next navigation station starts to provide navigation information for the moving body, the ending intersection point represents the position where the previous navigation station stops providing navigation information for the moving body, and the navigation information of the moving body is provided by the two navigation stations together between the starting intersection point and the ending intersection point and the transfer of the navigation right of the moving body is completed; one path is jointly represented by the positions of all intermediate waypoints;
step 2, encoding the intermediate waypoints by adopting an angle encoding mode, wherein the starting intersection point and the ending intersection point are respectively limited on an arc inlet and an arc outlet of the intersection area; the boundary arcs of the joint area comprise an inlet arc and an outlet arc, wherein the inlet arc is the boundary arc when the moving body enters the joint area, and the outlet arc is the boundary arc when the moving body leaves the joint area;
the angle coding method for the starting junction point comprises the following steps: establishing a local polar coordinate system by taking the position of the next navigation station in the handover area as a pole, the right direction in the horizontal direction as a polar axis and the counterclockwise direction as a positive direction; the starting intersection point is limited on the arc entering of the intersection area, the polar diameter of the starting intersection point under a local polar coordinate system is the effective acting radius of the next navigation station, and the value range of the polar angle coordinate is the polar angle closed interval corresponding to the arc entering of the intersection area;
the angle coding method for ending the intersection point comprises the following steps: establishing a local polar coordinate system by taking the position of a navigation station on the handover area as a pole, the right direction of the horizontal direction as a polar axis and the counterclockwise direction as a positive direction; the ending intersection point is limited on an arc outlet of the intersection area, the polar diameter of the ending intersection point under a local polar coordinate system is the effective acting radius of the last navigation station, and the polar angle coordinate value range is a polar angle closed interval corresponding to the arc outlet of the intersection area;
step 3, compressing the value ranges of the polar angle coordinate of the starting junction and the polar angle coordinate of the ending junction according to the geometric relation between the navigation handover constraint threshold and the position and effective acting radius of the navigation station;
compressing the value range of the polar angle coordinate of the starting intersection point according to the following conditions:
setting the positions of any two adjacent navigation stations as OjAnd Oj+1Radius is R respectivelyjAnd Rj+1Cross over to point AjAnd point Bj(ii) a Point CjIs the mid-point of the incoming arc; with Oj+1Establishing a local polar coordinate system for a pole, a polar axis at the right side in the horizontal direction and a positive direction at the counterclockwise direction, wherein a polar angle coordinate of a point on an arc under the local polar coordinate system is represented by theta; the polar angle coordinate of the starting intersection point has a value range of
Figure FDA0002623569250000021
Wherein
Figure FDA0002623569250000022
Is point AjThe polar-angle coordinate of (a) is,
Figure FDA0002623569250000023
is point BjPolar angle coordinates of (a);
case 1: the arc entry and the arc exit of the handover area are minor arcs:
case 1-A: the mid point of the incoming arc is on the line segment OjOj+1The method comprises the following steps:
if the navigation handover constraint threshold value is satisfied
Figure FDA0002623569250000024
The value range of the polar angle coordinate of the intersection point is unchanged;
if the navigation handover constraint threshold value is satisfied
Figure FDA0002623569250000025
The value range of the polar angle coordinate of the intersection point is compressed into
Figure FDA0002623569250000026
Wherein
Figure FDA0002623569250000027
Is point Fj,1The polar-angle coordinate of (a) is,
Figure FDA0002623569250000028
is point Fj,2Polar angle coordinates of (a); at point BjAs a circle center, LjMaking a circle with a radius, the intersection point of which with the arc is point Fj,1(ii) a At point AjAs a circle center, LjMaking a circle with a radius, the intersection point of which with the arc is point Fj,2；
If the navigation handover constraint threshold value is satisfied
Figure FDA0002623569250000029
Compressing the value range of the polar angle coordinate of the intersection point into an empty set;
case 1-B: the midpoint of the arc is not in the line segment OjOj+1The method comprises the following steps:
if navigation cross-over constraint threshold value LjSatisfy the requirement of
Figure FDA00026235692500000210
The value range of the polar angle coordinate of the intersection point is unchanged; wherein the navigation handover constraint threshold LjThe shortest path length of the moving body in the cross connection area is used for ensuring the success of navigation cross connection;
if the navigation handover constraint threshold value is satisfied
Figure FDA00026235692500000211
The value range of the polar angle coordinate of the intersection point is compressed into
Figure FDA00026235692500000212
Wherein, point Gj,1Is ray BjOjThe point of intersection with the incoming arc,
Figure FDA00026235692500000213
is point Hj,1The polar-angle coordinate of (a) is,
Figure FDA00026235692500000214
is point Hj,2Polar angle coordinates of (a); at point OjAs a circle center, Lj-RjMaking a circle with a radius, the intersection point of the circle and the arc is a point Hj,1And point Hj,2；
If the navigation handover constraint threshold value is satisfied
Figure FDA0002623569250000031
The value range of the polar angle coordinate of the intersection point is compressed into
Figure FDA0002623569250000032
If the navigation handover constraint threshold value is satisfied
Figure FDA0002623569250000033
Compressing the value range of the polar angle coordinate of the intersection point into an empty set;
case 2: the arc is inferior arc, and the arc is superior arc:
if the navigation handover constraint threshold value is satisfied
Figure FDA0002623569250000034
The value range of the polar angle coordinate of the intersection point is unchanged;
if the navigation handover constraint threshold value is satisfied
Figure FDA0002623569250000035
The value range of the polar angle coordinate of the intersection point is compressed into
Figure FDA0002623569250000036
If the navigation handover constraint threshold value meets Lj>2RjThen compressing the value range of the polar angle coordinate of the intersection point into an empty set;
case 3: the arc is a major arc, and the arc is a minor arc:
case 3-A:
Figure FDA0002623569250000037
if the navigation handover constraint threshold value is satisfied
Figure FDA0002623569250000038
The value range of the polar angle coordinate of the intersection point is unchanged;
if the navigation handover constraint threshold value is satisfied
Figure FDA0002623569250000039
The value range of the polar angle coordinate of the intersection point is compressed into
Figure FDA00026235692500000310
If the navigation handover constraint threshold value is satisfied
Figure FDA00026235692500000311
The value range of the polar angle coordinate of the intersection point is compressed into
Figure FDA00026235692500000312
Wherein
Figure FDA00026235692500000313
Is point Jj,2The polar-angle coordinate of (a) is,
Figure FDA00026235692500000314
is point Jj,3Polar angle coordinates of (a); at point BjAs a circle center, LjIs a circle with a radius, and the intersection point of the circle and the arc is a point Jj,1And point Jj,2(ii) a At point AjAs a circle center, LjIs a circle with a radius, and the intersection point of the circle and the arc is a point Jj,3And point Jj,4；
If the navigation handover constraint threshold value is satisfied
Figure FDA00026235692500000315
The value range of the polar angle coordinate of the intersection point is compressed into
Figure FDA0002623569250000041
Wherein
Figure FDA0002623569250000042
Is point Jj,1The polar-angle coordinate of (a) is,
Figure FDA0002623569250000043
is point Jj,4Polar angle coordinates of (a);
if the navigation handover constraint threshold value meets Lj>2Rj+1Then compressing the value range of the polar angle coordinate of the intersection point into an empty set;
case 3-B:
Figure FDA0002623569250000044
if the navigation handover constraint threshold value is satisfied
Figure FDA0002623569250000045
The value range of the polar angle coordinate of the intersection point is unchanged;
if the navigation handover constraint threshold value is satisfied
Figure FDA0002623569250000046
The value range of the polar angle coordinate of the intersection point is compressed into
Figure FDA0002623569250000047
If the navigation handover constraint threshold value is satisfied
Figure FDA0002623569250000048
The value range of the polar angle coordinate of the intersection point is compressed into
Figure FDA0002623569250000049
If the navigation handover constraint threshold value is satisfied
Figure FDA00026235692500000410
The value range of the polar angle coordinate of the intersection point is compressed into
Figure FDA00026235692500000411
If the navigation handover constraint threshold value meets Lj>2Rj+1Then compressing the value range of the polar angle coordinate of the intersection point into an empty set;
case 3-C:
Figure FDA00026235692500000412
if the navigation handover constraint threshold value is satisfied
Figure FDA00026235692500000413
The value range of the polar angle coordinate of the intersection point is unchanged;
if the navigation handover constraint threshold value is satisfied
Figure FDA00026235692500000414
The value range of the polar angle coordinate of the intersection point is compressed into
Figure FDA00026235692500000415
If the navigation handover constraint threshold value is satisfied
Figure FDA00026235692500000416
The value range of the polar angle coordinate of the intersection point is compressed into
Figure FDA00026235692500000417
If the navigation handover constraint threshold value is satisfied
Figure FDA00026235692500000418
The value range of the polar angle coordinate of the intersection point is compressed into
Figure FDA0002623569250000051
If the navigation handover constraint threshold value meets Lj>2Rj+1Then compressing the value range of the polar angle coordinate of the intersection point into an empty set;
and compressing the value range of the polar angle coordinate of the intersection point according to the following conditions:
at point DjIs the arc-out midpoint; with OjEstablishing a local polar coordinate system for a pole, a polar axis at the right side in the horizontal direction and a positive direction in the counterclockwise direction; the point on the arc is uniquely represented by a polar angle coordinate theta under the local polar coordinate system; the value range of the polar angle coordinate of the ending intersection point is
Figure FDA0002623569250000052
Case 1: the arc outlet and the arc inlet of the cross-over area are both minor arcs:
case 1-A: the midpoint of the arc is on the line segment OjOj+1The method comprises the following steps:
if the navigation handover constraint threshold value is satisfied
Figure FDA0002623569250000053
Ending the value range of the polar angle coordinate of the intersection point unchanged;
if the navigation handover constraint threshold value is satisfied
Figure FDA0002623569250000054
The value range of the polar angle coordinate of the intersection point is compressed into
Figure FDA0002623569250000055
Wherein
Figure FDA0002623569250000056
Is of point F'j,1The polar-angle coordinate of (a) is,
Figure FDA0002623569250000057
is of point F'j,2Polar angle coordinates of (a); at point AjAs a circle center, LjIs a circle with a radius, and the intersection point of the circle and the arc is point F'j,1(ii) a At point BjAs a circle center, LjIs a circle with a radius, and the intersection point of the circle and the arc is point F'j,2；
If the navigation handover constraint threshold value is satisfied
Figure FDA0002623569250000058
The value range of the polar angle coordinate of the intersection point is compressed into an empty set;
case 1-B: the midpoint of the arc is not in the line segment OjOj+1The method comprises the following steps:
if the navigation handover constraint threshold value is satisfied
Figure FDA0002623569250000059
Ending the value range of the polar angle coordinate of the intersection point unchanged;
if the navigation handover constraint threshold value is satisfied
Figure FDA00026235692500000510
The value range of the polar angle coordinate of the intersection point is compressed into
Figure FDA00026235692500000511
Wherein, point G'j,1Is ray AjOj+1The point of intersection with the outgoing arc,
Figure FDA00026235692500000512
is H'j,1The polar-angle coordinate of (a) is,
Figure FDA00026235692500000513
is H'j,2Polar angle coordinates of (a); at point Oj+1As a circle center, Lj-Rj+1Is a circle with a radius, and the intersection point of the circle and the arc is a point H'j,1And point H'j,2；
If the navigation handover constraint threshold value is satisfied
Figure FDA0002623569250000061
The value range of the polar angle coordinate of the intersection point is compressed into
Figure FDA0002623569250000062
If the navigation handover constraint threshold value is satisfied
Figure FDA0002623569250000063
The value range of the polar angle coordinate of the intersection point is compressed into an empty set;
case 2: the arc outlet is a minor arc, and the arc inlet is a major arc:
if the navigation handover constraint threshold value is satisfied
Figure FDA0002623569250000064
Ending the value range of the polar angle coordinate of the intersection point unchanged;
if the navigation handover constraint threshold value is satisfied
Figure FDA0002623569250000065
The value range of the polar angle coordinate of the intersection point is compressed into
Figure FDA0002623569250000066
If the navigation handover constraint threshold value meets Lj>2Rj+1Then, the value range of the polar angle coordinate of the intersection point is compressed into an empty set;
case 3: the outgoing arc is a major arc, and the incoming arc is a minor arc:
case 3-A:
Figure FDA0002623569250000067
if the navigation handover constraint threshold value is satisfied
Figure FDA0002623569250000068
Ending the value range of the polar angle coordinate of the intersection point unchanged;
if the navigation handover constraint threshold value is satisfied
Figure FDA0002623569250000069
The value range of the polar angle coordinate of the intersection point is compressed into
Figure FDA00026235692500000610
If the navigation handover constraint threshold value is satisfied
Figure FDA00026235692500000611
The value range of the polar angle coordinate of the intersection point is compressed into
Figure FDA00026235692500000612
Wherein
Figure FDA00026235692500000613
Is Point J'j,2The polar-angle coordinate of (a) is,
Figure FDA00026235692500000614
is Point J'j,3Polar angle coordinates of (a); at point AjAs a circle center, LjIs a circle with a radius, and the intersection point of the circle and the arc is point J'j,1And point J'j,2(ii) a At point BjAs a circle center, LjIs a circle with a radius, and the intersection point of the circle and the arc is point J'j,3And point J'j,4；
If the navigation handover constraint threshold value is satisfied
Figure FDA0002623569250000071
The value range of the polar angle coordinate of the intersection point is compressed into
Figure FDA0002623569250000072
Wherein
Figure FDA0002623569250000073
Is Point J'j,1The polar-angle coordinate of (a) is,
Figure FDA0002623569250000074
is Point J'j,4Polar angle coordinates of (a);
if the navigation handover constraint threshold value meets Lj>2RjThen, the value range of the polar angle coordinate of the intersection point is compressed into an empty set;
case 3-B:
Figure FDA0002623569250000075
if the navigation handover constraint threshold value is satisfied
Figure FDA0002623569250000076
Ending the value range of the polar angle coordinate of the intersection pointThe circumference is not changed;
if the navigation handover constraint threshold value is satisfied
Figure FDA0002623569250000077
The value range of the polar angle coordinate of the intersection point is compressed into
Figure FDA0002623569250000078
If navigation handover constraint threshold value LjSatisfy the requirement of
Figure FDA0002623569250000079
The value range of the polar angle coordinate of the intersection point is compressed into
Figure FDA00026235692500000710
If the navigation handover constraint threshold value is satisfied
Figure FDA00026235692500000711
The value range of the polar angle coordinate of the intersection point is compressed into
Figure FDA00026235692500000712
If the navigation handover constraint threshold value meets Lj>2RjThen, the value range of the polar angle coordinate of the intersection point is compressed into an empty set;
case 3-C:
Figure FDA00026235692500000713
if the navigation handover constraint threshold value is satisfied
Figure FDA00026235692500000714
Ending the value range of the polar angle coordinate of the intersection point unchanged;
if the navigation handover constraint threshold value is satisfied
Figure FDA00026235692500000715
The value range of the polar angle coordinate of the intersection point is compressed into
Figure FDA00026235692500000716
If the navigation handover constraint threshold value is satisfied
Figure FDA00026235692500000717
The value range of the polar angle coordinate of the intersection point is compressed into
Figure FDA00026235692500000718
If the navigation handover constraint threshold value is satisfied
Figure FDA0002623569250000081
The value range of the polar angle coordinate of the intersection point is compressed into
Figure FDA0002623569250000082
If the navigation handover constraint threshold value meets Lj>2RjThen, the value range of the polar angle coordinate of the intersection point is compressed into an empty set;
and 4, planning a path based on the compressed angle coding path planning space in the step 3, and optimizing the position of the middle waypoint.
2. The method for planning the path of the moving body under the multi-station relay navigation based on the space compression as claimed in claim 1, wherein in the step 4, a differential evolution algorithm is adopted for path optimization.
CN201811182099.6A 2018-10-10 2018-10-10 Moving body path planning method under multi-station relay navigation based on space compression Active CN109084801B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201811182099.6A CN109084801B (en) 2018-10-10 2018-10-10 Moving body path planning method under multi-station relay navigation based on space compression

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201811182099.6A CN109084801B (en) 2018-10-10 2018-10-10 Moving body path planning method under multi-station relay navigation based on space compression

Publications (2)

Publication Number Publication Date
CN109084801A CN109084801A (en) 2018-12-25
CN109084801B true CN109084801B (en) 2020-11-10

Family

ID=64843463

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201811182099.6A Active CN109084801B (en) 2018-10-10 2018-10-10 Moving body path planning method under multi-station relay navigation based on space compression

Country Status (1)

Country Link
CN (1) CN109084801B (en)

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102128627A (en) * 2010-01-19 2011-07-20 北京四维图新科技股份有限公司 Difference analysis method and device for electronic map
CN106643733A (en) * 2016-12-30 2017-05-10 北京理工大学 Moving body route planning method for relay navigation among multiple navigation stations
CN106679667A (en) * 2016-12-30 2017-05-17 北京理工大学 Method for planning paths of moving bodies for relay navigation of multiple navigation stations

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20130231862A1 (en) * 2011-06-03 2013-09-05 Microsoft Corporation Customizable route planning

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102128627A (en) * 2010-01-19 2011-07-20 北京四维图新科技股份有限公司 Difference analysis method and device for electronic map
CN106643733A (en) * 2016-12-30 2017-05-10 北京理工大学 Moving body route planning method for relay navigation among multiple navigation stations
CN106679667A (en) * 2016-12-30 2017-05-17 北京理工大学 Method for planning paths of moving bodies for relay navigation of multiple navigation stations

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
迁移工作流系统中迁移实例的路径规划研究;程杰等;《计算机科学与探索》;20081215(第06期);658-665 *

Also Published As

Publication number Publication date
CN109084801A (en) 2018-12-25

Similar Documents

Publication Publication Date Title
CN106643733B (en) Towards the movable body paths planning method of more guidance station relays navigation
CN106679667B (en) Towards the movable body paths planning method of more guidance station relays navigation
CN108762268A (en) More AGV collision-free Trajectory Planning of Welding algorithms
CN110567477A (en) Path planning method and device based on improved A-x algorithm and robot
CN111595343B (en) Unmanned aerial vehicle track planning method based on positioning error correction
CN109084801B (en) Moving body path planning method under multi-station relay navigation based on space compression
CN110285802A (en) Quick Extended random tree path smoothing method
CN112356033B (en) Mechanical arm path planning method integrating low-difference sequence and RRT algorithm
CN114200931B (en) Mobile robot path smoothing method based on B spline curve optimization
Wang et al. Ganet: Goal area network for motion forecasting
JP2020518925A (en) System and method for controlling vehicle motion
CN110728751A (en) Construction method of indoor 3D point cloud semantic map
CN109947135A (en) It is a kind of net connection unmanned plane flight path determine method and system
CN113822251B (en) Ground reconnaissance robot gesture control system and control method based on binocular vision
CN113189992B (en) Group intelligent obstacle avoidance method based on local orientation
CN109084800B (en) Moving body path planning method under multi-station relay navigation based on space compression
CN113325852A (en) Leader follower mode-based control method for formation change of multiple intelligent agents in advancing process
CN109084802B (en) Moving body path planning method under multi-station relay navigation based on hybrid coding
CN115592675B (en) Control system based on mobile beverage preparation mechanical arm
CN111121779B (en) Real-time detection method for flight area where unmanned aerial vehicle is located
Yong et al. Real-time dynamic path planning for Dubins' nonholonomic robot
Salaris et al. Shortest paths for wheeled robots with limited field-of-view: introducing the vertical constraint
Huang et al. An improved discriminator for GAN-based trajectory prediction models
Wu et al. CMP: Cooperative Motion Prediction with Multi-Agent Communication
Dai et al. On existence conditions of a class of continuous curvature paths

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