CN102541062B - A Local Path Planning Method for Underwater Autonomous Vehicles - 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 aircraft of autonomous type under water

Technical field

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

Background technology

Autonomous type aircraft is reconnoitred and need in horizontal projection, along intended target flight path, be travelled when measuring the unknown marine site of appointment being carried out to seabed under water.As shown in Figure 1, autonomous type aircraft is generally taked in horizontal projection to come the mode of flyback retrace to carry out seabed along " bow " font flight path and is reconnoitred and measure under water.This flight path can be broken down into a series of directed line line segments.Autonomous type aircraft is in the process of travelling along targetpath under water, owing to being subject to interference, sensor measurement error and the departure of self etc. of ocean current, the flight path that can depart from objectives travels, cause autonomous type aircraft under water not carry out seabed scanning survey according to targetpath, thereby affect measurement effect.Autonomous type aircraft must reduce and eliminate this Track In Track deviation under water, have the ability of independently carrying out local paths planning, realizes the Track In Track in certain accuracy rating.This is that autonomous type aircraft completes the important prerequisite of underwater operation and mission under water.The conventional Track In Track method of autonomous type aircraft has neural network (list of references: Tang Li, AUV neural network surface level Track In Track is controlled research, Harbin Engineering University's master thesis, 2009) etc. under water at present.But the method instantaneous perturbation resistance ability and anti-constant ocean current interference performance are not very good: adjustable range is long, tracking mileage is long, time and the space expense of calculation of complex, algorithm are large, are difficult to meet under water autonomous type aircraft and carry out seabed and reconnoitre and the demand of measuring.

Summary of the invention

The invention provides a kind of local paths planning method of the aircraft of autonomous type under water, it is short that this method has realized adjustable range, follows the tracks of mileage short, calculates simple, time and space expense little, met under water autonomous type aircraft and carried out seabed and reconnoitre and the demand of measuring, described below:

A local paths planning method for autonomous type aircraft under water, said method comprising the steps of:

(1) 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 autonomous type aircraft control system assignment file;

(2) by P _iand P _i+1be defined as starting point and the end point of one section of flight path, P is autonomous type aircraft current location under water, and P is to straight line P _ip _i+1vertical line section length d=| PH| is defined as current Track In Track deviation, and H is intersection point,

for the desired course angle of targetpath,

for uncorrected desired course angle, ψ _pHfor with desired course angle

vertical course, ψ _efor revising later desired course angle;

(3) from the described aircraft of autonomous type under water control system assignment file, read first aim destination P _i, i=1;

(4) if i>=N performs step (12), otherwise, from the described aircraft of autonomous type under water control system assignment file, read next target destination P _i+1, determine current localized target flight path directed line segment P _ip _i+1;

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

(6) calculate the current Track In Track deviation of autonomous type aircraft d under water, judge whether described current Track In Track deviation d is less than or equal to first threshold d _min, if so, execution step (7); If not, execution step (8);

(7) revise later desired course angle ψ _eequal

desired course does not adjust, execution step (12);

(8) judge whether described current Track In Track deviation d is less than or equal to Second Threshold d _max, and be greater than first threshold d _min, if so, execution step (9); If not, execution step (10);

(9) according to the first formula, calculate auxiliary dynamic radius of circle r, take the described aircraft of autonomous type under water current location P as the center of circle, the r of take sets up dynamic auxiliary circle as radius, according to described dynamic auxiliary circle and described current localized target flight path directed line segment P _ip _i+1position relationship select transient target destination, calculate and revise later desired course ψ _e;

(10) described current Track In Track deviation d is greater than described Second Threshold d _max, make desired course ψ _e=ψ _pH;

(11) control described under water autonomous type aircraft along ψ _edirection running, re-executes step (5);

(12) flow process finishes.

Described the 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 _maxthe maximal value and the minimum value that represent respectively dynamic radius of circle r.

Described according to described dynamic auxiliary circle and described current localized target flight path directed line segment P _ip _i+1position relationship select transient target destination, calculate and revise later desired course ψ _ebe specially:

1) as dynamic auxiliary circle and straight line P _ip _i+1from and P at P _ip _i+1on projection H at P _i+1p _iextended line on time, select P _ias transient target destination, revise desired course and make

2) as dynamic auxiliary circle and straight line P _ip _i+1from and P at P _ip _i+1on projection H at P _ip _i+1when upper, selected H as transient target destination, and revised desired course and make ψ _e=ψ _pH;

3) as dynamic auxiliary circle and straight line P _ip _i+1from and P at P _ip _i+1on projection H at P _ip _i+1extended line on time, select P _i+1as 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 time, select P _ifor 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+1upper, and P _ip in dynamic auxiliary circle _i+1in the time of outside dynamic auxiliary circle, select near P _i+1e point be 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+1upper, and P _iand P _i+1in the time of outside dynamic auxiliary circle, selecting E point is transient target destination, revises desired course and makes ψ _e=ψ _pE;

7) when P is 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 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+1upper, and P _i+1p in dynamic auxiliary circle _iin the time of outside dynamic auxiliary circle, select P _i+1point is 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 transient target destination, revises desired course and makes

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

The local paths planning method that the invention provides a kind of aircraft of autonomous type under water, when Track In Track deviation, d is less than or equal to Second Threshold d _max, and be greater than first threshold d _mintime, by dynamic auxiliary circle and current localized target flight path directed line segment P _ip _i+1position relationship select transient target destination, calculate and revise later desired course ψ _e, control under water autonomous type aircraft along ψ _edirection running, this method can effectively realize the local paths planning of autonomous type aircraft under water, the ability that the ability of its opposing instantaneous interference and the constant ocean current of opposing disturb is all better than traditional neural network, and have advantages of that space-time expense is little, it is easy to calculate, met under water autonomous type aircraft and carried out seabed and reconnoitre and the demand of measuring.

Accompanying drawing explanation

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

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

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

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

Dynamic auxiliary circle and the position relationship of interim local desired track and the schematic diagram that corresponding new expectation destination is chosen that Fig. 5-1 is 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 aircraft of autonomous type under water provided by the invention.

Embodiment

For making the object, technical solutions and advantages of the present invention clearer, below in conjunction with accompanying drawing, embodiment of the present invention is described further in detail.

Short in order to realize adjustable range, tracking mileage is short, calculate simple, time and space expense little, satisfied autonomous type aircraft is under water carried out seabed and is reconnoitred and the demand of measuring, referring to Fig. 6, the embodiment of the present invention provides a kind of local paths planning method of the aircraft of autonomous type under water, described below:

101: 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 autonomous type aircraft control system assignment file;

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

Wherein, referring to Fig. 2, establish autonomous type aircraft under water and travel at present at localized target flight path P _ip _i+1near.

102: by P _iand P _i+1be defined as starting point and the end point of one section of flight path, P is autonomous type aircraft current location under water, and P is to straight line P _ip _i+1vertical line section length d=| PH| is defined as Track In Track deviation, and H is intersection point, for the desired course angle of targetpath, for uncorrected desired course angle, ψ P _hfor with desired course angle

vertical course, ψ _efor revising later desired course angle;

103: from reading first aim destination P autonomous type aircraft control system assignment file under water _i, i=1;

104: if i>=N performs step 112, otherwise, from reading next target destination P autonomous type aircraft control system assignment file under water _i+1, determine current localized target flight path directed line segment P _ip _i+1;

Wherein, by first aim destination P _iwith next target destination P _i+1thereby determine current localized target flight path directed line segment P _ip _i+1.

105: from inertial navigation system, obtain autonomous type aircraft current location P and current course under water, judge whether to arrive P _i+1, if so, i=i+1, performs step 104; If not, perform step 106;

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

106: calculate the current Track In Track deviation of autonomous type aircraft d under water, judge whether current Track In Track deviation d is less than or equal to first threshold d _min, if so, perform step 107; If not, perform step 108;

107:

desired course does not adjust, and performs step 112;

108: judge whether current Track In Track deviation d is less than or equal to Second Threshold d _max, and be greater than first threshold d _min, if so, perform step 109; If not, perform step 110;

109: according to the first formula, calculate auxiliary dynamic radius of circle r, take under water autonomous type aircraft current location P as the center of circle, the r of take sets up dynamic auxiliary circle as radius, according to dynamic auxiliary circle and current localized target flight path directed line segment P _ip _i+1position relationship select transient target destination, calculate and revise later desired course ψ _e, perform step 111;

Wherein, the 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 _maxthe maximal value and the minimum value that represent respectively dynamic radius of circle r, d _min, d _max, r _minand r _maxsize, hydrodynamic property, the Track In Track accuracy requirement chosen according to autonomous type aircraft under water determine, during specific implementation, the embodiment of the present invention does not limit this.The function expression that meets (1) formula has various ways, and the embodiment of the present invention be take and met a kind of form (2) formula wherein and describe as example.

$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 relationships, described below:

1) as shown in Fig. 5-1, as dynamic auxiliary circle and straight line P _ip _i+1from and P at P _ip _i+1on projection H at P _i+1p _iextended line on time, select P _ias transient target destination, revise desired course and make

Control autonomous type aircraft edge under water travel, first autonomous type aircraft will get back to target destination P under water _iplace, then starts to follow the tracks of current localized target flight path directed line segment P _ip _i+1.

2) as shown in Fig. 5-2, as dynamic auxiliary circle and straight line P _ip _i+1from and P at P _ip _i+1on projection H at P _ip _i+1when upper, selected H as transient target destination, and revised desired course and make ψ _e=ψ _pH;

Control under water autonomous type aircraft along ψ _pHtravel, Track In Track deviation d will reduce rapidly.

3) as shown in Fig. 5-3, as dynamic auxiliary circle and straight line P _ip _i+1from and P at P _ip _i+1on projection H at P _ip _i+1extended line on time, select P _i+1as transient target destination, revise desired course and make

Control autonomous type aircraft edge under water

travel, first autonomous type aircraft will get back to target destination P under water _i+1place, starts to follow the tracks of next targetpath.

4) as 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 time, select P _ifor transient target destination, revise desired course and make

Control autonomous type aircraft edge under water

travel, first autonomous type aircraft will get back to target destination P under water _iplace, then starts to follow the tracks of current localized target flight path directed line segment P _ip _i+1.

5) as 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+1upper, and P _ip in dynamic auxiliary circle _i+1in the time of outside dynamic auxiliary circle, select near P _i+1e point be transient target destination, revise desired course and make ψ _e=ψ _pE;

Control under water autonomous type aircraft along ψ _pEtravel, autonomous type aircraft will revert to current localized target flight path directed line segment P gradually under water _ip _i+1on.

6) as 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+1upper, and P _iand P _i+1in the time of outside dynamic auxiliary circle, selecting E point is transient target destination, revises desired course and makes ψ _e=ψ _pE;

Control under water autonomous type aircraft along ψ _pEtravel, autonomous type aircraft will revert to current localized target flight path directed line segment P gradually under water _ip _i+1on.

7) as shown in Fig. 5-7, when P is 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 transient target destination, revises desired course and makes

Control autonomous type aircraft edge under water travel, autonomous type aircraft will revert to current localized target flight path directed line segment P gradually under water _ip _i+1on.

8) as 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+1upper, and P _i+1p in dynamic auxiliary circle _iin the time of outside dynamic auxiliary circle, select P _i+1point is transient target destination, revises desired course and makes

Control autonomous type aircraft edge under water

travel, autonomous type aircraft will revert to current localized target flight path directed line segment P gradually under water _ip _i+1on.

9) as 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 transient target destination, revises desired course and makes

Control autonomous type aircraft edge under water

travel, autonomous type aircraft will be got back to target destination P under water _i+1place, starts to follow the tracks of next targetpath.

110: current Track In Track deviation d is greater than Second Threshold d _max, revise desired course and make desired course ψ _e=ψ _pH, perform step 111;

111: control under water autonomous type aircraft along ψ _edirection running, re-executes step 105;

112: flow process finishes.

With a concrete experiment, verify the feasibility of the local paths planning method of a kind of aircraft of autonomous type under water that the embodiment of the present invention provides below, described below:

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 net method to do respectively two groups of experiments.D _min, d _max, r _max, r _min, d _max, c also can get other values, but once in experiment, is getting fixed value.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 actual flight path is some P, calculate directed line segment AP at the earth's surface along projection x and the y of east-west direction and North and South direction, with (x, y) for coordinate is figure, as shown in Fig. 3-1, Fig. 3-2, Fig. 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 neural network.In addition, this method memory consumption is that fixed value, computing time are also substantially constant, and the internal memory that the calculating of neural net method consumes and time will increase along with the increase of network size.

Table 1

Fig. 3-1, Fig. 3-2 and table 2 have shown the effect of testing.Target setting flight path is from A point to B point, on the way certain some course moment of the lower autonomous type aircraft of feedwater apply the increment of 60 °, make under water the autonomous type aircraft flight path that departs from objectives very soon, subsequently convergence targetpath progressively again.From Fig. 3-1 and Fig. 3-2, can find out, 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 approaches 30 meters; From applying, interfere with tracing deviation d and return to d < d _min, the projection of the distance that autonomous type aircraft experiences under water on targetpath, i.e. adjustable range, this method is significantly less than neural network; Targetpath AB is being carried out in tracing process, and the length of the track that autonomous type aircraft experiences under water, follows the tracks of mileage, and this method is smaller than neural network.Repeat 23 experiments, all obtain effect same.

Table 2

Fig. 4 has shown the effect of testing two, and target setting flight path is from A point to B point, 90 ° of ocean current size 0.2m/s, direction norths by east.Can find out that this method can make autonomous type aircraft under water constantly adjust self course in good time, travel all the time along targetpath, Track In Track deviation is minimum, basic controlling is in 3.5 meters, and neural network tracing deviation is increasing, finally disperse, can not complete Track In Track.Repeat 21 experiments, all obtain effect same.In sum, the embodiment of the present invention provides a kind of local paths planning method of the aircraft of autonomous type under water, and when Track In Track deviation, d is less than or equal to Second Threshold d _max, and be greater than first threshold d _mintime, by dynamic auxiliary circle and current localized target flight path directed line segment P _ip _i+1position relationship select transient target destination, calculate and revise later desired course ψ _e, control under water autonomous type aircraft along ψ _edirection running, this method can effectively realize the local paths planning of autonomous type aircraft under water, the ability that the ability of its opposing instantaneous interference and the constant ocean current of opposing disturb is all better than traditional neural network, and have advantages of that space-time expense is little, it is easy to calculate, met under water autonomous type aircraft and carried out seabed and reconnoitre and the demand of measuring.

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

The foregoing is only preferred embodiment of the present invention, in order to limit the present invention, within the spirit and principles in the present invention not all, any modification of doing, be equal to replacement, improvement etc., within all should being included in protection scope of the present invention.

Claims (1)

1. A local path planning method for an underwater autonomous vehicle, characterized in that the method comprises the following steps: (1) Decompose the global target track into a series of directed straight line segments P _i P _i+1 |i=1, 2, 3,..., N, and sequentially store N expected waypoints in underwater autonomous navigation In the controller control system task file; (2) Define P _i and P _i+1 as the starting point and end point of a track, P is the current position of the underwater autonomous vehicle, and the length of the perpendicular segment from P to the straight line P _i P _i+1 is d= |PH| is defined as the current track tracking deviation, H is the vertical foot, is the desired heading angle of the target track, is the uncorrected desired heading angle, ψ _PH is the desired heading angle

Vertical heading, ψ _E is the expected heading angle after correction; (3) Read the first target waypoint P _i from the task file of the underwater autonomous vehicle control system, i=1; (4) If i≥N, execute step (12), otherwise, read the next target waypoint P _i+1 from the task file of the underwater autonomous vehicle control system, and determine that the current local target track has To the line segment P _i P _i+1 ; (5) Obtain the current position P and current heading of the underwater autonomous vehicle from the inertial navigation system, judge whether it has reached P _i+1 , if yes, i=i+1, go to step (4); if not, go to step (6); (6) Calculate the current track tracking deviation d of the underwater autonomous vehicle, and judge whether the current track tracking deviation d is less than or equal to the first threshold d _min , if yes, perform step (7); if not, perform step ( 8); (7) The desired heading angle ψ _E after correction is equal to

If the desired heading is not adjusted, go to step (12); (8) Judging whether the current track tracking deviation d is less than or equal to the second threshold d _max and greater than the first threshold d _min , if yes, perform step (9); if not, perform step (10); (9) Calculate the auxiliary dynamic circle radius r according to the first formula, establish a dynamic auxiliary circle with the current position P of the underwater autonomous vehicle as the center and r as the radius, and use the dynamic auxiliary circle and the current local target Select the temporary target waypoint based on the positional relationship of the directed line segment P _i P _i+1 of the track, calculate the corrected expected course ψ _E , and execute step (11); (10) The current track tracking deviation d is greater than the second threshold d _max , so that the desired heading ψ _E =ψ _PH , and step (11) is executed; (11) Control the underwater autonomous vehicle to travel in the direction of ψ _E , and re-execute step (5); (12) The process ends; The first formula is specifically: r=f(d),f(d)'≤0,f(d _min )=r _max ,f(d _max )=r _min ,d _min <d _max Among them, r _min and r _max respectively represent the maximum value and minimum value of the dynamic circle radius r; Wherein, the temporary target waypoint is selected according to the positional relationship between the dynamic auxiliary circle and the directional line segment P _i P _i+1 of the current local target track, and the corrected desired course ψ _E is calculated as follows: 1) When the dynamic auxiliary circle is away from the straight line P _i P _i+1 and the projection H of P on P _i P _i+ 1 is on the extension line of P _i+1 P _i , select P _i as the temporary target waypoint , modify the desired heading so that 2) When the dynamic auxiliary circle is away from the straight line P _i P _i+1 and the projection H of P on P _i P _i+1 is on P _i P _i+1 , select H as the temporary target waypoint and modify the desired course Let ψ _E = ψ _PH ; 3) When the dynamic auxiliary circle is away from the straight line P _i P _i+1 and the projection H of P on P _i P _i+1 is on the extension line of P _i P _i+1 , select P _i+1 as the temporary target Waypoint, modify the desired course to make

4) When the dynamic auxiliary circle intersects the straight line P _i P _i+1 and the projection H of P on P _i P _i+ 1 is on the extension line of P _i+1 P _i , select P _i as the temporary target waypoint, Modify the desired heading so that

5) When the dynamic auxiliary circle and the straight line P _i P _i+1 intersect at points E and F, and the projection of P on P _i P _i+1 is on P _i P _i+1 , and P _i is within the dynamic auxiliary circle P When _i+1 is outside the dynamic auxiliary circle, select point E close to P _i+1 as the temporary target waypoint, and modify the desired course to use

6) When the dynamic auxiliary circle and the straight line P _i P _i+1 intersect at points E and F and the projection of P on P _i P _i+1 is on P _i P _i+1 , and P _i and P _i+1 are at When the dynamic auxiliary circle is outside, select point E close to P _i+1 as the temporary target waypoint, and modify the desired course to use 7) When the projection of P on P _i P _i+1 is on P _i P i ₊₁ and P _i and P _i+1 are within the dynamic auxiliary circle, select P _i+1 as the temporary target waypoint, modify expected heading

8) When the dynamic auxiliary circle intersects the straight line P _i P _i+1 , and the projection of P on P _i P _i+1 is on P _i P _i+1 , and P _i+1 is within the dynamic auxiliary circle, P _i is in When the dynamic auxiliary circle is outside, select P _i+1 point as the temporary target waypoint, and modify the desired course to use ${\mathrm{\&psi;}}_{\mathrm{E.}}={\mathrm{\&psi;}}_{{\mathrm{PP}}_{i+1}};$ 9) When the dynamic auxiliary circle intersects the straight line P _i P _i+1 and the projection of P on P _i P _i+1 is on the extension line of P _i P _i+ 1, select P _i+1 as the temporary target waypoint , modify the desired heading so that

Images