CN102541062A - Local path planning method for underwater autonomous aircraft - Google Patents

Abstract

The invention discloses a local path planning method for an underwater autonomous aircraft, and relates to the field of underwater autonomous aircrafts. The local path planning method comprises the following steps of: when track tracking deviation d is less than or equal to a second threshold value dmax and more than a first threshold value dmin, through the position relationship between a dynamic auxiliary circle and a directed line segment PiPi+1 of a current local target track, selecting a temporary target waypoint; computing a revised expected course PsiE; and controlling the underwater autonomous aircraft to run along the PsiE course. By the local path planning method, the local path planning of the underwater autonomous aircraft can be realized; the instantaneous interference resistance and the constant current interference resistance are higher than those in the conventional neural network method; the local path planning method has the advantages of low time consumption and space occupancy as well as simple and convenient computation; and requirements on undersea exploration and measurement by the underwater autonomous aircraft are met.

Description

A kind of local paths planning method of the ROV of autonomous type under water

Technical field

The present invention relates to autonomous type ROV field under water, particularly a kind of local paths planning method of the ROV of autonomous type under water.

Background technology

The autonomous type ROV need go along the intended target flight path in horizontal projection when the seabed is reconnoitred and measured in that the unknown marine site of appointment is carried out under water.As shown in Figure 1, the autonomous type ROV is generally taked in the horizontal projection upper edge " bow " font flight path to come the mode of flyback retrace to carry out the seabed to reconnoitre and measure under water.This flight path can be broken down into a series of directed line line segments.The autonomous type ROV is in the process of going along targetpath under water; Owing to receive interference, sensor measurement error and the departure of self etc. of ocean current; The flight path that can depart from objectives goes; Cause under water that the autonomous type ROV can not carry out the seabed scanning survey according to targetpath, thereby influence measurement effect.The autonomous type ROV must reduce and eliminate this Track In Track deviation under water, and the ability of independently carrying out local paths planning arranged, and is implemented in the Track In Track in certain accuracy rating.This is the important prerequisite of autonomous type ROV completion underwater operation under water and mission.The Track In Track method that the autonomous type ROV is commonly used under water at present has neural network method (list of references: Tang Li, AUV neural network surface level Track In Track Control Study, Harbin Engineering University's master thesis, 2009) etc.But this method instantaneous perturbation resistance ability is not very good with anti-constant ocean current interference performance: adjustable range is long; The tracking mileage is long; The time and the space expense of calculation of complex, algorithm are big, and being difficult to satisfy under water, the autonomous type ROV carries out the demand that the seabed is reconnoitred and measured.

Summary of the invention

The invention provides a kind of local paths planning method of the ROV of autonomous type under water; This method has realized that adjustable range is short, and it is short to follow the tracks of mileage, and it is little to calculate simple, time and space expense; Satisfied under water that the autonomous type ROV carries out the demand that the seabed is reconnoitred and measured, seen hereinafter for details and describe:

A kind of local paths planning method of the ROV of autonomous type under water said method comprising the steps of:

(1) the global object flight path is decomposed into a series of directed line line segment P _iP _I+1| i=1,2,3 ..., N, and successively N expection destination is stored under water in the autonomous type ROV control system assignment file;

(2) with P _iAnd P _I+1Be defined as the starting point and the end point of one section flight path, P is autonomous type ROV current location under water, and P is to straight line P _iP _I+1The vertical line section length d=| PH| is defined as current Track In Track deviation, and H is an intersection point, Be the desired course angle of targetpath,

Be uncorrected desired course angle, ψ _PHFor with the desired course angle

Vertical course, ψ _EFor revising later desired course angle;

(3) in the said ROV of autonomous type under water control system assignment file, read first target destination P _i, i=1;

(4) if i>=N, execution in step (12) then, otherwise, in the said ROV of autonomous type under water control system assignment file, read next target destination P _I+1, confirm current localized target flight path directed line segment P _iP _I+1

(5) obtain autonomous type ROV current location P and current course under water from inertial navigation system, judge whether to arrive P _I+1, if, i=i+1, execution in step (4); If not, execution in step (6);

(6) calculate the current Track In Track deviation of autonomous type ROV d under water, judge that whether said current Track In Track deviation d is smaller or equal to first threshold d _Min, if, execution in step (7); If not, execution in step (8);

(7) revise later desired course angle ψ _EEqual

Desired course does not adjust, execution in step (12);

(8) judge that whether said current Track In Track deviation d is smaller or equal to the second threshold value d _Max, and greater than first threshold d _Min, if, execution in step (9); If not, execution in step (10);

(9) calculate auxiliary dynamic radius of circle r according to first formula, be the center of circle, be that radius is set up dynamic auxiliary circle with r with the said ROV of autonomous type under water current location P, according to said dynamic auxiliary circle and said current localized target flight path directed line segment P _iP _I+1Position relation select the transient target destination, calculate and revise later desired course ψ _E

(10) said current Track In Track deviation d is greater than the said second threshold value d _Max, make desired course ψ _E=ψ _PH

(11) the said ROV of autonomous type under water of control is along ψ _EDirection running, execution in step (5) again;

(12) flow process finishes.

Said first formula is specially:

r＝f(d)，f(d)≤0，f(d _min)＝r _max，f(d _max)＝r _min，d _min＜d _max

Wherein, r _Min, r _MaxMaximal value and the minimum value of representing dynamic radius of circle r respectively.

Said according to said dynamic auxiliary circle and said current localized target flight path directed line segment P _iP _I+1Position relation select the transient target destination, calculate and revise later desired course ψ _EBe specially:

1) as dynamic auxiliary circle and straight line P _iP _I+1Mutually from and P at P _iP _I+1On projection H at P _I+1P _iExtended line on the time, select P _iAs the transient target destination, revise desired course and make

2) as dynamic auxiliary circle and straight line P _iP _I+1Mutually from and P at P _iP _I+1On projection H at P _iP _I+1When last, selected H, and revised desired course and make ψ as the transient target destination _E=ψ _PH

3) as dynamic auxiliary circle and straight line P _iP _I+1Mutually from and P at P _iP _I+1On projection H at P _iP _I+1Extended line on the time, select P _I+1As the transient target destination, revise desired course and make

4) as dynamic auxiliary circle and straight line P _iP _I+1Intersect and P at P _iP _I+1On projection H at P _I+1P _iExtended line on the time, select P _iBe the transient target destination, revise desired course and make

5) as dynamic auxiliary circle and straight line P _iP _I+1Intersect at an E, F and P at P _iP _I+1On be projected in P _iP _I+1On, and P _iP in dynamic auxiliary circle _I+1In the time of outside dynamic auxiliary circle, select near P _I+1The E point be the transient target destination, revise desired course and make ψ _E=ψ _PE

6) as dynamic auxiliary circle and straight line P _iP _I+1Intersect at an E, F and P at P _iP _I+1On be projected in P _iP _I+1On, and P _iAnd P _I+1In the time of outside dynamic auxiliary circle, selecting the E point is the transient target destination, revises desired course and makes ψ _E=ψ _PE

7) when P at P _iP _I+1On be projected in P _iP _I+1Go up and P _iAnd P _I+1In the time of in dynamic auxiliary circle, select P _I+1Point is the transient target destination, revises desired course and makes

8) as dynamic auxiliary circle and straight line P _iP _I+1Intersect, and P is at P _iP _I+1On be projected in P _iP _I+1On, and P _I+1P in dynamic auxiliary circle _iIn the time of outside dynamic auxiliary circle, select P _I+1Point is the transient target destination, revises desired course and makes

9) as dynamic auxiliary circle and straight line P _iP _I+1Intersect and P at P _iP _I+1On be projected in P _iP _I+1Extended line on, select P _I+1Point is the transient target destination, revises desired course and makes

The beneficial effect of technical scheme provided by the invention is:

The invention provides a kind of local paths planning method of the ROV of autonomous type under water, when Track In Track deviation d smaller or equal to the second threshold value d _Max, and greater than first threshold d _MinThe time, through dynamic auxiliary circle and current localized target flight path directed line segment P _iP _I+1Position relation select the transient target destination, calculate and revise later desired course ψ _E, control under water the autonomous type ROV along ψ _EDirection running; This method can effectively realize the local paths planning of autonomous type ROV under water; The ability of its opposing instantaneous interference all is better than the traditional neural networks method with the constant ocean current interference capability of opposing; And have the advantage that the space-time expense is little, calculating is easy, having satisfied under water, the autonomous type ROV carries out the demand that the seabed is reconnoitred and measured.

Description of drawings

The decomposition of the global object flight path that Fig. 1 desires to take for the ROV of autonomous type under water that prior art provides with express synoptic diagram;

Fig. 2 is provided by the invention illustrating and related definition;

Fig. 3-1 and 3-2 are the experiment effect figure of the instantaneous perturbation resistance ability of check this method provided by the invention;

Fig. 4 is the experiment effect figure of the anti-constant ocean current disturbance ability of check this method provided by the invention;

Fig. 5-1 is dynamic auxiliary circle and the position relation of interim local desired track and the synoptic diagram that corresponding new expectation destination is chosen of this method provided by the invention to 5-9;

Fig. 6 is the process flow diagram of the local paths planning method of a kind of ROV of autonomous type under water provided by the invention.

Embodiment

For making the object of the invention, technical scheme and advantage clearer, embodiment of the present invention is done to describe in detail further below in conjunction with accompanying drawing.

Short in order to realize adjustable range; The tracking mileage is short; It is little to calculate simple, time and space expense, satisfies under water that the autonomous type ROV carries out the demand that the seabed is reconnoitred and measured, referring to Fig. 6; The embodiment of the invention provides a kind of local paths planning method of the ROV of autonomous type under water, sees hereinafter for details and describes:

101: the global object flight path is decomposed into a series of directed line line segment P _iP _I+1| i=1,2,3 ..., N, and successively N expection destination is stored under water in the autonomous type ROV control system assignment file;

Wherein, referring to Fig. 1, this step is specially: will be under water the autonomous type ROV carry out each summit P of the global object flight path that subsea survey desires to take _i| i=1,2,3 ..., N is stored under water in the autonomous type ROV control system assignment file according to sequencing, N expection destination altogether, and N is a positive integer, each destination comprises the expection longitude, expect information such as latitude and goal pace.

Wherein, referring to Fig. 2, establishing under water, the autonomous type ROV goes at localized target flight path P at present _iP _I+1Near.

102: with P _iAnd P _I+1Be defined as the starting point and the end point of one section flight path, P is autonomous type ROV current location under water, and P is to straight line P _iP _I+1The vertical line section length d=| PH| is defined as the Track In Track deviation, and H is an intersection point,

Be the desired course angle of targetpath, Be uncorrected desired course angle, ψ P _HFor with the desired course angle

Vertical course, ψ _EFor revising later desired course angle;

103: from reading first target destination P in the autonomous type ROV control system assignment file under water _i, i=1;

104: if i>=N, then execution in step 112, otherwise, from reading next target destination P in the autonomous type ROV control system assignment file under water _I+1, confirm current localized target flight path directed line segment P _iP _I+1

Wherein, through first target destination P _iWith next target destination P _I+1Thereby confirm current localized target flight path directed line segment P _iP _I+1

105: obtain autonomous type ROV current location P and current course under water from inertial navigation system, judge whether to arrive P _I+1, if, i=i+1, execution in step 104; If not, execution in step 106;

Wherein, current location P generally includes longitude and latitude information, and for example: longitude is 117.00000000 °, and latitude is that 39.00000000 °, current course are 100.000 °.

106: calculate the current Track In Track deviation of autonomous type ROV d under water, judge that whether current Track In Track deviation d is smaller or equal to first threshold d _Min, if, execution in step 107; If not, execution in step 108;

107:

desired course does not adjust execution in step 112;

108: judge that whether current Track In Track deviation d is smaller or equal to the second threshold value d _Max, and greater than first threshold d _Min, if, execution in step 109; If not, execution in step 110;

109: calculate auxiliary dynamic radius of circle r according to first formula, be the center of circle, be that radius is set up dynamic auxiliary circle with r with autonomous type ROV current location P under water, according to dynamic auxiliary circle and current localized target flight path directed line segment P _iP _I+1Position relation select the transient target destination, calculate and revise later desired course ψ _E, execution in step 111;

Wherein, first formula is specially:

r＝f(d)，f(d)≤0，f(d _min)＝r _max，f(d _max)＝r _min，d _min＜d _max (1)

Wherein, r _Min, r _MaxMaximal value and the minimum value of representing dynamic radius of circle r respectively, d _Min, d _Max, r _MinAnd r _MaxSize, hydrodynamic property, the Track In Track accuracy requirement chosen according to autonomous type ROV under water confirm that when specifically realizing, the embodiment of the invention does not limit this.The function expression that satisfies (1) formula has multiple mode, and the embodiment of the invention is that example describes with a kind of form (2) formula that satisfies wherein.

$r=f\left(d\right)=(r_{\min }-r_{\max }){\left(\frac{d-d_{\min }}{d_{\max }-d_{\min }}\right)}^c+r_{\max }---\left(2\right)$

Wherein, referring to Fig. 5-1 to Fig. 5-9, dynamically auxiliary circle and directed line segment P _iP _I+1Totally 9 kinds of position relations, see hereinafter for details and describe:

1) shown in Fig. 5-1, as dynamic auxiliary circle and straight line P _iP _I+1Mutually from and P at P _iP _I+1On projection H at P _I+1P _iExtended line on the time, select P _iAs the transient target destination, revise desired course and make

Promptly control autonomous type ROV edge under water

Go, the autonomous type ROV will at first be got back to target destination P under water _iThe place begins to follow the tracks of current localized target flight path directed line segment P then _iP _I+1

2) shown in Fig. 5-2, as dynamic auxiliary circle and straight line P _iP _I+1Mutually from and P at P _iP _I+1On projection H at P _iP _I+1When last, selected H, and revised desired course and make ψ as the transient target destination _E=ψ _PH

Promptly control under water the autonomous type ROV along ψ _PHGo, Track In Track deviation d will reduce rapidly.

3) shown in Fig. 5-3, as dynamic auxiliary circle and straight line P _iP _I+1Mutually from and P at P _iP _I+1On projection H at P _iP _I+1Extended line on the time, select P _I+1As the transient target destination, revise desired course and make

Promptly control autonomous type ROV edge under water

Go, the autonomous type ROV will at first be got back to target destination P under water _I+1The place begins to follow the tracks of next targetpath.

4) shown in Fig. 5-4, as dynamic auxiliary circle and straight line P _iP _I+1Intersect and P at P _iP _I+1On projection H at P _I+1P _iExtended line on the time, select P _iBe the transient target destination, revise desired course and make

Promptly control autonomous type ROV edge under water

Go, the autonomous type ROV will at first be got back to target destination P under water _iThe place begins to follow the tracks of current localized target flight path directed line segment P then _iP _I+1

5) shown in Fig. 5-5, as dynamic auxiliary circle and straight line P _iP _I+1Intersect at an E, F and P at P _iP _I+1On be projected in P _iP _I+1On, and P _iP in dynamic auxiliary circle _I+1In the time of outside dynamic auxiliary circle, select near P _I+1The E point be the transient target destination, revise desired course and make ψ _E=ψ _PE

Promptly control under water the autonomous type ROV along ψ _PEGo, the autonomous type ROV will revert to current localized target flight path directed line segment P gradually under water _iP _I+1On.

6) shown in Fig. 5-6, as dynamic auxiliary circle and straight line P _iP _I+1Intersect at an E, F and P at P _iP _I+1On be projected in P _iP _I+1On, and P _iAnd P _I+1In the time of outside dynamic auxiliary circle, selecting the E point is the transient target destination, revises desired course and makes ψ _E=ψ _PE

Promptly control under water the autonomous type ROV along ψ _PEGo, the autonomous type ROV will revert to current localized target flight path directed line segment P gradually under water _iP _I+1On.

7) shown in Fig. 5-7, when P at P _iP _I+1On be projected in P _iP _I+1Go up and P _iAnd P _I+1In the time of in dynamic auxiliary circle, select P _I+1Point is the transient target destination, revises desired course and makes

Promptly control autonomous type ROV edge under water

Go, the autonomous type ROV will revert to current localized target flight path directed line segment P gradually under water _iP _I+1On.

8) shown in Fig. 5-8, as dynamic auxiliary circle and straight line P _iP _I+1Intersect, and P is at P _iP _I+1On be projected in P _iP _I+1On, and P _I+1P in dynamic auxiliary circle _iIn the time of outside dynamic auxiliary circle, select P _I+1Point is the transient target destination, revises desired course and makes

Promptly control autonomous type ROV edge under water

Go, the autonomous type ROV will revert to current localized target flight path directed line segment P gradually under water _iP _I+1On.

9) shown in Fig. 5-9, as dynamic auxiliary circle and straight line P _iP _I+1Intersect and P at P _iP _I+1On be projected in P _iP _I+1Extended line on, select P _I+1Point is the transient target destination, revises desired course and makes

Promptly control autonomous type ROV edge under water

Go, the autonomous type ROV will be got back to target destination P under water _I+1The place begins to follow the tracks of next targetpath.

110: current Track In Track deviation d is greater than the second threshold value d _Max, revise desired course and make desired course ψ _E=ψ _PH, execution in step 111;

111: control under water the autonomous type ROV along ψ _EDirection running, execution in step 105 again;

112: flow process finishes.

Verify the feasibility of the local paths planning method of a kind of ROV of autonomous type under water that the embodiment of the invention provides below with a concrete experiment, see hereinafter for details and describe:

Get d _Min=2.5 meters, d _Max=35 meters, r _Max=50 meters, r _Min=d _Max, c=1 adopts this method and traditional neural networks method to do two groups of experiments respectively.d _Min, d _Max, r _Max, r _Min, d _Max, c also can get other values, but is once getting fixed value in the experiment.The instantaneous perturbation resistance ability of first group of experimental check this method; The anti-constant ocean current interference performance of second group of experimental check this method.The data of every group of experiment have all been done following processing: choosing starting point A is initial point; If certain on targetpath and the actual flight path be P a bit, calculating directed line segment AP is at the earth's surface along the projection x and the y of east-west direction and North and South direction, with (x; Y) be figure for coordinate, like Fig. 3-1, Fig. 3-2, shown in Figure 4.

Table 1 has shown that the algorithm of two kinds of methods is good and bad, can find out that the algorithm space-time expense of this method is significantly less than the neural network method.In addition, this method memory consumption is that fixed value, computing time are also constant basically, and the internal memory that calculating consumed of neural net method and time will increase along with the increase of network size.

Table 1

Fig. 3-1, Fig. 3-2 and table 2 have shown tests one effect.The target setting flight path from the A point to the B point, on the way certain some feedwater down course moment of autonomous type ROV apply one 60 ° increment, make under water the autonomous type ROV flight path that departs from objectives very soon, subsequently convergence targetpath progressively again.Can find out that from Fig. 3-1 and Fig. 3-2 two kinds of methods all can realize Track In Track, but the adjustable range of the correction Track In Track deviation of this method is significantly shorter than neural net method.As shown in table 2, the maximal value of the current Track In Track deviation d of the two is close, all near 30 meters; Interfere with tracing deviation d and return to d＜d from applying _Min, the projection of distance on targetpath experienced of autonomous type ROV under water, i.e. adjustable range, this method is significantly less than the neural network method; Targetpath AB is being carried out in the tracing process, and the length of the track that experienced of autonomous type ROV is promptly followed the tracks of mileage under water, and this method is smaller than the neural network method.Repeat 23 experiments, all obtain effect same.

Table 2

Fig. 4 shown and tested two effect, the target setting flight path from the A point to the B point, 90 ° of ocean current size 0.2m/s, direction norths by east.Can find out that this method can make the course of the constantly in good time adjustment self of autonomous type ROV under water, go along targetpath all the time that the Track In Track deviation is minimum; Basic controlling is in 3.5 meters; And neural network method tracing deviation is increasing, finally disperses, and can not accomplish Track In Track.Repeat 21 experiments, all obtain effect same.In sum, the embodiment of the invention provides a kind of local paths planning method of the ROV of autonomous type under water, when Track In Track deviation d smaller or equal to the second threshold value d _Max, and greater than first threshold d _MinThe time, through dynamic auxiliary circle and current localized target flight path directed line segment P _iP _I+1Position relation select the transient target destination, calculate and revise later desired course ψ _E, control under water the autonomous type ROV along ψ _EDirection running; This method can effectively realize the local paths planning of autonomous type ROV under water; The ability of its opposing instantaneous interference all is better than the traditional neural networks method with the constant ocean current interference capability of opposing; And have the advantage that the space-time expense is little, calculating is easy, having satisfied under water, the autonomous type ROV carries out the demand that the seabed is reconnoitred and measured.

It will be appreciated by those skilled in the art that accompanying drawing is the synoptic diagram of a preferred embodiment, the invention described above embodiment sequence number is not represented the quality of embodiment just to description.

The above is merely preferred embodiment of the present invention, and is in order to restriction the present invention, not all within spirit of the present invention and principle, any modification of being done, is equal to replacement, improvement etc., all should be included within protection scope of the present invention.

Claims (3)

1. the local paths planning method of autonomous type ROV under water is characterized in that, said method comprising the steps of:

(1) the global object flight path is decomposed into a series of directed line line segment P _iP _I+1| i=1,2,3 ..., N, and successively N expection destination is stored under water in the autonomous type ROV control system assignment file;

(2) with P _iAnd P _I+1Be defined as the starting point and the end point of one section flight path, P is autonomous type ROV current location under water, and P is to straight line P _iP _I+1The length d=PH of vertical line section be defined as current Track In Track deviation, H is an intersection point,

Be the desired course angle of targetpath,

Be uncorrected desired course angle, ψ _PHFor with the desired course angle

Vertical course, ψ _EFor revising later desired course angle;

(3) in the said ROV of autonomous type under water control system assignment file, read first target destination P _i, i=1;

(4) if i>=N, execution in step (12) then, otherwise, in the said ROV of autonomous type under water control system assignment file, read next target destination P _I+1, confirm current localized target flight path directed line segment P _iP _I+1

(5) obtain autonomous type ROV current location P and current course under water from inertial navigation system, judge whether to arrive P _I+1, if, i=i+1, execution in step (4); If not, execution in step (6);

(6) calculate the current Track In Track deviation of autonomous type ROV d under water, judge that whether said current Track In Track deviation d is smaller or equal to first threshold d _Min, if, execution in step (7); If not, execution in step (8);

(7) revise later desired course angle ψ _EEqual Desired course does not adjust, execution in step (12);

(8) judge that whether said current Track In Track deviation d is smaller or equal to the second threshold value d _Max, and greater than first threshold d _Min, if, execution in step (9); If not, execution in step (10);

(9) calculate auxiliary dynamic radius of circle r according to first formula, be the center of circle, be that radius is set up dynamic auxiliary circle with r with the said ROV of autonomous type under water current location P, according to said dynamic auxiliary circle and said current localized target flight path directed line segment P _iP _I+1Position relation select the transient target destination, calculate and revise later desired course ψ _E, execution in step (11);

(10) said current Track In Track deviation d is greater than the said second threshold value d _Max, make desired course ψ _E=ψ _PH, execution in step (11);

(11) the said ROV of autonomous type under water of control is along ψ _EDirection running, execution in step (5) again;

(12) flow process finishes.

2. the local paths planning method of a kind of ROV of autonomous type under water according to claim 1 is characterized in that, said first formula is specially:

r＝f(d)，f(d)≤0，f(d _min)＝r _max，f(d _max)＝r _min，d _min＜d _max

Wherein, r _Min, r _MaxMaximal value and the minimum value of representing dynamic radius of circle r respectively.

3. the local paths planning method of a kind of ROV of autonomous type under water according to claim 1 is characterized in that, and is said according to said dynamic auxiliary circle and said current localized target flight path directed line segment P _iP _I+1Position relation select the transient target destination, calculate and revise later desired course ψ _EBe specially:

1) as dynamic auxiliary circle and straight line P _iP _I+1Mutually from and P at P _iP _I+1On projection H at P _I+1P _iExtended line on the time, select P _iAs the transient target destination, revise desired course and make

2) as dynamic auxiliary circle and straight line P _iP _I+1Mutually from and P at P _iP _I+1On projection H at P _iP _I+1When last, selected H, and revised desired course and make ψ as the transient target destination _E=ψ _PH

3) as dynamic auxiliary circle and straight line P _iP _I+1Mutually from and P at P _iP _I+1On projection H at P _iP _I+1Extended line on the time, select P _I+1As the transient target destination, revise desired course and make

4) as dynamic auxiliary circle and straight line P _iP _I+1Intersect and P at P _iP _I+1On projection H at P _I+1P _iExtended line on the time, select P _iBe the transient target destination, revise desired course and make

5) as dynamic auxiliary circle and straight line P _iP _I+1Intersect at an E, F and P at P _iP _I+1On be projected in P _iP _I+1On, and P _iP in dynamic auxiliary circle _I+1In the time of outside dynamic auxiliary circle, select near P _I+1The E point be the transient target destination, revise desired course and make ψ _E=ψ _PE

6) as dynamic auxiliary circle and straight line P _iP _I+1Intersect at an E, F and P at P _iP _I+1On be projected in P _iP _I+1On, and P _iAnd P _I+1In the time of outside dynamic auxiliary circle, selecting the E point is the transient target destination, revises desired course and makes ψ _E=ψ _PE

7) when P at P _iP _I+1On be projected in P _iP _I+1Go up and P _iAnd P _I+1In the time of in dynamic auxiliary circle, select P _I+1Point is the transient target destination, revises desired course and makes

8) as dynamic auxiliary circle and straight line P _iP _I+1Intersect, and P is at P _iP _I+1On be projected in P _iP _I+1On, and P _I+1P in dynamic auxiliary circle _iIn the time of outside dynamic auxiliary circle, select P _I+1Point is the transient target destination, revises desired course and makes

9) as dynamic auxiliary circle and straight line P _iP _I+1Intersect and P at P _iP _I+1On be projected in P _iP _I+1Extended line on, select P _I+1Point is the transient target destination, revises desired course and makes

Images