CN117950398A - Unmanned ship planning method based on speed obstacle and fuzzy parameters - Google Patents
Unmanned ship planning method based on speed obstacle and fuzzy parameters Download PDFInfo
- Publication number
- CN117950398A CN117950398A CN202311591721.XA CN202311591721A CN117950398A CN 117950398 A CN117950398 A CN 117950398A CN 202311591721 A CN202311591721 A CN 202311591721A CN 117950398 A CN117950398 A CN 117950398A
- Authority
- CN
- China
- Prior art keywords
- fuzzy
- unmanned ship
- speed
- obstacle
- constraint
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 60
- 238000011156 evaluation Methods 0.000 claims abstract description 54
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 claims abstract description 13
- 230000004927 fusion Effects 0.000 claims abstract description 4
- 230000006870 function Effects 0.000 claims description 62
- 238000005070 sampling Methods 0.000 claims description 39
- 238000004422 calculation algorithm Methods 0.000 claims description 30
- 238000004364 calculation method Methods 0.000 claims description 15
- 239000011159 matrix material Substances 0.000 claims description 12
- 238000004088 simulation Methods 0.000 claims description 12
- 230000008859 change Effects 0.000 claims description 8
- 230000004888 barrier function Effects 0.000 claims description 4
- 238000012935 Averaging Methods 0.000 claims description 3
- 238000013016 damping Methods 0.000 claims description 3
- 238000013178 mathematical model Methods 0.000 claims description 3
- 238000002156 mixing Methods 0.000 claims description 3
- 238000011161 development Methods 0.000 abstract description 3
- 238000005516 engineering process Methods 0.000 description 5
- 238000011160 research Methods 0.000 description 4
- 230000009286 beneficial effect Effects 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 238000005457 optimization Methods 0.000 description 2
- 230000001154 acute effect Effects 0.000 description 1
- 238000013528 artificial neural network Methods 0.000 description 1
- 230000002457 bidirectional effect Effects 0.000 description 1
- 238000013135 deep learning Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000007667 floating Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000010606 normalization Methods 0.000 description 1
- 238000002360 preparation method Methods 0.000 description 1
- 230000008569 process Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000012549 training Methods 0.000 description 1
- 239000003643 water by type Substances 0.000 description 1
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention provides an unmanned ship planning method based on speed obstacle and fuzzy parameters, which solves the problems of low obstacle avoidance success rate and non-optimal obstacle avoidance route of the traditional dynamic window method in a multi-obstacle water area environment. The method comprises the following steps: establishing an unmanned ship motion model and a track planning problem model; a system constraint window method (SC-DWA) is applied, the speed constraint in the dynamic window method is converted into a system input constraint, and the input of an unmanned ship power system is limited; scoring the generated predicted track through an evaluation function, and selecting an optimal track; and adjusting the evaluation function weight by a fusion fuzzy parameter selector (FSC-DWA) to realize the obstacle avoidance route which is more in line with the manipulation habit of people. The invention can effectively improve the obstacle avoidance success rate of the unmanned ship in the complex water area and optimize the obstacle avoidance route, is suitable for various water surface unmanned ship applications such as marine survey, resource development, border patrol and the like, and has important practical value and technical innovation.
Description
Technical field:
The invention belongs to the technical field of unmanned boats, and particularly relates to an unmanned boat planning method based on speed obstacle and fuzzy parameters.
The background technology is as follows:
Along with the rapid development of unmanned surface vehicle technology, the application field of the unmanned surface vehicle technology is increasingly expanded, and the unmanned surface vehicle technology covers various aspects such as marine survey, resource development, border patrol and the like. Therefore, the research on the motion planning of the unmanned surface vehicle becomes particularly critical, and the main objective in the research field is to improve the autonomy, efficiency and safety of the unmanned surface vehicle. The unmanned craft's motion planning can be generally divided into two parts, path planning and trajectory planning. Path planning is responsible for searching and determining the best path from a starting point to a target point in a global environment, taking global environment information such as maps, obstacles, etc. into account, and searching for the best path in known maps, typically using path planning algorithms. And the track planning is responsible for generating a collision avoidance track conforming to the dynamics of the unmanned ship under a given path so as to ensure the efficient and safe running of the unmanned ship in a water area.
In the technical field of obstacle avoidance of unmanned boats, various solutions have been proposed by researchers. For example, the modified A algorithm is suitable for global path planning in a simple environment, but does not fully solve the obstacle avoidance problem. The iterative potential field algorithm based on sparse point constraint improves obstacle avoidance efficiency, but the motion characteristics of the unmanned ship are not fully considered. The improved RRT algorithm considers the motion characteristics of the unmanned ship, but the randomness of the improved RRT algorithm leads to poor obstacle avoidance effect in a multi-obstacle environment. The obstacle avoidance algorithm based on model predictive control has higher obstacle avoidance success rate, but does not carry out real-time adjustment of weight parameters when constructing a cost function, so that the problem of non-optimal obstacle avoidance route exists in a multi-obstacle environment. The bidirectional RRT algorithm reduces the randomness of the search tree, but is mainly concentrated on the kinematics level, and dynamics are not deeply considered. Although the deep learning technology has potential in path planning, the neural network is difficult to construct by combining the unmanned ship movement characteristics, and a large amount of time and data are consumed for model training.
In view of the above research, the invention aims to solve the problems of low obstacle avoidance success rate and non-optimal obstacle avoidance route of unmanned boats in a multi-obstacle environment. The speed constraint in the traditional dynamic window method is improved from the perspective of the kinematics and dynamics of the unmanned ship, the speed constraint is converted into the power constraint on the unmanned ship, and the speed obstacle method is adopted to limit the input quantity of the power system at each moment so as to strictly ensure that all obstacles can be avoided when the unmanned ship travels. And then, designing a fuzzy parameter selector, constructing a fuzzy rule by means of expert experience, and dynamically adjusting the weight of an evaluation function in an algorithm in real time, so that the input quantity of the system is more in line with the manual operation habit, and the obstacle avoidance route is optimized.
The invention comprises the following steps:
The invention aims at overcoming the defects of the prior art and provides an unmanned ship planning method based on speed obstacle and fuzzy parameters so as to solve the problems of the background art.
In order to achieve the above purpose, the present invention provides the following technical solutions: the unmanned ship planning method based on the speed obstacle and the fuzzy parameters comprises the following specific steps:
s1: establishing an unmanned ship motion model and a track planning problem model;
S2: a system constraint window method (SC-DWA) is applied, the speed constraint in the dynamic window method is converted into a system input constraint, and the system input constraint is suitable for the motion characteristics of the underactuated unmanned ship; applying restrictions to the unmanned ship power system input, including a maximum value restriction and a maximum variation restriction of variation; limiting the pitching and swaying speeds of the unmanned ship by using a speed barrier method, so as to ensure obstacle avoidance;
S3: the track evaluation step comprises the steps of sampling a system input space to generate a plurality of predicted tracks; designing an evaluation function, scoring each predicted track, and considering the factors of direction angle, obstacle distance and sloshing linear velocity;
S4: selecting an optimal track, wherein the operation is as follows: normalizing the evaluation function result, and matching a weight parameter for each evaluation index; selecting the track with the highest score as the optimal track based on the total score;
S5: blending a fuzzy parameter selector (FSC-DWA) to adjust the evaluation function weights, comprising: fuzzification is carried out on the input quantity, and fuzzy reasoning is carried out by adopting a Mamdani fuzzy rule; and performing deblurring on the output fuzzy value, and adjusting the weight parameters in real time.
As a technical preferred solution of the present invention, the unmanned ship motion model in step S1, for the underactuated unmanned ship to travel in a water area, may be described as a kinematic and kinetic mathematical model:
Wherein, subscripts k and k+1 respectively represent the current time and the next time; η= [ x, y, ψ ] T is a pose vector comprising the coordinates (x, y) of the unmanned aerial vehicle in the inertial coordinate system and the yaw angle ψ; v= [ u, v, r ] T is a velocity vector, including the pitch/yaw velocity (u, v) and yaw rate r of the unmanned ship in the appendage coordinate system; τ= [ τ u,0,τr]T ] is the system input vector; m=diag (M 11,m22,m33) and D (v) =diag (D 11,d22,d33) are the inertial mass matrix and the system damping matrix, respectively; c (v) is a coriolis Li Xiangxin matrix; r is a rotation matrix function with respect to the attitude vector "×".
As a technical preferred solution of the present invention, the track planning problem in step S1 is specifically expressed as follows: the speed constraint formula adopted in the dynamic window method cannot be fully adapted to the motion characteristics of the underactuated unmanned ship, so that obstacle avoidance decisions in a water area environment are limited, and the following speed constraint formula is adopted:
Where u m is the maximum heave linear velocity, r m is the maximum yaw angular velocity, and dist (u, r) is the closest distance between the unmanned boat and the obstacle intersecting the predicted trajectory.
As a technical preferred solution of the present invention, the system constraint window method in step S2, the algorithm further includes the following steps:
S2-1, converting the speed constraint in the dynamic window method into a system input constraint, wherein the system input is required to be constrained as the unmanned ship self power system has limit:
Constraint one: the system input at the current moment cannot exceed the maximum input of the system rating:
τs1∈{(τu_k,τr_k)|τu_min≤τu_k≤τu_max,τr_min≤τr_k≤τr_max} (3)
constraint II: the system input variation cannot exceed the maximum variation of the system rating:
τs2∈{(τu_k,τr_k)|τu_k-1-Δτu_m≤τu_k≤τu_k-1+Δτu_m,
τr_k-1-Δτr_m≤τr_k≤τr_k-1+Δτr_m} (4)
Where Δτ u_m is the maximum change in τ u and Δτ r_m is the maximum change in τ r;
Constraint three: the achievable obstacle avoidance speed at the next moment also has constraints on the system input at the current moment:
Wherein τ k＝(τu_k,0,τr_k)T is the system input vector at the current moment, C is the collision cone, and v k+1 is the achievable obstacle avoidance speed at the next moment;
S2-2 after the constraint in the step S2-1, assuming that all the system inputs meeting the constraint at the current moment together form a system input space τ s, it can be described as:
τs∈τs1∩τs2∩τs3 (6)
s2-3, assuming that an obstacle o is subjected to expansion treatment, two tangential lines l1 and l2 passing through the geometric center P of the unmanned ship and making an o expansion boundary are called collision cone C, and v' represents a heave speed u, a sway speed v and a closing speed of an obstacle speed vo, the following relation is required to be satisfied for the unmanned ship to avoid the obstacle:
as a technical preferred solution of the present invention, the implementation trace appraise and choose excellent step in step S3, specifically appraise and choose excellent, is composed of the following steps:
S3-1 for the system input space τ s, sampling τ u and τ r in τ s with 0.5 as sampling interval, respectively, to obtain several groups of sampling inputs (τ ui,τri)
(τui,τri)∈{(τui,τri)|τui∈τs,τui∈τs,i∈N*} (8)
Wherein N is a positive integer set, i represents the ith group of samples;
s3-2: after sampling, for each group of sampling results, a predicted track can be simulated in the simulation time of 2 s;
s3-3: the following evaluation function is used to calculate the merits of each track, and the specific calculation formula is as follows:
Evaluation function one: the magnitude of the angle between the running direction of the end point of the predicted track and the end point direction is evaluated, and the smaller the angle is, the higher the evaluation function score is:
head(τu,τr)＝180-λ (9)
Wherein lambda is the angle between the running direction of the predicted track end point and the end point direction;
Evaluation function two: the distance between the end point of the predicted track and the nearest obstacle is evaluated, and the larger the distance is, the higher the evaluation function score is:
Wherein, (x, y) is the end point coordinate of the predicted track, (x o,yo) is the center coordinate of the expansion ellipse of the nearest obstacle, and d is the distance from the expansion boundary to the center of the expansion ellipse;
evaluation function three: the magnitude of the pitch line speed of the end point of the predicted track is evaluated, and the larger the line speed is, the higher the evaluation function score is:
vel＝abs(u) (11)
where u is the pitch line velocity of the predicted track end point, and abs (u) is the absolute value of u.
As a technical preferred scheme of the invention, the S3-1 samples the system input space, and the specific sampling comprises the following steps:
S3-1-1: using the calculated speed constraint results Taus and Taus, corresponding to equations (3) and (4) in the algorithm, respectively, and another set of speed constraints Taus3, corresponding to equation (5);
S3-1-2: the sample Space is determined by computing Taus an intersection of Taus2, where: the first two elements of Space are the maximum value and the minimum value of corresponding elements in Taus and Taus respectively, and the sampling range of unmanned ship thrust input tau ui is determined; the latter two elements of Space likewise determine the sampling range of unmanned boat steering input τ ri based on the maximum and minimum values of the corresponding elements in Taus and Taus;
s3-1-3: by performing two-cycle sampling of Space, the sampling interval is 0.5, to generate a plurality of thrust and steering input combinations (τ ui,τri);
S3-1-4: checking each sampling point (tau ui,τri) to judge whether the sampling points simultaneously meet the constraint condition in Taus;
s3-1-5: sample points (τ ui,τri) meeting the conditions are added to the result set Res for subsequent trajectory planning or other related calculations.
As a technical preferred scheme of the present invention, in step S4, the optimal track is selected, the results of the evaluation functions are normalized respectively, and each evaluation function is provided with a corresponding weight parameter, so that the total score of each predicted track can be calculated, and the calculation formula is as follows:
In the formula, alpha, beta and gamma are weight parameters, the subscript i represents an ith track, and max represents the maximum value in the symbols.
As a technical preferred scheme of the invention, the step S5 of adjusting the weight of the evaluation function by the fusion fuzzy parameter selector comprises the following specific steps:
s5-1: the fuzzy set preparation method is used for preparing fuzzy sets by using a stepping fuzzy set method, and triangle membership functions are selected, wherein the input quantity of a fuzzy parameter selector is fuzzified, and the relation of the triangle membership functions is as follows:
wherein a and c are the bottom abscissa of the membership function, and b is the top abscissa of the membership function;
S5-2: selecting a Mamdani fuzzy rule to perform fuzzy reasoning to obtain an output fuzzy value, wherein the calculation formula is as follows:
Wherein, R i is the ith fuzzy rule, i epsilon N, and the linguistic variables A i,n and B i,m in the fuzzy rule are the input variable x i and the output variable y i of the fuzzy parameter selector respectively, and a fuzzy set in the ith fuzzy rule is defined;
S5-3: and finally, performing deblurring on the output fuzzy value by adopting an area averaging method, wherein the calculation formula is as follows:
Wherein N is the number of fuzzy sets contained in the fuzzy operation result, y i * is a value corresponding to a bisector of the area formed by the i-th fuzzy set and the coordinate axis, mu i max is the membership degree of the i-th set, and y * is an accurate value obtained by resolving the fuzzy.
Compared with the related prior art, the application has the following main technical advantages: the beneficial effects of the application are as follows:
The obstacle avoidance success rate is improved: by introducing a system constraint window method (SC-DWA), the invention constrains the system input of the unmanned ship from the dynamic perspective. Particularly, the strict limitation on the transverse floating speed ensures that the unmanned ship can always effectively avoid the obstacle when advancing, and the success rate of obstacle avoidance is obviously improved.
Optimizing obstacle avoidance tracks: the invention further provides a system constraint window method (FSC-DWA) based on the fuzzy parameter selector. Through combining expert experience and adjusting weight parameters in real time, the FSC-DWA algorithm enables planned input quantity to be more in line with human operating habit, and optimizes obstacle avoidance tracks to be more reasonable.
Track performance improves: simulation verifies that the track generated by the method is obviously superior to the traditional dynamic window method in the aspects of tortuosity and track length. This demonstrates the effectiveness of the present invention in improving trajectory planning, providing a smoother and shorter range obstacle avoidance path.
Engineering applicability enhancement: the invention provides a new direction for subsequent research, namely, the algorithm is combined with a global path planning algorithm. The combination is expected to further improve the engineering applicability of unmanned ship motion planning, and provides stronger support for autonomous operation of unmanned ships in complex water area environments.
Description of the drawings:
FIG. 1 is a workflow diagram of an unmanned ship planning method based on speed obstacle and fuzzy parameters provided by the invention;
FIG. 2 is a non-optimal schematic view of an obstacle avoidance trajectory according to an embodiment of the present invention;
FIG. 3 is a velocity obstacle model of an embodiment of the invention;
FIG. 4 is a diagram showing physical quantities in an evaluation function according to an embodiment of the present invention;
FIG. 5 is a graph of FSC-DWA simulation results of an embodiment of the present invention;
FIG. 6 is a plot of system input versus time for an embodiment of the present invention;
FIG. 7 is a graph of speed versus time for an embodiment of the present invention;
FIG. 8 is a comparison of simulated trajectories for a different algorithm of a provided embodiment of the present invention.
The specific embodiment is as follows:
The invention is further described below with reference to the drawings and examples. The invention may, however, be embodied in many different forms and should not be construed as being limited to the embodiments set forth herein; rather, these embodiments provide those skilled in the art with a means to meet applicable legal requirements.
Example 1: as shown in fig. 1: the unmanned ship planning method based on the speed obstacle and the fuzzy parameters comprises the following specific steps:
S1: establishing an unmanned ship motion model and a track planning problem model, wherein the weight parameters of an evaluation function adopted by a dynamic window method are unchanged, and the generated track has a large optimization space due to the fact that the change of a dynamic water area environment cannot be flexibly dealt with;
The unmanned ship motion model, for the underactuated unmanned ship to travel in the waters, the kinematic and kinetic mathematical model can be described as:
Wherein Δt represents a unit time, and subscripts k and k+1 represent a current time and a next time respectively; η= [ x, y, ψ ] T is a pose vector comprising the coordinates (x, y) of the unmanned aerial vehicle in the inertial coordinate system and the yaw angle ψ; v= [ u, v, r ] T is a velocity vector, including the pitch/yaw velocity (u, v) and yaw rate r of the unmanned ship in the appendage coordinate system; τ= [ τ u,0,τr]T ] is the system input vector; m=diag (M 11,m22,m33) and D (v) =diag (D 11,d22,d33) are the inertial mass matrix and the system damping matrix, respectively; c (v) is a coriolis Li Xiangxin matrix; r is a rotation matrix function with respect to the attitude vector ";
the track planning problem is specifically expressed as follows: the speed constraint formula adopted in the dynamic window method cannot be fully adapted to the motion characteristics of the underactuated unmanned ship, so that obstacle avoidance decisions in a water area environment are limited, and the following speed constraint formula is adopted:
Where u m is the maximum heave linear velocity, r m is the maximum yaw angular velocity, and dist (u, r) is the closest distance between the unmanned boat and the obstacle intersecting the predicted trajectory.
S2: a system constraint windowing (SC-DWA) method is applied, wherein: converting the speed constraint in the dynamic window method into a system input constraint, and adapting to the motion characteristics of the underactuated unmanned ship; applying restrictions to the unmanned ship power system input, including a maximum value restriction and a maximum variation restriction of variation; limiting the pitching and swaying speeds of the unmanned ship by using a speed barrier method, so as to ensure obstacle avoidance;
A system constraint windowing (SC-DWA), the algorithm further comprising the steps of:
S2-1, converting the speed constraint in the dynamic window method into a system input constraint, wherein the system input is required to be constrained as the unmanned ship self power system has limit:
Constraint one: the system input at the current moment cannot exceed the maximum input of the system rating:
τs1∈{(τu_k,τr_k)|τu_min≤τu_k≤τu_max,τr_min≤τr_k≤τr_max} (3)
constraint II: the system input variation cannot exceed the maximum variation of the system rating:
τs2∈{(τu_k,τr_k)|τu_k-1-Δτu_m≤τu_k≤τu_k-1+Δτu_m,
τr_k-1-Δτr_m≤τr_k≤τr_k-1+Δτr_m} (4)
Where Δτ u_m is the maximum change in τ u and Δτ r_m is the maximum change in τ r;
Constraint three: the achievable obstacle avoidance speed at the next moment also has constraints on the system input at the current moment:
Wherein τ k＝(τu_k,0,τr_k)T is the system input vector at the current moment, C is the collision cone, and v k+1 is the achievable obstacle avoidance speed at the next moment;
S2-2 after the constraint in the step S2-1, assuming that all the system inputs meeting the constraint at the current moment together form a system input space τ s, it can be described as:
τs∈τs1∩τs2∩τs3 (6)
s2-3, assuming that an obstacle o is subjected to expansion treatment, two tangential lines l1 and l2 passing through the geometric center P of the unmanned ship and making an o expansion boundary are called collision cone C, and v' represents a heave speed u, a sway speed v and a closing speed of an obstacle speed vo, the following relation is required to be satisfied for the unmanned ship to avoid the obstacle:
s3: the track evaluation step comprises the following steps: sampling the input space of the system to generate a plurality of prediction tracks; designing an evaluation function, scoring each predicted track, and considering the factors of direction angle, obstacle distance and sloshing linear velocity;
the track appraise and choose excellent step is implemented, and the specific appraise and choose excellent consists of the following steps:
S3-1, for a system input space tau s, taking 0.5 as a sampling interval, respectively sampling tau u and tau r in tau s to obtain a plurality of groups of sampling inputs (tau ui,τri):
(τui,τri)∈{(τui,τri)|τui∈τs,τui∈τs,i∈N*} (8)
Wherein N is a positive integer set, i represents the ith group of samples;
S3-1-1: using the calculated speed constraint results Taus and Taus, corresponding to equations (3) and (4) in the algorithm, respectively, and another set of speed constraints Taus3, corresponding to equation (5);
S3-1-2: the sample Space is determined by computing Taus an intersection of Taus2, where: the first two elements of Space are the maximum value and the minimum value of corresponding elements in Taus and Taus respectively, and the sampling range of unmanned ship thrust input tau ui is determined; the latter two elements of Space likewise determine the sampling range of unmanned boat steering input τ ri based on the maximum and minimum values of the corresponding elements in Taus and Taus;
s3-1-3: by performing two-cycle sampling of Space, the sampling interval is 0.5, to generate a plurality of thrust and steering input combinations (τ ui,τri);
S3-1-4: checking each sampling point (tau ui,τri) to judge whether the sampling points simultaneously meet the constraint condition in Taus;
S3-1-5: adding the sampling points (tau ui,τri) meeting the conditions to a result set Res for subsequent track planning or other related calculations;
s3-2: after sampling, for each group of sampling results, a predicted track can be simulated in the simulation time of 2 s;
s3-3: the following evaluation function is used to calculate the merits of each track, and the specific calculation formula is as follows:
Evaluation function one: the magnitude of the angle between the running direction of the end point of the predicted track and the end point direction is evaluated, and the smaller the angle is, the higher the evaluation function score is:
head(τu,τr)＝180-λ (9)
Wherein lambda is the angle between the running direction of the predicted track end point and the end point direction;
Evaluation function two: the distance between the end point of the predicted track and the nearest obstacle is evaluated, and the larger the distance is, the higher the evaluation function score is:
Wherein, (x, y) is the end point coordinate of the predicted track, (x o,yo) is the center coordinate of the expansion ellipse of the nearest obstacle, and d is the distance from the expansion boundary to the center of the expansion ellipse;
evaluation function three: the magnitude of the pitch line speed of the end point of the predicted track is evaluated, and the larger the line speed is, the higher the evaluation function score is:
vel＝abs(u) (11)
Wherein u is the pitch and yaw linear velocity of the end point of the predicted track, and abs (u) is the absolute value of u;
S4: selecting an optimal track, wherein the operation is as follows: normalizing the evaluation function result, and matching a weight parameter for each evaluation index; selecting the track with the highest score as the optimal track based on the total score;
Selecting an optimal track, specifically calculating by adopting an evaluation function, respectively carrying out normalization processing on the results of the evaluation function, and matching corresponding weight parameters for each evaluation function, wherein the calculation formula is as follows:
Wherein, alpha, beta and gamma are weight parameters, the subscript i represents an ith track, and max represents the maximum value in the symbols;
s5: blending a fuzzy parameter selector (FSC-DWA) to adjust the evaluation function weights, comprising: fuzzification is carried out on the input quantity, and fuzzy reasoning is carried out by adopting a Mamdani fuzzy rule; deblurring the output fuzzy value and adjusting weight parameters in real time;
The fusion fuzzy parameter selector adjusts the weight of the evaluation function, and the specific steps are as follows:
S5-1: the fuzzy set is prepared by using a grading fuzzy set method, and a triangle membership function is selected: fuzzifies the input quantity of the fuzzy parameter selector, and the relation of the triangle membership functions is as follows:
wherein a and c are the bottom abscissa of the membership function, and b is the top abscissa of the membership function;
S5-2: selecting a Mamdani fuzzy rule to perform fuzzy reasoning to obtain an output fuzzy value, wherein the calculation formula is as follows:
Wherein, R i is the ith fuzzy rule, i epsilon N, and the linguistic variables A i,n and B i,m in the fuzzy rule are the input variable x i and the output variable y i of the fuzzy parameter selector respectively, and a fuzzy set in the ith fuzzy rule is defined;
S5-3: and finally, performing deblurring on the output fuzzy value by adopting an area averaging method, wherein the calculation formula is as follows:
Wherein N is the number of fuzzy sets contained in the fuzzy operation result, y i * is a value corresponding to a bisector of the area formed by the i-th fuzzy set and the coordinate axis, mu i max is the membership degree of the i-th set, and y * is an accurate value obtained by resolving the fuzzy.
As shown in fig. 2, when performing unmanned ship trajectory planning, an important problem is also faced: the generated obstacle avoidance trajectory is often not optimal. The method is characterized in that the weight parameters of the evaluation function adopted by the dynamic window method are unchanged, and the change of the dynamic water area environment cannot be flexibly dealt with, so that a large optimization space exists in the generated track. In fig. 1, oi (i e1, 4) represents four obstacles after the expansion process. Although the travel track of the unmanned ship successfully avoids all obstacles, the actual travel track has a large gap from the ideal travel track in terms of track length and tortuosity.
As shown in fig. 3, the achievable obstacle avoidance speed mentioned by the constraint three refers to that after the speed obstacle avoidance method is integrated on the basis of the dynamic window method, the surge and the sway speed of the unmanned ship are strictly limited, so that the speed set achieved by obstacle avoidance can be completed. In fig. 3, the obstacle o is inflated, and two tangents l1 and l2 to the inflated boundary are made through the geometric center P of the unmanned ship. The acute angle l1Pl2 formed by l1, l2 and point P is called the collision cone C, v' representing the combined speed of the heave speed u, the heave speed v, and the obstacle speed vo.
As shown in fig. 4, o is the center of the expansion ellipse of the obstacle, d is the distance from the expansion boundary to the center, L is the distance between the predicted trajectory end point and the obstacle, b is the predicted trajectory end point, and λ is the angle between the driving direction and the end point direction.
The invention adopts CyberShipII under-actuated unmanned ship as a model, and simulation experiments are carried out in Matlab, and the parameters of the main model of the unmanned ship are shown in table 1.
Table 1: unmanned ship model parameters
During simulation, a closed simulation water area with the length of 20m and the width of 20m is built in Matlab, barriers are randomly generated, the initial speed of the unmanned ship is set to be v (0) = [0, 0] T, the initial pose is set to be eta (0) = [1, 0] T, the fuzzy parameter selector is set to be two-input three-output, the two-input three-output is respectively head and odist in an evaluation function, and the three-output is respectively weight parameters alpha, beta and gamma.
The head interval is set to 0,180, odist, and if the distance from the nearest obstacle exceeds 5, the distance is also set to 5, and the alpha, beta and gamma intervals are set to 0, 1. The fuzzy sets of the input and output parameters are all set to be { zero; is small; the middle part; is big; positive big, symbolized as { ZO }; PS; PM; PB; PHg }. And finally, setting a Mamdani fuzzy rule according to expert knowledge, and establishing a relation between the input quantity and the output quantity. In a multi-obstacle environment, a system constraint window method (FSC-DWA) simulation trace based on a fuzzy parameter selector is shown in FIG. 5. In fig. 5, the system inputs and speed changes over time for each instant of the unmanned boat are shown in fig. 6 and 7.
In order to compare the FSC-DWA algorithm with the traditional algorithm, the initial bow swing angles of the unmanned ship are respectively set to be 0, pi/4 and pi/2, and the traditional algorithm and the new algorithm are simulated at the same time, and the simulation track is shown in figure 8. As can be observed from fig. 8 (a), 8 (b) and 8 (c), the track generated by the FSC-DWA algorithm well solves the problem of failure in obstacle avoidance in the multi-obstacle environment proposed by the present invention, and the obstacle avoidance track is not optimal. On the premise of keeping a safe distance from the obstacles, the track generated by the new algorithm obviously better passes through the obstacles and has less tortuosity compared with the traditional algorithm, thereby being beneficial to steering navigation of the unmanned ship. The simulation experiment results verify the rationality and applicability of the new algorithm.
In summary, the invention provides a system constraint window method (SC-DWA), the SC-DWA algorithm constrains the system input from the dynamics point of view, and fully considers the motion characteristics of the unmanned ship, and severely limits the transverse drift speed, so that the unmanned ship can always avoid the obstacle when advancing. Secondly, aiming at the problem that the obstacle avoidance track left by the SC-DWA algorithm is not optimal, the invention provides a solution, and provides a system constraint window method (FSC-DWA) based on a fuzzy parameter selector, wherein the FSC-DWA algorithm combines expert experience to adjust weight parameters in real time, so that the planned input quantity is more in line with the operation habit of people, and the track is more reasonable. Simulation verifies that the track generated by the algorithm provided by the invention can avoid the obstacle, and is obviously superior to the traditional dynamic window method in the aspects of tortuosity and track length, so that the feasibility of the method is further verified.
The foregoing examples merely illustrate embodiments of the invention and are described in more detail and are not to be construed as limiting the scope of the invention. It should be noted that it will be apparent to those skilled in the art that several variations and modifications can be made without departing from the spirit of the invention, which are all within the scope of the invention.
Claims (8)
1. The unmanned ship planning method based on the speed obstacle and the fuzzy parameter is characterized by comprising the following steps of:
s1: establishing an unmanned ship motion model and a track planning problem model;
S2: a system constraint window method (SC-DWA) is applied, the speed constraint in the dynamic window method is converted into a system input constraint, and the system input constraint is suitable for the motion characteristics of the underactuated unmanned ship; applying restrictions to the unmanned ship power system input, including a maximum value restriction and a maximum variation restriction of variation; limiting the pitching and swaying speeds of the unmanned ship by using a speed barrier method, so as to ensure obstacle avoidance;
S3: the track evaluation step comprises the steps of sampling a system input space to generate a plurality of predicted tracks; designing an evaluation function, scoring each predicted track, and considering the factors of direction angle, obstacle distance and sloshing linear velocity;
S4: selecting an optimal track, wherein the operation is as follows: normalizing the evaluation function result, and matching a weight parameter for each evaluation index; selecting the track with the highest score as the optimal track based on the total score;
S5: blending a fuzzy parameter selector (FSC-DWA) to adjust the evaluation function weights, comprising: fuzzification is carried out on the input quantity, and fuzzy reasoning is carried out by adopting a Mamdani fuzzy rule; and performing deblurring on the output fuzzy value, and adjusting the weight parameters in real time.
2. The unmanned ship planning method based on speed obstacle and fuzzy parameters of claim 1, wherein the unmanned ship motion model in S1, for the underactuated unmanned ship to travel in the water area, can be described as a kinematic and kinetic mathematical model:
Wherein Δt represents a unit time, and subscripts k and k+1 represent a current time and a next time respectively; η= [ x, y, ψ ] T is a pose vector comprising the coordinates (x, y) of the unmanned aerial vehicle in the inertial coordinate system and the yaw angle ψ; v= [ u, v, r ] T is a velocity vector, including the pitch/yaw velocity (u, v) and yaw rate r of the unmanned ship in the appendage coordinate system; τ= [ τ u,0,τr]T ] is the system input vector; m=diag (M 11,m22,m33) and D (v) =diag (D 11,d22,d33) are the inertial mass matrix and the system damping matrix, respectively; c (v) is a coriolis Li Xiangxin matrix; r is a rotation matrix function with respect to the attitude vector "×".
3. The unmanned ship planning method according to claim 1, wherein the trajectory planning problem in S1 is specifically expressed as: the speed constraint formula adopted in the dynamic window method cannot be fully adapted to the motion characteristics of the underactuated unmanned ship, so that obstacle avoidance decisions in a water area environment are limited, and the following speed constraint formula is adopted:
Where u m is the maximum heave linear velocity, r m is the maximum yaw angular velocity, and dist (u, r) is the closest distance between the unmanned boat and the obstacle intersecting the predicted trajectory.
4. The unmanned ship planning method according to claim 1, wherein the system constraint window method (SC-DWA) in S2, the algorithm further comprises the steps of:
S2-1, converting the speed constraint in the dynamic window method into a system input constraint, wherein the system input is required to be constrained as the unmanned ship self power system has limit:
Constraint one: the system input at the current moment cannot exceed the maximum input of the system rating:
τs1∈{(τu_k,τr_k)|τu_min≤τu_k≤τu_max,τr_min≤τr_k≤τr_max} (3)
constraint II: the system input variation cannot exceed the maximum variation of the system rating:
τs2∈{(τu_k,τr_k)|τu_k-1-Δτu_m≤τu_k≤τu_k-1+Δτu_m,
τr_k-1-Δτr_m≤τr_k≤τr_k-1+Δτr_m} (4)
Where Δτ u_m is the maximum change in τ u and Δτ r_m is the maximum change in τ r;
Constraint three: the achievable obstacle avoidance speed at the next moment also has constraints on the system input at the current moment:
Wherein τ k＝(τu_k,0,τr_k)T is the system input vector at the current moment, C is the collision cone, and v k+1 is the achievable obstacle avoidance speed at the next moment;
S2-2 after the constraint in the step S2-1, assuming that all the system inputs meeting the constraint at the current moment together form a system input space τ s, it can be described as:
τs∈τs1∩τs2∩τs3 (6)
s2-3, assuming that an obstacle o is subjected to expansion treatment, two tangential lines l1 and l2 passing through the geometric center P of the unmanned ship and making an o expansion boundary are called collision cone C, and v' represents a heave speed u, a sway speed v and a closing speed of an obstacle speed vo, the following relation is required to be satisfied for the unmanned ship to avoid the obstacle:
5. the unmanned ship planning method according to claim 1, wherein the implementation track appraise and choose excellent in S3, specifically appraise and choose excellent, comprises the following steps:
S3-1, for a system input space tau s, taking 0.5 as a sampling interval, respectively sampling tau u and tau r in tau s to obtain a plurality of groups of sampling inputs (tau ui,τri):
(τui,τri)∈{(τui,τri)|τui∈τs,τui∈τs,i∈N*} (8)
Wherein N is a positive integer set, i represents the ith group of samples;
s3-2: after sampling, for each group of sampling results, a predicted track can be simulated in the simulation time of 2 s;
s3-3: the following evaluation function is used to calculate the merits of each track, and the specific calculation formula is as follows:
Evaluation function one: the magnitude of the angle between the running direction of the end point of the predicted track and the end point direction is evaluated, and the smaller the angle is, the higher the evaluation function score is:
head(τu,τr)＝180-λ (9)
Wherein lambda is the angle between the running direction of the predicted track end point and the end point direction;
Evaluation function two: the distance between the end point of the predicted track and the nearest obstacle is evaluated, and the larger the distance is, the higher the evaluation function score is:
Wherein, (x, y) is the end point coordinate of the predicted track, (x o,yo) is the center coordinate of the expansion ellipse of the nearest obstacle, and d is the distance from the expansion boundary to the center of the expansion ellipse;
evaluation function three: the magnitude of the pitch line speed of the end point of the predicted track is evaluated, and the larger the line speed is, the higher the evaluation function score is:
vel＝abs(u) (11)
where u is the pitch line velocity of the predicted track end point, and abs (u) is the absolute value of u.
6. The unmanned ship planning method based on speed obstacle and fuzzy parameters of claim 5, wherein the step of sampling the system input space in S3-1 comprises the following steps:
S3-1-1: using the calculated speed constraint results Taus and Taus, corresponding to equations (3) and (4) in the algorithm, respectively, and another set of speed constraints Taus3, corresponding to equation (5);
S3-1-2: the sample Space is determined by computing Taus an intersection of Taus2, where: the first two elements of Space are the maximum value and the minimum value of corresponding elements in Taus and Taus respectively, and the sampling range of unmanned ship thrust input tau ui is determined; the latter two elements of Space likewise determine the sampling range of unmanned boat steering input τ ri based on the maximum and minimum values of the corresponding elements in Taus and Taus;
s3-1-3: by performing two-cycle sampling of Space, the sampling interval is 0.5, to generate a plurality of thrust and steering input combinations (τ ui,τri);
S3-1-4: checking each sampling point (tau ui,τri) to judge whether the sampling points simultaneously meet the constraint condition in Taus;
s3-1-5: sample points (τ ui,τri) meeting the conditions are added to the result set Res for subsequent trajectory planning or other related calculations.
7. The unmanned ship planning method based on speed obstacle and fuzzy parameters according to claim 1, wherein the optimal trajectory selected in S4 is calculated by adopting the evaluation function in claim 5, the results of the evaluation function are normalized respectively, and each evaluation function is matched with a corresponding weight parameter, so that the total score of each predicted trajectory can be calculated, and the calculation formula is as follows:
In the formula, alpha, beta and gamma are weight parameters, the subscript i represents an ith track, and max represents the maximum value in the symbols.
8. The unmanned ship planning method based on speed obstacle and fuzzy parameters according to claim 1, wherein the step of adjusting the evaluation function weight by the fusion fuzzy parameter selector in S5 comprises the following specific steps:
S5-1: the fuzzy set is prepared by using a grading fuzzy set method, and a triangle membership function is selected: fuzzifies the input quantity of the fuzzy parameter selector, and the relation of the triangle membership functions is as follows:
wherein a and c are the bottom abscissa of the membership function, and b is the top abscissa of the membership function;
S5-2: selecting a Mamdani fuzzy rule to perform fuzzy reasoning to obtain an output fuzzy value, wherein the calculation formula is as follows:
Wherein, R i is the ith fuzzy rule, i epsilon N, and the linguistic variables A i,n and B i,m in the fuzzy rule are the input variable x i and the output variable y i of the fuzzy parameter selector respectively, and a fuzzy set in the ith fuzzy rule is defined;
S5-3: and finally, performing deblurring on the output fuzzy value by adopting an area averaging method, wherein the calculation formula is as follows:
Wherein N is the number of fuzzy sets contained in the fuzzy operation result, y i * is a value corresponding to a bisector of the area formed by the i-th fuzzy set and the coordinate axis, mu i max is the membership degree of the i-th set, and y * is an accurate value obtained by resolving the fuzzy.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311591721.XA CN117950398A (en) | 2023-11-27 | 2023-11-27 | Unmanned ship planning method based on speed obstacle and fuzzy parameters |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311591721.XA CN117950398A (en) | 2023-11-27 | 2023-11-27 | Unmanned ship planning method based on speed obstacle and fuzzy parameters |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN117950398A true CN117950398A (en) | 2024-04-30 |
Family
ID=90791239
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311591721.XA Pending CN117950398A (en) | 2023-11-27 | 2023-11-27 | Unmanned ship planning method based on speed obstacle and fuzzy parameters |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117950398A (en) |
-
2023
- 2023-11-27 CN CN202311591721.XA patent/CN117950398A/en active Pending
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Chen et al. | Path planning and obstacle avoiding of the USV based on improved ACO-APF hybrid algorithm with adaptive early-warning | |
| CN108803321B (en) | Autonomous underwater vehicle track tracking control method based on deep reinforcement learning | |
| CN109283842B (en) | Unmanned ship track tracking intelligent learning control method | |
| CN109240288B (en) | Unmanned ship collision avoidance path planning method based on track unit under condition of obstacle | |
| CN108876065B (en) | Unmanned ship motion planning method based on track unit | |
| CN111123923B (en) | Unmanned ship local path dynamic optimization method | |
| CN104765368A (en) | Unmanned ship heading and speed cooperative control method based on fuzzy adaptive algorithm | |
| CN111240345A (en) | Underwater robot trajectory tracking method based on double BP network reinforcement learning framework | |
| CN113538973B (en) | Automatic ship collision avoidance method based on improved particle swarm optimization | |
| CN113093804B (en) | Unmanned ship formation control method and control system based on inversion sliding mode control | |
| CN114545935A (en) | Fuzzy PID (proportion integration differentiation) dynamic positioning cable laying ship tracking control method based on improved sparrow algorithm | |
| Guo et al. | Mission-driven path planning and design of submersible unmanned ship with multiple navigation states | |
| Zhang et al. | Ship heading control based on fuzzy PID control | |
| Dong et al. | Gliding motion optimization for a biomimetic gliding robotic fish | |
| CN117311142A (en) | DP ship motion control and thrust distribution cooperative control method integrating particle swarm optimization and neural network predictive control | |
| Liang et al. | Economic MPC-based planning for marine vehicles: Tuning safety and energy efficiency | |
| CN117950398A (en) | Unmanned ship planning method based on speed obstacle and fuzzy parameters | |
| Cheng et al. | A new fuzzy autopilot for way-point tracking control of ships | |
| Vianna et al. | Neural Network Based Model Predictive Control for an Autonomous Vehicle | |
| Yan et al. | A hybrid PSO-WG algorithm for AUV path planning in unknown oceanic environment | |
| Malecki | Applying of fuzzy logic to precise control of the ship motion | |
| Sun et al. | Predicting the Trajectory tracking control of unmanned surface vehicle based on deep learning | |
| Zalewski | 13. Path Following Problem for a DP Ship Simulation Model | |
| CN117519136B (en) | Unmanned ship path tracking method considering large curvature turning | |
| Guan et al. | Motorized buoy path following based on improved LOS algorithm and Aquila Optimizer algorithm |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination |