CN114408122B - Design method of ship anti-collision control system - Google Patents

Design method of ship anti-collision control system Download PDF

Info

Publication number
CN114408122B
CN114408122B CN202210102027.6A CN202210102027A CN114408122B CN 114408122 B CN114408122 B CN 114408122B CN 202210102027 A CN202210102027 A CN 202210102027A CN 114408122 B CN114408122 B CN 114408122B
Authority
CN
China
Prior art keywords
ship
module
path
man
machine
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202210102027.6A
Other languages
Chinese (zh)
Other versions
CN114408122A (en
Inventor
彭周华
阮明昊
王安青
王冠杰
王丹
刘陆
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Dalian Maritime University
Original Assignee
Dalian Maritime University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Dalian Maritime University filed Critical Dalian Maritime University
Priority to CN202210102027.6A priority Critical patent/CN114408122B/en
Publication of CN114408122A publication Critical patent/CN114408122A/en
Application granted granted Critical
Publication of CN114408122B publication Critical patent/CN114408122B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B63—SHIPS OR OTHER WATERBORNE VESSELS; RELATED EQUIPMENT
    • B63B—SHIPS OR OTHER WATERBORNE VESSELS; EQUIPMENT FOR SHIPPING 
    • B63B79/00—Monitoring properties or operating parameters of vessels in operation
    • B63B79/40—Monitoring properties or operating parameters of vessels in operation for controlling the operation of vessels, e.g. monitoring their speed, routing or maintenance schedules
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B63—SHIPS OR OTHER WATERBORNE VESSELS; RELATED EQUIPMENT
    • B63B—SHIPS OR OTHER WATERBORNE VESSELS; EQUIPMENT FOR SHIPPING 
    • B63B79/00—Monitoring properties or operating parameters of vessels in operation
    • B63B79/10—Monitoring properties or operating parameters of vessels in operation using sensors, e.g. pressure sensors, strain gauges or accelerometers
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B63—SHIPS OR OTHER WATERBORNE VESSELS; RELATED EQUIPMENT
    • B63B—SHIPS OR OTHER WATERBORNE VESSELS; EQUIPMENT FOR SHIPPING 
    • B63B79/00—Monitoring properties or operating parameters of vessels in operation
    • B63B79/20—Monitoring properties or operating parameters of vessels in operation using models or simulation, e.g. statistical models or stochastic models
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02A—TECHNOLOGIES FOR ADAPTATION TO CLIMATE CHANGE
    • Y02A30/00—Adapting or protecting infrastructure or their operation
    • Y02A30/30—Adapting or protecting infrastructure or their operation in transportation, e.g. on roads, waterways or railways

Landscapes

  • Chemical & Material Sciences (AREA)
  • Engineering & Computer Science (AREA)
  • Combustion & Propulsion (AREA)
  • Mechanical Engineering (AREA)
  • Ocean & Marine Engineering (AREA)
  • Physics & Mathematics (AREA)
  • Probability & Statistics with Applications (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Traffic Control Systems (AREA)

Abstract

The invention discloses a design method of a ship anti-collision control system, which is characterized by comprising the following steps: the system comprises a ship motion module, a path tracking guidance module and a man-machine co-fusion guidance module; the input end of the ship motion module is connected with the output end of the man-machine co-fusion guidance module, and the output end of the ship motion module is connected with the input ends of the path tracking guidance module and the man-machine co-fusion guidance module; the input end of the path tracking guidance module is connected with the output end of the ship motion module, and the output end of the path tracking guidance module is connected with the input end of the man-machine co-fusion guidance module; the input end of the man-machine co-fusion guidance module is connected with the output ends of the ship motion module and the path tracking guidance module, and the output end of the man-machine co-fusion guidance module is connected with the input end of the ship motion module. According to the unmanned water surface vessel and obstacle-based unmanned water surface vessel, the movement parameters of a human driver are introduced, the collision of man-machine co-driving is alleviated by adjusting the co-fusion control right, constraint can be enforced or optimal actions can be taken when intention is made, and the coordination capability of human beings and automation is fully exerted.

Description

Design method of ship anti-collision control system
Technical Field
The invention relates to the technical field of ship motion control, in particular to a design method of a ship anti-collision control system.
Background
The ship anti-collision control means that the ship can safely and effectively avoid various obstacles under different marine environments, smoothly arrives at a destination, and has real-time performance while avoiding according to the avoidance rules of the marine ship. At present, the obstacles involved in autonomous anti-collision of unmanned water surface vessels are divided into two types: firstly, static barriers, mainly reefs, shoals, buoys on water surfaces and the like; and the dynamic barriers are mainly ships and the like in sailing. The autonomous anti-collision of the ship can be divided into a known global obstacle avoidance planning and an unknown local obstacle avoidance planning in the driving process. The global obstacle avoidance is generally to seek an optimal motion track or path for avoiding obstacles under the condition that the global environment is known, and is suitable for avoiding obstacles aiming at static obstacles; the local obstacle avoidance is to autonomously avoid unexpected unknown obstacles according to detected real-time environmental information under the condition that the local environment is unknown, so that the ship can safely complete a running task, and the method is generally suitable for dynamic obstacle avoidance planning for avoiding the ship. In the existing autonomous marine collision avoidance system, the following disadvantages exist: first, in the existing global obstacle avoidance study aiming at static obstacles, a ship is generally regarded as a mass point, and the obstacle avoidance is often avoided in a safe and timely manner due to neglecting the constraint on the safe distance. Secondly, the existing unmanned surface vessel anti-collision usually uses a pure motion control model, the model is nonlinear and high in uncertainty, has poor adaptability to waterway navigation, is difficult to meet complex sea conditions, and cannot achieve the balance of the obstacle avoidance efficiency and navigation comfort of the vessel.
Disclosure of Invention
The invention provides a ship anti-collision control system and a design method thereof, which are used for overcoming the problems.
The system of the invention comprises: the system comprises a ship motion module, a path tracking guidance module and a man-machine co-fusion guidance module;
the ship movement module is used for outputting the position information of the ship; the input end of the ship motion module is connected with the output end of the man-machine co-fusion guidance module, and the output end of the ship motion module is connected with the input ends of the path tracking guidance module and the man-machine co-fusion guidance module;
the path tracking guidance module is used for designing a path tracking guidance law psi LOS And parameterizing update law, controlling a ship navigation path; the input end of the path tracking guidance module is connected with the output end of the ship motion module, and the output end of the path tracking guidance module is connected with the input end of the man-machine co-fusion guidance module;
the man-machine co-fusion guidance module is used for designing a man-machine co-fusion guidance law, controlling an instruction of a driver and controlling the specific gravity of the path tracking guidance module on ship navigation; the input end of the man-machine co-fusion guidance module is connected with the output ends of the ship motion module and the path tracking guidance module, and the output end of the man-machine co-fusion guidance module is connected with the input end of the ship motion module.
Further, the location information is obtained by the following strategy:
step 1, calculating derivative of ship position information:
Figure GDA0004075706880000021
wherein ,
Figure GDA0004075706880000022
derivative of the ship position information (x, y) x-axis position coordinates and y-axis position coordinates, respectively; psi represents the bow angle of the ship under the earth coordinate system, U represents the combined speed of the heave speed and the surge speed of the anti-collision ship, U is empirically set, beta represents the sideslip angle of the anti-collision ship, and the sideslip angle beta of the anti-collision ship is empirically set, beta epsilon (-pi, pi)]；
And 2, automatically generating an x-axis position coordinate x of the ship and a y-axis position coordinate y of the ship according to the ship position coordinate derivative.
Further, the path tracking guidance law ψ LOS Obtained by the following formula:
Figure GDA0004075706880000023
wherein ,ψLOS The path-tracking guidance law is represented,
Figure GDA0004075706880000024
the tangential angle of the planned path is indicated,
Figure GDA0004075706880000025
and represents the planned path (x d (θ),y d (θ)) in the plan abscissa x d (θ)Is used for the purpose of determining the derivative of (c),
Figure GDA0004075706880000026
representing a planned path (x d (θ),y d (θ)) in-plan ordinate y d (θ) derivative, e represents empirically set longitudinal tracking error along the planned path, Δ represents empirically set forward looking distance, ΔεR; planning path (x) d (θ),y d (θ)) is empirically set.
Further, the parameter update law is obtained by the following formula:
Figure GDA0004075706880000027
wherein ,
Figure GDA0004075706880000028
representing a parameter update law, < >>
Figure GDA0004075706880000029
Represents ideal speed, k represents empirically set constant gain parameter, k>0, s represents an empirically set lateral tracking error along the planned path; psi phi type d Representing an empirically set desired heading angle.
Further, the calculation formula of the man-machine co-fusion guidance law is as follows:
ψ cmd ＝λψ H +(1-λ)ψ LOS (4)
wherein ,ψcmd Represents man-machine co-fusion guidance law, psi H The control instruction set by the driver is represented, and lambda represents a weight factor of the driver participating in dynamic decision; psi phi type LOS Representing a path tracking guidance law.
Further, the calculation formula of the weight factor lambda of the driver participating in the dynamic decision is as follows:
Figure GDA0004075706880000031
wherein ,
Figure GDA0004075706880000032
Distance p between obstacle and ship measured by vision distance sensor o ＝(x o ,y o ) For the position coordinates of the obstacle in the earth's coordinate system, gamma, measured by the line-of-sight sensor ρ As gain parameter ρ safe For a safe distance from an obstacle ρ danger Is the dangerous distance from the obstacle; x represents the x-axis position coordinate of the ship in the earth coordinate system, and y represents the y-axis position coordinate of the ship in the earth coordinate system; safety distance ρ from obstacle safe Dangerous distance ρ from obstacle danger Gain parameter gamma ρ Are all empirically set. />
The method comprises the following steps:
step 1, building a ship movement module and acquiring ship position information (x, y);
step 2, setting ship parameterized path information according to experience, and acquiring a path tracking guidance law psi according to the ship position information and the ship parameterized path information in the step 1 LOS And establishing a module path tracking guidance module according to the parameterized update law;
step 3, tracking a guidance law psi according to the path LOS Based on the apparent distance rho of the target ship and the obstacle, a man-machine co-fusion weight lambda is obtained, a man-machine co-fusion guidance module is established, a man-machine co-fusion guidance law is designed, and the control proportion of the driver's instruction and the path tracking guidance module to ship navigation is controlled; the line of sight p of the target vessel and the obstacle is acquired by sensors on the vessel.
Further, establishing the ship movement module in step 1 includes:
step 11, calculating derivative of ship position information:
Figure GDA0004075706880000033
wherein ,
Figure GDA0004075706880000034
derivative of the ship position information (x, y) x-axis position coordinates and y-axis position coordinates, respectively; psi represents the bow angle of the ship under the earth coordinate system, U represents the combined speed of the heave speed and the surge speed of the anti-collision ship, U is empirically set, beta represents the sideslip angle of the anti-collision ship, and the sideslip angle beta of the anti-collision ship is empirically set, beta epsilon (-pi, pi)]；
And 12, automatically generating an x-axis position coordinate x of the ship and a y-axis position coordinate y of the ship according to the ship position coordinate derivative.
Further, the establishing the path tracking guidance module in the step 2 includes:
step 21, designing a path tracking guidance law, wherein the path tracking guidance law is used for controlling the ship heading:
Figure GDA0004075706880000041
wherein ,ψLOS The path-tracking guidance law is represented,
Figure GDA0004075706880000042
the tangential angle of the planned path is indicated,
Figure GDA0004075706880000043
and represents the planned path (x d (θ),y d (θ)) in the plan abscissa x d The derivative of (theta) is used,
Figure GDA0004075706880000044
representing a planned path (x d (θ),y d (θ)) in-plan ordinate y d (θ) derivative, e represents empirically set longitudinal tracking error along the planned path, Δ represents empirically set forward looking distance, ΔεR; planning path (x) d (θ),y d (θ)) is empirically set;
step 22, designing a parameter update law for updating the planned path (x) of the ship in real time d (θ),y d (θ)), the parameter is furtherThe new law is:
Figure GDA0004075706880000045
wherein ,
Figure GDA0004075706880000046
representing a parameter update law, < >>
Figure GDA0004075706880000047
Represents ideal speed, k represents empirically set constant gain parameter, k>0, s represents an empirically set lateral tracking error along the planned path; psi phi type d Representing an empirically set desired heading angle.
Further, the man-machine co-fusion guidance module in the step 3 is as follows:
ψ cmd ＝λψ H +(1-λ)ψ LOS (9)
wherein ,ψcmd Represents man-machine co-fusion guidance law, psi H The control instruction set by the driver is represented, and lambda represents a weight factor of the driver participating in dynamic decision; psi phi type LOS Representing a path tracking guidance law.
Further, the calculation formula of the weight factor lambda of the driver participating in the dynamic decision is as follows:
Figure GDA0004075706880000048
wherein ,
Figure GDA0004075706880000049
distance p between obstacle and ship measured by vision distance sensor o ＝(x o ,y o ) For the position coordinates of the obstacle in the earth's coordinate system, gamma, measured by the line-of-sight sensor ρ As gain parameter ρ safe For a safe distance from an obstacle ρ danger Is the dangerous distance from the obstacle; x represents the x-axis position coordinates of the ship in the earth coordinate system, and y represents the earthThe y-axis position coordinates of the ship under the coordinate system; safety distance ρ from obstacle safe Dangerous distance ρ from obstacle danger Gain parameter gamma ρ Are all empirically set.
According to the invention, the man-machine co-fusion guidance module is established, so that the autonomous control authority of the ship during path tracking is maintained, and simultaneously, the human pilot is allowed to participate in dynamic avoidance decision (through designing the man-machine co-fusion guidance law, the instruction of the pilot and the control proportion of the path tracking guidance module to ship navigation) so as to realize continuous authority transfer of the human pilot and the ship automatic driving, so that the ship track is smooth, and the navigation comfort is improved. According to the invention, based on the distance between the unmanned water surface vessel and the obstacle, the activity parameters (the weight factor lambda of the driver participating in dynamic decision) of the human driver are introduced, the collision of the co-driving of the human and the aircraft is alleviated, the restraint can be forced to be executed or the optimal action can be taken when the intention is made, and the coordination capability of the human and the automation is fully exerted.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions of the prior art, the drawings that are needed in the embodiments or the description of the prior art will be briefly described below, it will be obvious that the drawings in the following description are some embodiments of the present invention, and that other drawings can be obtained according to these drawings without inventive effort to a person skilled in the art.
FIG. 1 is a block diagram of a system of the present invention;
FIG. 2 is a flow chart of the method of the present invention;
FIG. 3 is a graph showing the gain weight effect of the present invention;
FIG. 4 is a control effect diagram of the ship under guidance law;
FIG. 5 is a view showing the effect of observing the tracking error of the actual path of the ship;
fig. 6 is a view showing the observation effect of the ship motion preventing track.
Detailed Description
For the purpose of making the objects, technical solutions and advantages of the embodiments of the present invention more apparent, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention, and it is apparent that the described embodiments are some embodiments of the present invention, but not all embodiments of the present invention. All other embodiments, which can be made by those skilled in the art based on the embodiments of the invention without making any inventive effort, are intended to be within the scope of the invention.
As shown in fig. 1, the present embodiment includes:
the system of the invention comprises: the system comprises a ship motion module, a path tracking guidance module and a man-machine co-fusion guidance module;
the ship movement module is used for outputting the position information of the ship; the input end of the ship motion module is connected with the output end of the man-machine co-fusion guidance module, and the output end of the ship motion module is connected with the input ends of the path tracking guidance module and the man-machine co-fusion guidance module;
the path tracking guidance module is used for designing a path tracking guidance law psi LOS And parameterizing update law, controlling a ship navigation path; the input end of the path tracking guidance module is connected with the output end of the ship motion module, and the output end of the path tracking guidance module is connected with the input end of the man-machine co-fusion guidance module;
the man-machine co-fusion guidance module is used for designing a man-machine co-fusion guidance law, controlling an instruction of a driver and controlling the specific gravity of the path tracking guidance module on ship navigation;
the input end of the man-machine co-fusion guidance module is connected with the output ends of the ship motion module and the path tracking guidance module, and the output end of the man-machine co-fusion guidance module is connected with the input end of the ship motion module.
Preferably, the location information is obtained by the following strategy:
step 1, calculating derivative of ship position information:
Figure GDA0004075706880000061
wherein ,
Figure GDA0004075706880000062
derivative of the ship position information (x, y) x-axis position coordinates and y-axis position coordinates, respectively; psi represents the bow angle of the ship under the earth coordinate system, U represents the combined speed of the heave speed and the surge speed of the anti-collision ship, U is empirically set, beta represents the sideslip angle of the anti-collision ship, and the sideslip angle beta of the anti-collision ship is empirically set, beta epsilon (-pi, pi)]；
And 2, automatically generating an x-axis position coordinate x of the ship and a y-axis position coordinate y of the ship according to the ship position coordinate derivative.
Preferably, the path tracking guidance law ψ LOS Obtained by the following formula:
Figure GDA0004075706880000063
wherein ,ψLOS The path-tracking guidance law is represented,
Figure GDA0004075706880000064
the tangential angle of the planned path is indicated,
Figure GDA0004075706880000065
and represents the planned path (x d (θ),y d (θ)) in the plan abscissa x d The derivative of (theta) is used,
Figure GDA0004075706880000066
representing a planned path (x d (θ),y d (θ)) in-plan ordinate y d (θ) derivative, e represents empirically set longitudinal tracking error along the planned path, Δ represents empirically set forward looking distance, ΔεR; planning path (x) d (θ),y d (θ)) is empirically set.
In particular, the path-tracking guidance law is used to control the vessel heading.
Preferably, the parameter update law is obtained by the following formula:
Figure GDA0004075706880000071
wherein ,
Figure GDA0004075706880000072
representing a parameter update law, < >>
Figure GDA0004075706880000073
Represents ideal speed, k represents empirically set constant gain parameter, k>0, s represents an empirically set lateral tracking error along the planned path; psi phi type d Representing an empirically set desired heading angle.
Specifically, the parameter update law is used to update the planned path (x d (θ),y d (θ))。
Preferably, the calculation formula of the man-machine co-fusion guidance law is as follows:
ψ cmd ＝λψ H +(1-λ)ψ LOS (4)
wherein ,ψcmd Represents man-machine co-fusion guidance law, psi H The control instruction set by the driver is represented, and lambda represents a weight factor of the driver participating in dynamic decision; psi phi type LOS Representing a path tracking guidance law.
Specifically, the man-machine co-fusion guidance law combines a control instruction set by a driver and a path tracking guidance law, controls the control proportion of the driver and the path tracking guidance law on the ship, and realizes the man-machine co-fusion ship control.
Preferably, the calculation formula of the weight factor λ of the driver participating in the dynamic decision is:
Figure GDA0004075706880000074
wherein ,
Figure GDA0004075706880000075
distance p between obstacle and ship measured by vision distance sensor o ＝(x o ,y o ) For looking atPosition coordinates of the obstacle in the earth coordinate system, gamma, measured from the sensor ρ As gain parameter ρ safe For a safe distance from an obstacle ρ danger Is the dangerous distance from the obstacle; x represents the x-axis position coordinate of the ship in the earth coordinate system, and y represents the y-axis position coordinate of the ship in the earth coordinate system; safety distance ρ from obstacle safe Dangerous distance ρ from obstacle danger Gain parameter gamma ρ Are all empirically set.
Specifically ρ>ρ safe The time represents the safety of ships and no anti-collision operation is performed; ρ danger <ρ<ρ safe The time represents that a human driver participates in anti-collision in a weighted mode and the weight factor is gradually close to 1; ρ<ρ danger When the ship is in a ship, the ship is in a ship collision-proof state, and the ship is in a ship collision-proof state.
As shown in fig. 2, the method of the present invention comprises the steps of:
step 1, building a ship movement module and acquiring ship position information (x, y);
step 2, setting ship parameterized path information according to experience, and acquiring a path tracking guidance law psi according to the ship position information and the ship parameterized path information in the step 1 LOS And establishing a module path tracking guidance module according to the parameterized update law;
step 3, tracking a guidance law psi according to the path LOS Based on the apparent distance rho of the target ship and the obstacle, a man-machine co-fusion weight lambda is obtained, a man-machine co-fusion guidance module is established, a man-machine co-fusion guidance law is designed, and the control proportion of the driver's instruction and the path tracking guidance module to ship navigation is controlled; the line of sight p of the target vessel and the obstacle is acquired by sensors on the vessel.
Preferably, the building a ship movement module in step 1 includes:
Figure GDA0004075706880000081
wherein ,
Figure GDA0004075706880000082
derivative of the ship position information (x, y) x-axis position coordinates and y-axis position coordinates, respectively; psi represents the bow angle of the ship under the earth coordinate system, U represents the combined speed of the heave speed and the surge speed of the anti-collision ship, U is empirically set, beta represents the sideslip angle of the anti-collision ship, and the sideslip angle beta of the anti-collision ship is empirically set, beta epsilon (-pi, pi)]；/>
wherein ,
Figure GDA0004075706880000083
derivative of the ship position information (x, y) x-axis position coordinates and y-axis position coordinates, respectively; psi represents the bow angle of the ship under the earth coordinate system, U represents the combined speed of the heave speed and the surge speed of the anti-collision ship, U is empirically set, beta represents the sideslip angle of the anti-collision ship, and the sideslip angle beta of the anti-collision ship is empirically set, beta epsilon (-pi, pi)]；
And 12, automatically generating an x-axis position coordinate x of the ship and a y-axis position coordinate y of the ship through python according to the ship position coordinate derivative.
Specifically, the ship closing speed u=1.2 m/s is set in the present embodiment.
The step 2 of establishing the path tracking guidance module comprises the following steps:
step 21, designing a path tracking guidance law, wherein the path tracking guidance law is used for controlling the ship heading:
Figure GDA0004075706880000084
wherein ,ψLOS The path-tracking guidance law is represented,
Figure GDA0004075706880000085
the tangential angle of the planned path is indicated,
Figure GDA0004075706880000086
and represents the planned path (x d (θ),y d (θ)) in the plan abscissa x d The derivative of (theta) is used,
Figure GDA0004075706880000087
representing a planned path (x d (θ),y d (θ)) in-plan ordinate y d (θ) derivative, e represents empirically set longitudinal tracking error along the planned path, Δ represents empirically set forward looking distance, ΔεR; planning path (x) d (θ),y d (θ)) is empirically set;
specifically, the control parameter Δ=5 in the present embodiment.
Step 22, designing a parameter update law for updating the planned path (x) of the ship in real time d (θ),y d (θ)), the parameter update law is:
Figure GDA0004075706880000091
wherein ,
Figure GDA0004075706880000092
representing a parameter update law, < >>
Figure GDA0004075706880000093
Represents ideal speed, k represents empirically set constant gain parameter, k>0, s represents an empirically set lateral tracking error along the planned path; psi phi type d Representing an empirically set desired heading angle.
Specifically, where the gain parameter k=10.
The man-machine co-fusion guidance module in the step 3 is as follows:
ψ cmd ＝λψ H +(1-λ)ψ LOS (9)
wherein ,ψcmd Represents man-machine co-fusion guidance law, psi H The control instruction set by the driver is represented, and lambda represents a weight factor of the driver participating in dynamic decision; psi phi type LOS Representing a path tracking guidance law.
Preferably, the calculation formula of the weight factor λ of the driver participating in the dynamic decision is:
Figure GDA0004075706880000094
/>
wherein ,
Figure GDA0004075706880000095
distance p between obstacle and ship measured by vision distance sensor o ＝(x o ,y o ) For the position coordinates of the obstacle in the earth's coordinate system, gamma, measured by the line-of-sight sensor ρ As gain parameter ρ safe For a safe distance from an obstacle ρ danger Is the dangerous distance from the obstacle; x represents the x-axis position coordinate of the ship in the earth coordinate system, and y represents the y-axis position coordinate of the ship in the earth coordinate system; safety distance ρ from obstacle safe Dangerous distance ρ from obstacle danger Gain parameter gamma ρ Are all empirically set.
Specifically, the dangerous distance ρ is set in the present embodiment danger =15m, set a security distance ρ safe Control gain parameter gamma =30m ρ ＝5.5。
As shown in fig. 3, λ=0 indicates that there is no risk of collision; λ=1 indicates that the risk of collision is very high, and forced anti-collision measures need to be taken; a 0< lambda <1 indicates that there is a certain risk of collision, and that the input weighting by the human driver is required to smoothly transition from path tracking to collision avoidance maneuvers.
As shown in table 1, parameter information of the obstacle encountered in the unmanned surface vessel collision avoidance experiment is displayed.
TABLE 1 parameter information of unmanned surface vessel encounter obstacle in anti-collision experiment
Figure GDA0004075706880000101
The simulation result is shown in FIG. 3FIG. 3 shows the different control gains gamma ρ A schematic diagram of the observation effect of human participation weight factor lambda is shown; FIG. 4 is a graph of the control effect of heading under guidance law for unmanned surface vessels during anti-collision; FIG. 5 is a schematic illustration of longitudinal and lateral errors in unmanned surface vessel path tracking navigation; fig. 6 is a view of the effect of track observation in the event of an unmanned surface vessel collision. As can be seen by combining fig. 3-6, the unmanned water surface vessel has smooth track during navigation, does not have the phenomenon of drastic change of course angle, effectively completes anti-collision operation and simultaneously can effectively avoid the side turning of the vessel caused by overlarge steering angle, and the like.
The ship aims at avoiding the area with the collision danger in the future, and the position where the collision danger can be generated between the ship and the target ship in the planned navigation track is found, so that the dynamic avoidance problem can be converted into the advanced avoidance problem of the dangerous area in the route. Simulation results prove that the invention provides a man-machine co-fused ship anti-collision control design method, and under the conditions that partial ships such as sensor failure cannot independently collide or avoid collision in real time, the anti-collision tasks are reasonably and safely completed by grading collision risk and weighting and participating in operation of human drivers, so that the ships can safely navigate, and various tasks such as transportation, cruising and monitoring are smoothly completed.
The beneficial effects are that:
1. according to the invention, the man-machine co-fusion guidance module is established, so that the autonomous control authority of the ship during path tracking is maintained, and simultaneously, the human pilot is allowed to participate in dynamic avoidance decision (through designing the man-machine co-fusion guidance law, the instruction of the pilot and the control proportion of the path tracking guidance module to ship navigation) so as to realize continuous authority transfer of the human pilot and the ship automatic driving, so that the ship track is smooth, and the navigation comfort is improved.
2. According to the invention, based on the distance between the unmanned water surface vessel and the obstacle, the activity parameters (the weight factor lambda of the driver participating in dynamic decision) of the human driver are introduced, the collision of the co-driving of the human and the aircraft is alleviated, the restraint can be forced to be executed or the optimal action can be taken when the intention is made, and the coordination capability of the human and the automation is fully exerted.
Finally, it should be noted that: the above embodiments are only for illustrating the technical solution of the present invention, and not for limiting the same; although the invention has been described in detail with reference to the foregoing embodiments, it will be understood by those of ordinary skill in the art that: the technical scheme described in the foregoing embodiments can be modified or some or all of the technical features thereof can be replaced by equivalents; such modifications and substitutions do not depart from the spirit of the invention.

Claims (3)

1. A design method of a ship anti-collision control system is characterized in that,
a marine vessel collision avoidance control system, comprising: the system comprises a ship motion module, a path tracking guidance module and a man-machine co-fusion guidance module;
the ship movement module is used for outputting the position information of the ship; the input end of the ship motion module is connected with the output end of the man-machine co-fusion guidance module, and the output end of the ship motion module is connected with the input ends of the path tracking guidance module and the man-machine co-fusion guidance module;
the path tracking guidance module is used for designing a path tracking guidance law psi LOS And parameterizing update law, controlling a ship navigation path; the input end of the path tracking guidance module is connected with the output end of the ship motion module, and the output end of the path tracking guidance module is connected with the input end of the man-machine co-fusion guidance module;
the man-machine co-fusion guidance module is used for designing a man-machine co-fusion guidance law, controlling an instruction of a driver and controlling the specific gravity of the path tracking guidance module on ship navigation; the input end of the man-machine co-fusion guidance module is connected with the output ends of the ship motion module and the path tracking guidance module, and the output end of the man-machine co-fusion guidance module is connected with the input end of the ship motion module; the design method of the ship anti-collision control system comprises the following steps:
step 1, building a ship movement module and acquiring ship position information (x, y);
step 2, setting ship parameterized path information according to experience, and according to step 1Ship position information and ship parameterized path information, and acquiring path tracking guidance law psi LOS And establishing a module path tracking guidance module according to the parameterized update law;
step 3, tracking a guidance law psi according to the path LOS Based on the apparent distance rho of the target ship and the obstacle, a man-machine co-fusion weight lambda is obtained, a man-machine co-fusion guidance module is established, a man-machine co-fusion guidance law is designed, and the control proportion of the driver's instruction and the path tracking guidance module to ship navigation is controlled; the apparent distance rho between the target ship and the obstacle is acquired by a sensor on the ship;
the man-machine co-fusion guidance module in the step 3 is as follows:
ψ cmd ＝λψ H +(1-λ)ψ LOS (9)
wherein ,ψcmd Represents man-machine co-fusion guidance law, psi H The control instruction set by the driver is represented, and lambda represents a weight factor of the driver participating in dynamic decision; psi phi type LOS Representing a path tracking guidance law;
the calculation formula of the weight factor lambda of the driver participating in the dynamic decision is as follows:
Figure FDA0004067484350000021
wherein ,
Figure FDA0004067484350000022
distance p between obstacle and ship measured by vision distance sensor o ＝(x o ,y o ) For the position coordinates of the obstacle in the earth's coordinate system, gamma, measured by the line-of-sight sensor ρ As gain parameter ρ safe For a safe distance from an obstacle ρ danger Is the dangerous distance from the obstacle; x represents the x-axis position coordinate of the ship in the earth coordinate system, and y represents the y-axis position coordinate of the ship in the earth coordinate system; safety distance ρ from obstacle safe Gain parameter gamma ρ Are all empirically set;
λ=0 indicates that there is no risk of collision; when λ=1, a forced anti-collision measure needs to be taken; when 0< lambda <1, the input weighting of the human driver is needed, and the transition from the path tracking to the collision prevention operation is smooth.
2. The method for designing a ship anti-collision control system according to claim 1, wherein the establishing a ship movement module in step 1 comprises:
step 11, calculating derivative of ship position information:
Figure FDA0004067484350000023
wherein ,
Figure FDA0004067484350000024
derivative of the ship position information (x, y) x-axis position coordinates and y-axis position coordinates, respectively; psi represents the bow angle of the ship under the earth coordinate system, U represents the combined speed of the heave speed and the roll speed of the anti-collision ship, U is empirically set, beta represents the sideslip angle of the anti-collision ship,
the sideslip angle beta of the anti-collision ship is set according to experience, and beta epsilon (-pi, pi ];
and 12, automatically generating an x-axis position coordinate x of the ship and a y-axis position coordinate y of the ship according to the ship position coordinate derivative.
3. The method for designing a ship anti-collision control system according to claim 1, wherein the step 2 of establishing a path-tracking guidance module comprises:
step 21, designing a path tracking guidance law, wherein the path tracking guidance law is used for controlling the ship heading:
Figure FDA0004067484350000025
wherein ,ψLOS The path-tracking guidance law is represented,
Figure FDA0004067484350000026
the tangential angle of the planned path is indicated,
Figure FDA0004067484350000027
and represents the planned path (x d (θ),y d (θ)) in the plan abscissa x d The derivative of (theta) is used,
Figure FDA0004067484350000031
representing a planned path (x d (θ),y d (θ)) in-plan ordinate y d (θ) derivative, e represents empirically set longitudinal tracking error along the planned path, Δ represents empirically set forward looking distance, ΔεR; planning path (x) d (θ),y d (θ)) is empirically set;
step 22, designing a parameter update law for updating the planned path (x) of the ship in real time d (θ),y d (θ)), the parameter update law is:
Figure FDA0004067484350000032
wherein ,
Figure FDA0004067484350000033
representing a parameter update law, < >>
Figure FDA0004067484350000034
Represents ideal speed, k represents empirically set constant gain parameter, k>0, s represents an empirically set lateral tracking error along the planned path; psi phi type d Representing an empirically set desired heading angle. />
CN202210102027.6A 2022-01-27 2022-01-27 Design method of ship anti-collision control system Active CN114408122B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210102027.6A CN114408122B (en) 2022-01-27 2022-01-27 Design method of ship anti-collision control system

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210102027.6A CN114408122B (en) 2022-01-27 2022-01-27 Design method of ship anti-collision control system

Publications (2)

Publication Number Publication Date
CN114408122A CN114408122A (en) 2022-04-29
CN114408122B true CN114408122B (en) 2023-04-25

Family

ID=81278935

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210102027.6A Active CN114408122B (en) 2022-01-27 2022-01-27 Design method of ship anti-collision control system

Country Status (1)

Country Link
CN (1) CN114408122B (en)

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US7446291B1 (en) * 2005-10-03 2008-11-04 Lockheed Martin Corporation Augmented proportional navigation guidance law using angular acceleration measurements
CN109976340B (en) * 2019-03-19 2022-02-08 中国人民解放军国防科技大学 Man-machine cooperation dynamic obstacle avoidance method and system based on deep reinforcement learning
CN111409695B (en) * 2020-04-13 2021-07-27 安徽卡思普智能科技有限公司 Steering-by-wire man-machine sharing control method for intelligent automobile and intelligent automobile
CN113173232B (en) * 2021-03-08 2022-06-14 武汉理工大学 Intelligent ship auxiliary driving control system and method based on man-machine integration
CN113093742B (en) * 2021-03-30 2023-05-26 大连海事大学 Unmanned ship path tracking system capable of automatically avoiding multiple obstacles
CN113101079A (en) * 2021-05-20 2021-07-13 南京邮电大学 Intelligent wheelchair based on multiple constraint conditions, and dynamic sharing control method and system

Also Published As

Publication number Publication date
CN114408122A (en) 2022-04-29

Similar Documents

Publication Publication Date Title
CN110262492B (en) Real-time collision avoidance and target tracking method for unmanned ship
CN109828570B (en) Control and guide method for self-adaptive boundary layer water surface unmanned ship
Statheros et al. Autonomous ship collision avoidance navigation concepts, technologies and techniques
WO2021230356A1 (en) Method for autonomously guiding vessel, program for autonomously guiding vessel, system for autonomously guiding vessel, and vessel
Moreira et al. Path following control system for a tanker ship model
Zhang et al. Novel DVS guidance and path-following control for underactuated ships in presence of multiple static and moving obstacles
US11804137B1 (en) Marine propulsion control system and method with collision avoidance override
US11600184B2 (en) Marine propulsion control system and method
CN109960262B (en) Unmanned ship dynamic obstacle avoidance method and system based on geometric method
CN113093804B (en) Unmanned ship formation control method and control system based on inversion sliding mode control
US11862026B2 (en) Marine propulsion control system and method with proximity-based velocity limiting
WO2016109601A1 (en) Proactive directional control systems and methods
CN115963841A (en) Unmanned sailing ship dynamic event trigger control method based on improved LVS guidance
CN113359737A (en) Ship formation self-adaptive event trigger control method considering formation expansion
Wang et al. A distributed model predictive control using virtual field force for multi-ship collision avoidance under COLREGs
JP6686249B2 (en) Ship automatic control system, ship, and automatic ship control method
CN113126492B (en) Automatic path tracking method for dynamic positioning ship
CN114408122B (en) Design method of ship anti-collision control system
CN110928278B (en) Autonomous safe navigation method for unmanned ship and unmanned ship
Qu et al. Simultaneous planning and executing trajectory tracking control for underactuated unmanned surface vehicles from theory to practice
CN115145282A (en) Automatic berthing robust self-adaptive control method for under-actuated ship
Miller Model predictive ship trajectory tracking system based on line of sight method
Kula Heading control system with limited turning radius based on IMC structure
Piaggio et al. Follow-the-Leader Guidance, Navigation, and Control of Surface Vessels: Design and Experiments
JPH0330557B2 (en)

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant