CN113031598B - Path tracking obstacle avoidance guidance method and system - Google Patents
Path tracking obstacle avoidance guidance method and system Download PDFInfo
- Publication number
- CN113031598B CN113031598B CN202110228719.0A CN202110228719A CN113031598B CN 113031598 B CN113031598 B CN 113031598B CN 202110228719 A CN202110228719 A CN 202110228719A CN 113031598 B CN113031598 B CN 113031598B
- Authority
- CN
- China
- Prior art keywords
- ship
- obstacle
- module
- navigation
- obstacle avoidance
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 34
- 238000007781 pre-processing Methods 0.000 claims abstract description 22
- 238000004891 communication Methods 0.000 claims abstract description 17
- 238000013500 data storage Methods 0.000 claims abstract description 11
- 230000008569 process Effects 0.000 claims abstract description 6
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 claims description 34
- 238000012544 monitoring process Methods 0.000 claims description 23
- 239000011159 matrix material Substances 0.000 claims description 22
- 238000012545 processing Methods 0.000 claims description 22
- 238000001514 detection method Methods 0.000 claims description 12
- 230000003068 static effect Effects 0.000 claims description 12
- 230000007613 environmental effect Effects 0.000 claims description 8
- 238000001914 filtration Methods 0.000 claims description 7
- 238000009499 grossing Methods 0.000 claims description 6
- 238000005259 measurement Methods 0.000 claims description 6
- 238000005070 sampling Methods 0.000 claims description 6
- 230000011218 segmentation Effects 0.000 claims description 6
- 230000005540 biological transmission Effects 0.000 claims description 4
- 230000006870 function Effects 0.000 claims description 4
- 238000004590 computer program Methods 0.000 claims description 3
- 238000011156 evaluation Methods 0.000 claims description 3
- 238000003384 imaging method Methods 0.000 claims description 3
- 230000000877 morphologic effect Effects 0.000 claims description 3
- 238000000605 extraction Methods 0.000 claims 1
- 230000000007 visual effect Effects 0.000 abstract description 2
- 230000004888 barrier function Effects 0.000 description 2
- 230000010355 oscillation Effects 0.000 description 2
- 230000007547 defect Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 239000000284 extract Substances 0.000 description 1
- 239000000835 fiber Substances 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 238000012423 maintenance Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000008092 positive effect Effects 0.000 description 1
- 230000002265 prevention Effects 0.000 description 1
- 239000004065 semiconductor Substances 0.000 description 1
- 238000012163 sequencing technique Methods 0.000 description 1
- 239000007787 solid Substances 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/0206—Control of position or course in two dimensions specially adapted to water vehicles
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Traffic Control Systems (AREA)
Abstract
The invention belongs to the technical field of ship control engineering and ship automatic navigation equipment application, and discloses a path tracking obstacle avoidance guidance method and a system, wherein the path tracking obstacle avoidance guidance system comprises the following components: the system comprises an image data acquisition module, a DSP image preprocessing module, a navigation parameter determining module, a wireless communication module, a central control module, a path planning module, a path tracking control module, an obstacle avoidance control guidance module, a motion control module, a data storage module and an updating display module. The invention carries out navigation by tracking the pre-aiming point based on machine vision, has simple operation and strong applicability; navigation is performed by controlling the position relation between the pre-aiming point and the right edge of the road, so that the instantaneity and the simplicity of ship navigation are improved; meanwhile, the obstacle information of the road is perceived through the ultrasonic sensor, so that the safety and reliability of the visual navigation of the ship are guaranteed, the problem of reality that the ship avoids obstacles on multiple targets in the path tracking process is solved, and the automation degree of the ship is improved.
Description
Technical Field
The invention belongs to the technical field of ship control engineering and ship automatic navigation equipment application, and particularly relates to a path tracking obstacle avoidance guidance method and system.
Background
At present, the realization of the track maintenance and collision prevention/obstacle avoidance of a ship through a guidance strategy is a key technology for controlling the motion of the ship, and the traditional ship path planning based on the waypoints requires the ship to automatically navigate along a set route and is attributed to the guidance problem of path tracking. However, the existing path tracking obstacle avoidance guidance system is difficult to carry out path planning, and is easy to lose stability when passing through a narrow channel, so that oscillation is caused, and the system is not suitable for the actual sea conditions with variability. Therefore, a new path tracking obstacle avoidance guidance method and system are needed.
Through the above analysis, the problems and defects existing in the prior art are as follows: the conventional path tracking obstacle avoidance guidance system is difficult to carry out path planning, and is easy to lose stability when passing through a narrow channel, so that oscillation is caused, and the system is not suitable for the real sea condition with variability.
Disclosure of Invention
Aiming at the problems existing in the prior art, the invention provides a path tracking obstacle avoidance guidance method and a system.
The invention is realized in such a way that a path tracking obstacle avoidance guidance system comprises:
the image data acquisition module is connected with the central control module and used for acquiring the route information of the ship and the environment monitoring information of the water area around the ship in real time through the CCD camera;
the DSP image preprocessing module is connected with the central control module and is used for carrying out filtering, graying, threshold segmentation and morphological preprocessing on the acquired ship route information and the environmental monitoring information of the water area around the ship through the digital image processor DSP;
the filtering processing of the collected ship route information and the environment monitoring information of the surrounding water area of the ship by the digital image processor DSP comprises the following steps:
(1) Establishing a weighted Gaussian smoothing filter matrix model:
wherein Q is a filter matrix and Q is a matrix of 1*n; n is a matrix size threshold; i is the relative coordinate value from the center coordinate point, i.e. Q [ i ] is the weight difference of the coordinate point relative to the center point;
(2) Calculating the difference sum of the Gaussian smoothing center point relative to the left threshold value and the right threshold value;
wherein, S [ k ] is the sum of the differences of the central point relative to the left and right threshold values; buf [ k ] is a sample measurement value of the central point; n is the filter matrix size;
(3) Calculating sample values after Gaussian smoothing:
wherein buf' k is the value after the central point processing; buf [ k ] is a sample measurement value of the central point; n is the filter matrix size;
the navigation parameter determining module is connected with the central control module and is used for extracting the ship aiming point coordinates according to the preprocessed image information through a parameter determining program so as to determine the ship navigation parameters;
the wireless communication module is connected with the central control module and is used for carrying out wireless transmission of the path tracking obstacle avoidance guidance system data through wireless communication equipment;
the central control module is connected with the image data acquisition module, the DSP image preprocessing module, the navigation parameter determining module, the wireless communication module, the path planning module, the path tracking control module, the obstacle avoidance control guidance module, the motion control module, the data storage module and the updating display module, and is used for controlling the normal operation of each module of the path tracking obstacle avoidance guidance system in a coordinated manner through the central processing unit and controlling the autonomous navigation of the ship tracking pre-aiming point;
the path planning module is connected with the central control module and is used for planning the navigation route of the ship through a path planning program and determining the optimal navigation track of the ship;
the path tracking control module is connected with the central control module and is used for acquiring the real-time navigation track of the ship and carrying out path tracking control on the ship according to the determined optimal navigation track of the ship through a path tracking control program;
the obstacle avoidance control guidance module is connected with the central control module and used for sensing obstacle information of a water area around the ship, processing obstacle signals according to an obstacle priority sequence through an obstacle avoidance control guidance program, realizing obstacle avoidance control guidance of the ship and carrying out safety early warning notification;
the motion control module is connected with the central control module and used for controlling the overall motion of the ship in real time according to the navigation track and obstacle avoidance control guidance information of the ship through a motion control program;
the data storage module is connected with the central control module and used for storing the acquired route information of the ship, the environment monitoring information of the surrounding water area of the ship, the DSP image preprocessing result, the ship navigation parameters, the optimal navigation track, the path tracking control information, the obstacle avoidance control guidance information, the safety early warning notification and the ship motion control information through the memory;
the updating display module is connected with the central control module and is used for updating and displaying the acquired route information of the ship, the environment monitoring information of the surrounding water area of the ship, the DSP image preprocessing result, the ship navigation parameters, the optimal navigation track, the path tracking control information, the obstacle avoidance control guidance information, the safety early warning notification and the real-time data of the ship motion control information through the display.
Further, the graying processing of the environmental information includes:
in the second step, the threshold segmentation is performed on the collected ship route information and the environmental monitoring information of the surrounding water area of the ship, and the method includes:
the method comprises the steps of calculating the inter-class variance by taking each column as a whole, dividing the inter-class variance by using the optimal threshold value, combining the images subjected to the threshold value division in each column, and outputting the optimal threshold value division image.
In the third step, the navigation parameter determining module extracts the coordinates of the aiming point of the ship according to the preprocessed image information by using a parameter determining program, so as to determine the navigation parameters of the ship, and the method comprises the following steps:
(1) Extracting path edge information and pre-aiming point coordinates of the image through a navigation parameter determining module;
(2) Combining with calibration parameters of the CCD camera, and finally determining position information of a pre-aiming point in a world coordinate system and navigation parameters for tracking the pre-aiming point by utilizing a parameter determination program;
(3) And transmitting the determined navigation parameters of the ship to the central processing unit through the wireless communication module.
In a fourth step, the controlling, by the central control module, autonomous navigation of the ship tracking pre-aiming point by using the central processor includes:
(1) After receiving a ship navigation parameter signal transmitted by a digital image processor DSP, the central processing unit takes navigation parameters as input of a fuzzy-PID hybrid controller;
(2) The controller outputs a correct control signal to control a driving motor of the ship, so as to control the ship to track the pre-aiming point for autonomous navigation.
In a sixth step, the obstacle information of the water area around the ship is sensed by the obstacle avoidance control guidance module, and the obstacle signal is processed according to the obstacle priority sequence by using the obstacle avoidance control guidance program, so as to realize the obstacle avoidance control guidance of the ship, including:
(1) Sensing obstacle information of a water area around the ship by using obstacle detection equipment through an obstacle avoidance control guidance module; wherein the obstacle detection device comprises a navigation radar, an imaging device and an ultrasonic sensor;
(2) Setting navigation point information, sampling at a sampling time point, and judging whether a ship enters an obstacle detection ring or not; if the obstacle detection ring is entered, entering an obstacle sorting process;
(3) The priority of the obstacle avoidance objects is ordered to obtain a priority sequence of the obstacle, and the obstacle with the highest priority is selected as the current obstacle avoidance target, so that obstacle avoidance control guidance of the ship is realized.
Further, the step of sorting the priorities of the obstacle avoidance objects to obtain a priority sequence of the obstacle includes:
1) Dividing the obstacle into a static obstacle and a dynamic obstacle, wherein the priority of the static obstacle is higher than that of the dynamic obstacle;
2) The priority of a static obstacle is inversely proportional to the distance e of the dynamic vessel from the safety limit cycle of the static obstacle, and the obstacle with the smallest e has the highest priority.
Further, the priority of the dynamic barrier is determined by the following formula:
wherein F is an evaluation function, R m For detecting the radius of the ring, R o E is the distance from the dynamic virtual boat to the safety limit ring of the dynamic obstacle,the derivative of the distance between the dynamic virtual boat and the obstacle in the path tracking mode; u (u) do The obstacle avoidance speed of the dynamic virtual boat is represented as a constant, and the value of the obstacle avoidance speed is larger than the speeds of all dynamic obstacles; />Is a weight parameter.
It is another object of the present invention to provide a computer program product stored on a computer readable medium, comprising a computer readable program for providing a user input interface for implementing the path tracking obstacle avoidance guidance system when executed on an electronic device.
It is another object of the present invention to provide a computer readable storage medium storing instructions that, when executed on a computer, cause the computer to perform the path tracking obstacle avoidance guidance system.
By combining all the technical schemes, the invention has the advantages and positive effects that: the path tracking obstacle avoidance guidance method provided by the invention is based on machine vision, and navigation is performed by tracking the pre-aiming point, so that the operation is simple and convenient, and the applicability is strong; navigation is performed by controlling the position relation between the pre-aiming point and the right edge of the road, so that the instantaneity and the simplicity of ship navigation are greatly improved; the obstacle information of the road is sensed through the ultrasonic sensor, safety pre-warning is carried out, the safety and reliability of ship visual navigation are guaranteed, the problem that the ship keeps away from the obstacle for multiple targets in the path tracking process is solved, and the automation degree of the ship is improved.
Drawings
In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings that are needed in the embodiments of the present invention will be briefly described below, and it is obvious that the drawings described below are only some embodiments of the present invention, and other drawings may be obtained according to these drawings without inventive effort for a person skilled in the art.
FIG. 1 is a block diagram of a path tracking obstacle avoidance guidance system provided by an embodiment of the present invention;
in the figure: 1. an image data acquisition module; 2. a DSP image preprocessing module; 3. a navigation parameter determining module; 4. a wireless communication module; 5. a central control module; 6. a path planning module; 7. a path tracking control module; 8. obstacle avoidance control guidance module; 9. a motion control module; 10. a data storage module; 11. updating the display module.
Fig. 2 is a flowchart of a path tracking obstacle avoidance guidance method provided by an embodiment of the present invention.
Fig. 3 is a flowchart of a method for determining navigation parameters of a ship by extracting coordinates of an aiming point of the ship according to preprocessed image information by using a navigation parameter determining module according to an embodiment of the present invention.
Fig. 4 is a flowchart of a method for controlling autonomous navigation of a ship tracking pretightening point by a central control module using a central processor according to an embodiment of the present invention.
Fig. 5 is a flowchart of a method for realizing obstacle avoidance control guidance of a ship by sensing obstacle information of a water area around the ship through an obstacle avoidance control guidance module and processing obstacle signals according to an obstacle priority sequence by using an obstacle avoidance control guidance program.
Detailed Description
The present invention will be described in further detail with reference to the following examples in order to make the objects, technical solutions and advantages of the present invention more apparent. It should be understood that the specific embodiments described herein are for purposes of illustration only and are not intended to limit the scope of the invention.
Aiming at the problems existing in the prior art, the invention provides a path tracking obstacle avoidance guidance method and a system, and the invention is described in detail below with reference to the accompanying drawings.
As shown in fig. 1, the path tracking obstacle avoidance guidance system provided by the embodiment of the invention includes: the system comprises an image data acquisition module 1, a DSP image preprocessing module 2, a navigation parameter determination module 3, a wireless communication module 4, a central control module 5, a path planning module 6, a path tracking control module 7, an obstacle avoidance control guidance module 8, a motion control module 9, a data storage module 10 and an updating display module 11.
The image data acquisition module 1 is connected with the central control module 5 and is used for acquiring the route information of the ship and the environment monitoring information of the water area around the ship in real time through the CCD camera;
the DSP image preprocessing module 2 is connected with the central control module 5 and is used for preprocessing the collected ship route information and the environment monitoring information of the water area around the ship through the digital image processor DSP, such as filtering, graying, threshold segmentation and morphology;
the navigation parameter determining module 3 is connected with the central control module 5 and is used for extracting the ship aiming point coordinates according to the preprocessed image information through a parameter determining program so as to determine the ship navigation parameters;
the wireless communication module 4 is connected with the central control module 5 and is used for carrying out wireless transmission of the path tracking obstacle avoidance guidance system data through wireless communication equipment;
the central control module 5 is connected with the image data acquisition module 1, the DSP image preprocessing module 2, the navigation parameter determining module 3, the wireless communication module 4, the path planning module 6, the path tracking control module 7, the obstacle avoidance control guidance module 8, the motion control module 9, the data storage module 10 and the updating display module 11 and is used for controlling the normal operation of each module of the path tracking obstacle avoidance guidance system in a coordinated manner through the central processing unit and controlling the autonomous navigation of a ship tracking pre-aiming point;
the path planning module 6 is connected with the central control module 5 and is used for planning the navigation route of the ship through a path planning program to determine the optimal navigation track of the ship;
the path tracking control module 7 is connected with the central control module 5 and is used for acquiring the real-time navigation track of the ship and carrying out path tracking control on the ship according to the determined optimal navigation track of the ship through a path tracking control program;
the obstacle avoidance control guidance module 8 is connected with the central control module 5 and is used for sensing obstacle information of a surrounding water area of the ship, processing obstacle signals according to an obstacle priority sequence through an obstacle avoidance control guidance program, realizing obstacle avoidance control guidance of the ship and carrying out safety early warning notification;
the motion control module 9 is connected with the central control module 5 and is used for controlling the overall motion of the ship in real time according to the navigation track and obstacle avoidance control guidance information of the ship through a motion control program;
the data storage module 10 is connected with the central control module 5 and is used for storing the acquired route information of the ship, the environment monitoring information of the surrounding water area of the ship, the DSP image preprocessing result, the ship navigation parameters, the optimal navigation track, the path tracking control information, the obstacle avoidance control guidance information, the safety early warning notification and the ship motion control information through the memory;
the updating display module 11 is connected with the central control module 5 and is used for updating and displaying the acquired route information of the ship, the environment monitoring information of the surrounding water area of the ship, the DSP image preprocessing result, the ship navigation parameters, the optimal navigation track, the path tracking control information, the obstacle avoidance control guidance information, the safety early warning notification and the real-time data of the ship motion control information through a display.
As shown in fig. 2, the path tracking obstacle avoidance guidance method provided by the embodiment of the invention includes the following steps:
s101, acquiring the route information of a ship and the environment monitoring information of the water area around the ship in real time by using a CCD camera through an image data acquisition module;
s102, filtering, graying, threshold segmentation and morphological preprocessing are carried out on collected ship route information and environment monitoring information of a ship surrounding water area by using a digital image processor DSP through a DSP image preprocessing module;
s103, extracting the ship aiming point coordinates according to the preprocessed image information by using a navigation parameter determining module by using a parameter determining program, so as to determine ship navigation parameters; the wireless communication module is used for carrying out wireless transmission on the path tracking obstacle avoidance guidance system data by utilizing wireless communication equipment;
s104, the central control module is used for cooperatively controlling the normal operation of each module of the path tracking obstacle avoidance guidance system by using the central processor, and controlling the autonomous navigation of the ship tracking pre-aiming point; planning a navigation route of the ship by using a path planning program through a path planning module, and determining an optimal navigation track of the ship;
s105, acquiring a real-time navigation track of the ship through a path tracking control module, and performing path tracking control of the ship according to the determined optimal navigation track of the ship by utilizing a path tracking control program;
s106, sensing obstacle information of a water area around the ship through an obstacle avoidance control guidance module, processing obstacle signals according to an obstacle priority sequence by using an obstacle avoidance control guidance program, realizing obstacle avoidance control guidance of the ship, and carrying out safety early warning notification;
s107, controlling the overall motion of the ship in real time by a motion control module according to the navigation track and obstacle avoidance control guidance information of the ship by using a motion control program;
s108, storing the acquired route information of the ship, the environment monitoring information of the surrounding water area of the ship, the DSP image preprocessing result, the ship navigation parameters, the optimal navigation track, the path tracking control information, the obstacle avoidance control guidance information, the safety early warning notification and the ship motion control information by using a memory through a data storage module;
and S109, updating and displaying the acquired line information of the ship, the environmental monitoring information of the water area around the ship, the DSP image preprocessing result, the ship navigation parameters, the optimal navigation track, the path tracking control information, the obstacle avoidance control guidance information, the safety early warning notification and the real-time data of the ship motion control information by using an updating and displaying module.
The technical scheme of the invention is further described below with reference to specific embodiments.
Example 1
As shown in FIG. 2, as a preferred embodiment, the method for performing filtering processing on the acquired ship route information and the environmental monitoring information of the water area around the ship by using the digital image processor DSP according to the embodiment of the invention comprises the following steps:
(1) Establishing a weighted Gaussian smoothing filter matrix model:
wherein Q is a filter matrix and Q is a matrix of 1*n; n is a matrix size threshold; i is the relative coordinate value from the center coordinate point, i.e. Q [ i ] is the weight difference of the coordinate point relative to the center point;
(2) Calculating the difference sum of the Gaussian smoothing center point relative to the left threshold value and the right threshold value;
wherein, S [ k ] is the sum of the differences of the central point relative to the left and right threshold values; buf [ k ] is a sample measurement value of the central point; n is the filter matrix size;
(3) Calculating sample values after Gaussian smoothing:
wherein buf' k is the value after the central point processing; buf [ k ] is a sample measurement value of the central point; n is the filter matrix size.
The method for carrying out gray processing on the acquired ship route information and the environment monitoring information of the water area around the ship by the digital image processor DSP comprises the following steps:
acquiring gray values of pixels in the filtered image to obtain a gray matrix; obtaining an equalization array according to the gray level distribution trend of the rows/columns in the gray level matrix; and correcting the gray matrix according to the balance array to obtain the corrected gray matrix of the image.
Example 2
As shown in FIG. 2, the method for path tracking obstacle avoidance guidance provided by the embodiment of the invention, as a preferred embodiment, performs threshold segmentation on the acquired ship route information and the environmental monitoring information of the water area around the ship, and comprises the following steps:
the method comprises the steps of calculating the inter-class variance by taking each column as a whole, dividing the inter-class variance by using the optimal threshold value, combining the images subjected to the threshold value division in each column, and outputting the optimal threshold value division image.
Example 3
The path tracking obstacle avoidance guidance method provided by the embodiment of the invention is shown in fig. 2, and as a preferred embodiment, as shown in fig. 3, the method provided by the embodiment of the invention uses a parameter determination program to extract the coordinates of aiming points of a ship according to preprocessed image information by a navigation parameter determination module, and further determines navigation parameters of the ship, and comprises the following steps:
s201, extracting path edge information and pre-aiming point coordinates of an image through a navigation parameter determining module;
s202, combining calibration parameters of a CCD camera, and finally determining position information of a pre-aiming point in a world coordinate system and navigation parameters for tracking the pre-aiming point by utilizing a parameter determination program;
and S203, transmitting the determined ship navigation parameters to a central processing unit through a wireless communication module.
Example 4
The path tracking obstacle avoidance guidance method provided by the embodiment of the invention is shown in fig. 2, and as a preferred embodiment, as shown in fig. 4, the autonomous navigation of the ship tracking pre-aiming point is controlled by the central control module through the central processor, and the method comprises the following steps:
s301, after receiving a ship navigation parameter signal transmitted by a digital image processor DSP, a central processing unit takes navigation parameters as input of a fuzzy-PID hybrid controller;
s302, outputting a correct control signal through a controller to control a driving motor of the ship, and further controlling the ship to track and pre-aim the point for autonomous navigation.
Example 5
The path tracking obstacle avoidance guidance method provided by the embodiment of the invention is shown in fig. 2, and as a preferred embodiment, as shown in fig. 5, the obstacle information of the water area around the ship is sensed by the obstacle avoidance guidance module, and the obstacle signal is processed according to the obstacle priority sequence by using the obstacle avoidance guidance program, so as to realize the obstacle avoidance guidance of the ship, and the path tracking obstacle avoidance guidance method comprises the following steps:
s401, sensing obstacle information of a water area around a ship by using obstacle detection equipment through an obstacle avoidance control guidance module; wherein the obstacle detection device comprises a navigation radar, an imaging device and an ultrasonic sensor;
s402, setting navigation point information, sampling at a sampling time point, and judging whether a ship enters an obstacle detection ring or not; if the obstacle detection ring is entered, entering an obstacle sorting process;
s403, sorting the priority of the obstacle avoidance objects to obtain a priority sequence of the obstacle, and selecting the obstacle with the highest priority as the current obstacle avoidance target to realize obstacle avoidance control guidance of the ship.
The method for sequencing the priority of the obstacle avoidance object provided by the embodiment of the invention to obtain the priority sequence of the obstacle comprises the following steps:
1) Dividing the obstacle into a static obstacle and a dynamic obstacle, wherein the priority of the static obstacle is higher than that of the dynamic obstacle;
2) The priority of a static obstacle is inversely proportional to the distance e of the dynamic vessel from the safety limit cycle of the static obstacle, and the obstacle with the smallest e has the highest priority.
The priority of the dynamic barrier provided by the embodiment of the invention is determined by the following formula:
wherein F is an evaluation function, R m For detecting the radius of the ring, R o E is the distance from the dynamic virtual boat to the safety limit ring of the dynamic obstacle,the derivative of the distance between the dynamic virtual boat and the obstacle in the path tracking mode; u (u) do The obstacle avoidance speed of the dynamic virtual boat is represented as a constant, and the value of the obstacle avoidance speed is larger than the speeds of all dynamic obstacles; />Is a weight parameter.
In the above embodiments, it may be implemented in whole or in part by software, hardware, firmware, or any combination thereof. When used in whole or in part, is implemented in the form of a computer program product comprising one or more computer instructions. When loaded or executed on a computer, produces a flow or function in accordance with embodiments of the present invention, in whole or in part. The computer may be a general purpose computer, a special purpose computer, a computer network, or other programmable apparatus. The computer instructions may be stored in a computer-readable storage medium or transmitted from one computer-readable storage medium to another computer-readable storage medium, for example, the computer instructions may be transmitted from one website, computer, server, or data center to another website, computer, server, or data center by a wired (e.g., coaxial cable, fiber optic, digital Subscriber Line (DSL), or wireless (e.g., infrared, wireless, microwave, etc.) means. The computer readable storage medium may be any available medium that can be accessed by a computer or a data storage device such as a server, data center, etc. that contains an integration of one or more available media. The usable medium may be a magnetic medium (e.g., floppy Disk, hard Disk, magnetic tape), an optical medium (e.g., DVD), or a semiconductor medium (e.g., solid State Disk (SSD)), etc.
The foregoing is merely illustrative of specific embodiments of the present invention, and the scope of the invention is not limited thereto, but any modifications, equivalents, improvements and alternatives falling within the spirit and principles of the present invention will be apparent to those skilled in the art within the scope of the present invention.
Claims (9)
1. A path tracking obstacle avoidance guidance system, the path tracking obstacle avoidance guidance system comprising:
the image data acquisition module is connected with the central control module and used for acquiring the route information of the ship and the environment monitoring information of the water area around the ship in real time through the CCD camera;
the DSP image preprocessing module is connected with the central control module and is used for carrying out filtering, graying, threshold segmentation and morphological preprocessing on the acquired ship route information and the environmental monitoring information of the water area around the ship through the digital image processor DSP;
the filtering processing of the collected ship route information and the environment monitoring information of the surrounding water area of the ship by the digital image processor DSP comprises the following steps:
(1) Establishing a weighted Gaussian smoothing filter matrix model:
wherein: q is a filter matrix, Q is a matrix of 1*n; n is a matrix size threshold; i is the relative coordinate value from the center coordinate point, i.e. Q [ i ] is the weight difference of the coordinate point relative to the center point;
(2) Calculating the difference sum of the Gaussian smoothing center point relative to the left threshold value and the right threshold value;
wherein, S [ k ] is the difference sum of the center point relative to the left and right threshold values; buf [ k ] is a sample measurement value of the central point; n is the filter matrix size;
(3) Calculating sample values after Gaussian smoothing:
wherein: buf' k is the value after the center point processing; buf [ k ] is a sample measurement value of the central point; n is the filter matrix size;
the navigation parameter determining module is connected with the central control module and is used for extracting the ship aiming point coordinates according to the preprocessed image information through a parameter determining program so as to determine the ship navigation parameters;
the wireless communication module is connected with the central control module and is used for carrying out wireless transmission of the path tracking obstacle avoidance guidance system data through wireless communication equipment;
the central control module is connected with the image data acquisition module, the DSP image preprocessing module, the navigation parameter determining module, the wireless communication module, the path planning module, the path tracking control module, the obstacle avoidance control guidance module, the motion control module, the data storage module and the updating display module, and is used for controlling the normal operation of each module of the path tracking obstacle avoidance guidance system in a coordinated manner through the central processing unit and controlling the autonomous navigation of the ship tracking pre-aiming point;
the path planning module is connected with the central control module and is used for planning the navigation route of the ship through a path planning program and determining the optimal navigation track of the ship;
the path tracking control module is connected with the central control module and is used for acquiring the real-time navigation track of the ship and carrying out path tracking control on the ship according to the determined optimal navigation track of the ship through a path tracking control program;
the obstacle avoidance control guidance module is connected with the central control module and used for sensing obstacle information of a water area around the ship, processing obstacle signals according to an obstacle priority sequence through an obstacle avoidance control guidance program, realizing obstacle avoidance control guidance of the ship and carrying out safety early warning notification;
the motion control module is connected with the central control module and used for controlling the overall motion of the ship in real time according to the navigation track and obstacle avoidance control guidance information of the ship through a motion control program;
the data storage module is connected with the central control module and used for storing the acquired route information of the ship, the environment monitoring information of the surrounding water area of the ship, the DSP image preprocessing result, the ship navigation parameters, the optimal navigation track, the path tracking control information, the obstacle avoidance control guidance information, the safety early warning notification and the ship motion control information through the memory;
the updating display module is connected with the central control module and is used for updating and displaying the acquired route information of the ship, the environment monitoring information of the surrounding water area of the ship, the DSP image preprocessing result, the ship navigation parameters, the optimal navigation track, the path tracking control information, the obstacle avoidance control guidance information, the safety early warning notification and the real-time data of the ship motion control information through the display.
2. The path-following obstacle avoidance guidance system of claim 1 wherein thresholding the collected vessel route information and environmental monitoring information of the body of water surrounding the vessel comprises:
the method comprises the steps of calculating the inter-class variance by taking each column as a whole, dividing the inter-class variance by using the optimal threshold value, combining the images subjected to the threshold value division in each column, and outputting the optimal threshold value division image.
3. The path-following obstacle avoidance guidance system of claim 1 wherein the navigation parameter determination module performs extraction of vessel aiming coordinates from the preprocessed image information using a parameter determination program to determine vessel navigation parameters, comprising:
(1) Extracting path edge information and pre-aiming point coordinates of the image through a navigation parameter determining module;
(2) Combining with calibration parameters of the CCD camera, and finally determining position information of a pre-aiming point in a world coordinate system and navigation parameters for tracking the pre-aiming point by utilizing a parameter determination program;
(3) And transmitting the determined navigation parameters of the ship to the central processing unit through the wireless communication module.
4. The path-tracking obstacle-avoidance guidance system of claim 1 wherein the central control module controls autonomous navigation of the vessel tracking pre-aiming point with the central processor, comprising:
(1) After receiving a ship navigation parameter signal transmitted by a digital image processor DSP, the central processing unit takes navigation parameters as input of a fuzzy-PID hybrid controller;
(2) The controller outputs a correct control signal to control a driving motor of the ship, so as to control the ship to track the pre-aiming point for autonomous navigation.
5. The path-following obstacle avoidance guidance system of claim 1, wherein the obstacle avoidance maneuver guidance module senses obstacle information in the water surrounding the vessel and processes the obstacle signals according to the obstacle priority sequence using the obstacle avoidance maneuver guidance program to implement obstacle avoidance maneuver guidance for the vessel, comprising:
(1) Sensing obstacle information of a water area around the ship by using obstacle detection equipment through an obstacle avoidance control guidance module; wherein the obstacle detection device comprises a navigation radar, an imaging device and an ultrasonic sensor;
(2) Setting navigation point information, sampling at a sampling time point, and judging whether a ship enters an obstacle detection ring or not; if the obstacle detection ring is entered, entering an obstacle sorting process;
(3) The priority of the obstacle avoidance objects is ordered to obtain a priority sequence of the obstacle, and the obstacle with the highest priority is selected as the current obstacle avoidance target, so that obstacle avoidance control guidance of the ship is realized.
6. The path-following obstacle avoidance guidance system of claim 5 wherein the ordering of the priorities of the obstacles to obtain a priority sequence of the obstacles comprises:
1) Dividing the obstacle into a static obstacle and a dynamic obstacle, wherein the priority of the static obstacle is higher than that of the dynamic obstacle;
2) The priority of a static obstacle is inversely proportional to the distance e of the dynamic vessel from the safety limit cycle of the static obstacle, and the obstacle with the smallest e has the highest priority.
7. The path tracking obstacle avoidance guidance system of claim 6 wherein the priority of said dynamic obstacle is determined by the following equation:
wherein F is an evaluation function, R m For detecting the radius of the ring, R o E is the distance from the dynamic virtual boat to the safety limit ring of the dynamic obstacle,on-road for dynamic virtual boatDerivative of its distance from the obstacle in the path tracking mode; u (u) do The obstacle avoidance speed of the dynamic virtual boat is represented as a constant, and the value of the obstacle avoidance speed is larger than the speeds of all dynamic obstacles; />Is a weight parameter.
8. A computer program product stored on a computer readable medium, comprising a computer readable program for providing a user input interface to implement the path tracking obstacle avoidance guidance system of any of claims 1 to 7 when executed on an electronic device.
9. A computer readable storage medium storing instructions which, when run on a computer, cause the computer to perform the path tracking obstacle avoidance guidance system of any of claims 1 to 7.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110228719.0A CN113031598B (en) | 2021-03-02 | 2021-03-02 | Path tracking obstacle avoidance guidance method and system |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110228719.0A CN113031598B (en) | 2021-03-02 | 2021-03-02 | Path tracking obstacle avoidance guidance method and system |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113031598A CN113031598A (en) | 2021-06-25 |
| CN113031598B true CN113031598B (en) | 2024-02-06 |
Family
ID=76465327
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110228719.0A Active CN113031598B (en) | 2021-03-02 | 2021-03-02 | Path tracking obstacle avoidance guidance method and system |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113031598B (en) |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| GB2439773A (en) * | 1995-11-06 | 2008-01-09 | Secr Defence | Weapon guidance by target state estimation |
| EP2423774A1 (en) * | 2010-08-23 | 2012-02-29 | MBDA UK Limited | Guidance method and apparatus |
| CN107168335A (en) * | 2017-06-28 | 2017-09-15 | 大连海事大学 | A kind of unmanned surface vehicle path trace method of guidance for considering mixing multiple target avoidance |
| CN111832121A (en) * | 2020-07-17 | 2020-10-27 | 中国人民解放军火箭军工程大学 | Multi-aircraft cooperative detection and guidance integrated method and system |
| CN112114521A (en) * | 2020-07-30 | 2020-12-22 | 南京航空航天大学 | Intelligent prediction control entry guidance method for spacecraft |
-
2021
- 2021-03-02 CN CN202110228719.0A patent/CN113031598B/en active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| GB2439773A (en) * | 1995-11-06 | 2008-01-09 | Secr Defence | Weapon guidance by target state estimation |
| EP2423774A1 (en) * | 2010-08-23 | 2012-02-29 | MBDA UK Limited | Guidance method and apparatus |
| CN107168335A (en) * | 2017-06-28 | 2017-09-15 | 大连海事大学 | A kind of unmanned surface vehicle path trace method of guidance for considering mixing multiple target avoidance |
| CN111832121A (en) * | 2020-07-17 | 2020-10-27 | 中国人民解放军火箭军工程大学 | Multi-aircraft cooperative detection and guidance integrated method and system |
| CN112114521A (en) * | 2020-07-30 | 2020-12-22 | 南京航空航天大学 | Intelligent prediction control entry guidance method for spacecraft |
Non-Patent Citations (1)
| Title |
|---|
| 水面无人艇自主导航与控制系统的设计与实现;陈卓;金建海;张波;包涛;;中国造船(第S1期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113031598A (en) | 2021-06-25 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107567412B (en) | Object position measurement using vehicle motion data with automotive camera | |
| CN109271944B (en) | Obstacle detection method, obstacle detection device, electronic apparatus, vehicle, and storage medium | |
| CN110879991B (en) | Obstacle identification method and system | |
| EP3667642A1 (en) | Collision avoidance assistance system | |
| CN112528773B (en) | Obstacle information fusion method and device, electronic equipment and storage medium | |
| CN106210484A (en) | Waters surveillance polynary associating sensing device and cognitive method thereof | |
| KR102265980B1 (en) | Device and method for monitoring ship and port | |
| CN115993597A (en) | Visual radar perception fusion method and terminal equipment | |
| CN112631314A (en) | Robot control method and system based on multi-line laser radar and event camera SLAM | |
| CN110751836A (en) | Vehicle driving early warning method and system | |
| CN113838125A (en) | Target position determining method and device, electronic equipment and storage medium | |
| CN115723919A (en) | Auxiliary navigation method and device for ship yaw | |
| CN111273701A (en) | Visual control system and control method for holder | |
| CN114325635A (en) | Target fusion method for laser radar and navigation radar | |
| CN113125795B (en) | Obstacle speed detection method, device, equipment and storage medium | |
| CN113031598B (en) | Path tracking obstacle avoidance guidance method and system | |
| CN107767366B (en) | A kind of transmission line of electricity approximating method and device | |
| CN114519671B (en) | Unmanned aerial vehicle remote sensing image dynamic rapid splicing method | |
| KR102558387B1 (en) | System and method for providing motion information and size information of ship based on real-time radar image | |
| CN115373383A (en) | Autonomous obstacle avoidance method and device for garbage recovery unmanned boat and related equipment | |
| CN114708544A (en) | Intelligent violation monitoring helmet based on edge calculation and monitoring method thereof | |
| CN114445761A (en) | Image recognition-based remote Raikang state monitoring method | |
| CN116859383B (en) | Determination method, device and equipment for navigation-obstacle | |
| CN113325826B (en) | Underground vehicle control method and device, electronic equipment and storage medium | |
| CN117788463B (en) | Ship draft detection method based on video AI and multi-mode data fusion |
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 |