CN112747760B - Autonomous navigation route planning method and device for unmanned platform on water surface of narrow water channel - Google Patents
Autonomous navigation route planning method and device for unmanned platform on water surface of narrow water channel Download PDFInfo
- Publication number
- CN112747760B CN112747760B CN202011493165.9A CN202011493165A CN112747760B CN 112747760 B CN112747760 B CN 112747760B CN 202011493165 A CN202011493165 A CN 202011493165A CN 112747760 B CN112747760 B CN 112747760B
- Authority
- CN
- China
- Prior art keywords
- grid
- unmanned platform
- obstacle
- route
- navigation
- 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
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/34—Route searching; Route guidance
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention provides a method and a device for planning autonomous navigation routes of an unmanned platform on a water surface of a narrow water channel, wherein the method comprises the steps of determining an operation area and planning an initial global route before the narrow water channel navigates; establishing a two-dimensional plane rectangular coordinate system of the unmanned platform, and mapping the initial global route to the two-dimensional plane rectangular coordinate system of the unmanned platform by the unmanned platform; converting the coordinates of the obstacles into a two-dimensional plane rectangular coordinate system of the unmanned platform; carrying out grid map modeling on the unmanned platform two-dimensional plane rectangular coordinate system, and constructing a local grid map of the water surface and underwater navigation environment with the unmanned platform as the center; and performing collision analysis and dynamically adjusting the air route in real time based on the local grid map of the water surface and underwater navigation environment. According to the scheme of the invention, the navigation control precision is high, the air route is dynamically updated according to the navigation environment information, the obstacles underwater on the water surface are effectively avoided, and the autonomous navigation capability of the unmanned platform on the water surface in the narrow water channel environment is improved.
Description
Technical Field
The invention relates to the technical field of water surface unmanned platform control, in particular to a method and a device for planning autonomous navigation routes of a narrow-channel water surface unmanned platform.
Background
Narrow channels are usually navigable areas of Narrow width and limited vessel maneuvering. In fact, the width of the navigable water area can only be considered as narrow, and it is difficult to give concrete quantification, and there is no uniform standard internationally for many years. It is a common practice in the world to consider waterways having a width of up to about 2 nautical miles as narrow waterways, but it is difficult to consider waterways having a width of up to 4 nautical miles as narrow waterways. With the development of large-scale and rapid ships, the displacement of ships is increasing, the traffic density of ships is increasing, and the concept of narrow water channels is changed to meet the requirement of safe ship navigation. In international rules of collision avoidance at sea, it is determined whether a waterway belongs to a narrow waterway, and whether the narrow waterway clause is applied, and the basis is generally considered to be the conventional sailing method and the opinion of a sailing specialist. Although "narrow channels" are often understood as rivers going through the sea or narrow straits, in practice, some channels opened up in ice, thunderlands, and reef areas and a part of the channels at the entrances and exits of rivers and harbors are also considered as "narrow channels" and the narrow channel term applies.
The unmanned water surface platform has the advantages of small target characteristic and good concealment, and can perform tasks such as environmental reconnaissance, information reconnaissance, water area rescue, warning patrol and the like in complex terrain areas such as near shore, shoals, island reefs, narrow water areas, harbors, inland rivers and the like and dangerous water areas. Although the unmanned water surface platform can execute various tasks, the unmanned water surface platform also has various problems, on one hand, the unmanned water surface platform is a control problem, the unmanned water surface platform has a long operation area and a complex operation environment, and the unmanned water surface platform is difficult to remotely control and control to navigate; on the other hand, the unmanned platform is mainly used for sensing the environment, the unmanned platform operation area is mainly located in narrow water channel areas such as near shore, shoal, island reef, narrow water area, harbor and inland river, the water surface and underwater environment can change in real time, and the navigation environment needs to be sensed in real time and decision-making processing in the navigation process; and finally, planning the air route, wherein the environment of the unmanned platform operation area is complex and dynamically changed, and an accurate or safe air route is difficult to preset.
Disclosure of Invention
The invention provides an autonomous navigation route planning method and device for an unmanned water surface platform in a narrow water channel, aiming at solving the technical problems that the unmanned water surface platform in the prior art is difficult to realize remote control, cannot dynamically sense the environment and combines complex environment and dynamic change to plan the route.
According to a first aspect of the invention, a method for planning autonomous navigation routes of an unmanned platform on a narrow water surface is provided, which comprises the following steps:
step S101: before the narrow water channel sails, determining an operation area on a sea-going electronic map, and planning an initial global air route;
step S102: the unmanned platform loads the operation area and the initial global air route, and establishes a two-dimensional plane rectangular coordinate system of the unmanned platform, wherein the coordinate system takes the center of the unmanned platform as an origin, takes the east direction as an x axis and takes the true north direction as a y axis; the unmanned platform maps the initial global route to a two-dimensional plane rectangular coordinate system of the unmanned platform;
step S103: detecting an obstacle in the navigation of the unmanned platform, and converting the coordinates of the obstacle into a two-dimensional plane rectangular coordinate system of the unmanned platform;
step S104: performing grid map modeling on the unmanned platform two-dimensional plane rectangular coordinate system, determining a corresponding grid value based on the barrier and the coordinate of the initial global air route, and constructing a local grid map of the water surface and underwater navigation environment with the unmanned platform as the center;
step S105: and performing collision analysis and dynamically adjusting the air route in real time based on the local grid map of the water surface and underwater navigation environment.
According to a second aspect of the present invention, there is provided a narrow water surface unmanned platform autonomous navigation route planning device, comprising:
an initial route acquisition module: before the narrow water channel sails, determining an operation area on a sea electronic map, and planning an initial global air route;
a first coordinate conversion module: configuring the unmanned platform to load the operation area and the initial global air route, and establishing a two-dimensional plane rectangular coordinate system of the unmanned platform, wherein the coordinate system takes the center of the unmanned platform as an origin, takes the east direction as an x axis, and takes the true north direction as a y axis; the unmanned platform maps the initial global route to a two-dimensional plane rectangular coordinate system of the unmanned platform;
a second coordinate conversion module: the unmanned platform is configured to detect an obstacle in navigation, and the coordinates of the obstacle are converted into a two-dimensional plane rectangular coordinate system of the unmanned platform;
the grid map modeling module: the method comprises the steps that a grid map modeling is carried out on a two-dimensional plane rectangular coordinate system of the unmanned platform, a corresponding grid value is determined based on a barrier and the coordinate of the initial global route, and a water surface and underwater navigation environment local grid map with the unmanned platform as the center is constructed;
the air route dynamic adjustment module: and performing collision analysis and dynamically adjusting the air route in real time based on the local grid map of the water surface underwater navigation environment.
According to a third aspect of the invention, there is provided a narrow passage water surface unmanned platform autonomous navigation route planning system, comprising:
a processor for executing a plurality of instructions;
a memory to store a plurality of instructions;
the instructions are stored in the memory, and loaded and executed by the processor to implement the autonomous navigation route planning method for the unmanned platform on water surface of narrow water channel.
According to a fourth aspect of the present invention, there is provided a computer readable storage medium having a plurality of instructions stored therein; the instructions are used for loading and executing the self-navigation route planning method of the unmanned platform on the water surface of the narrow water channel by the processor.
According to the scheme, the unmanned platform senses the surrounding navigation environment, intelligently decides to generate the operation route, and dynamically updates in real time according to the change of the environment. The method of the invention improves the autonomous navigation capability of the unmanned platform on the water surface in the narrow water channel environment. The method has the following technical effects: (1) The navigation control precision is high, accurate information such as barrier distance and direction is obtained in real time through the multiple sensors, a navigation environment local map under the coordinate system of the unmanned platform is constructed, the positioning precision of the unmanned platform is effectively improved, and the navigation control precision can be improved. (2) The autonomous navigation method has the advantages that the autonomous navigation method is high, the unmanned platform air route is dynamically updated according to navigation environment information on the basis of the preset global air route, obstacles underwater on the water surface are effectively avoided, and the autonomous navigation capability of the unmanned platform on the water surface in the narrow water channel environment is improved.
The foregoing description is only an overview of the technical solutions of the present invention, and in order to make the technical solutions of the present invention more clearly understood and to make the technical solutions of the present invention practical in accordance with the contents of the specification, the following detailed description is given of preferred embodiments of the present invention with reference to the accompanying drawings.
Drawings
The accompanying drawings, which are incorporated in and constitute a part of this specification, illustrate embodiments of the invention and, together with the description, serve to explain the principles of the invention. In the drawings:
fig. 1 is a flowchart of an autonomous navigation route planning method for an unmanned water surface platform according to an embodiment of the invention;
FIG. 2 is a schematic structural diagram of an autonomous navigation route planning system for a narrow channel water surface unmanned platform according to an embodiment of the invention;
fig. 3 is a block diagram of an autonomous navigation route planning device for a narrow channel water surface unmanned platform according to an embodiment of the invention.
Detailed Description
First, a flow of an autonomous navigation route planning method for an unmanned platform on a narrow water channel according to an embodiment of the present invention will be described with reference to fig. 1. As shown in fig. 1, the method comprises the steps of:
step S101: before the narrow water channel sails, determining an operation area on a sea-going electronic map, and planning an initial global air route;
step S102: the unmanned platform loads the operation area and the initial global air route, and establishes a two-dimensional plane rectangular coordinate system of the unmanned platform, wherein the coordinate system takes the center of the unmanned platform as an origin, takes the east direction as an x axis and takes the true north direction as a y axis; the unmanned platform maps the initial global route to a two-dimensional plane rectangular coordinate system of the unmanned platform;
step S103: detecting an obstacle in the navigation of the unmanned platform, and converting the coordinates of the obstacle into a two-dimensional plane rectangular coordinate system of the unmanned platform;
step S104: performing grid map modeling on the unmanned platform two-dimensional plane rectangular coordinate system, determining a corresponding grid value based on the barrier and the coordinate of the initial global air route, and constructing a local grid map of the water surface and underwater navigation environment with the unmanned platform as the center;
step S105: and performing collision analysis and dynamically adjusting the air route in real time based on the local grid map of the water surface and underwater navigation environment.
The step S101: determining a working area on a navigation electronic map before the narrow water channel sails, and planning an initial global course, wherein:
before the narrow channel sails, a working area is defined on the sea electronic chart, and an initial global course is planned. The route is formed by connecting discrete route points and comprises a navigation starting point, a route point and an end point.
The step S102: the unmanned platform loads the operation area and the initial global air route, and establishes a two-dimensional plane rectangular coordinate system of the unmanned platform, wherein the coordinate system takes the center of the unmanned platform as an origin, takes the east direction as an x axis and takes the true north direction as a y axis; the unmanned platform maps the initial global route to the unmanned platform two-dimensional plane rectangular coordinate system, and the method comprises the following steps:
the unmanned platform is loaded with a preset operation area and a route formed by discrete waypoints, and navigates in the operation area according to the preset route. And establishing a two-dimensional plane rectangular coordinate system of the unmanned platform by taking the center of the unmanned platform as an O point, the true north direction as a Y axis and the east direction as an X axis.
The unmanned platform acquires the geodetic coordinate position of the unmanned platform by the integrated navigation positioning system, combines the latitude and longitude of a route point in a pre-established route, and maps the route to the unmanned platform two-dimensional plane rectangular coordinate system through the conversion of the geodetic coordinate and the unmanned platform two-dimensional plane rectangular coordinate system.
The step S103: the unmanned platform detects an obstacle in navigation, converts the coordinates of the obstacle into a two-dimensional plane rectangular coordinate system of the unmanned platform, and comprises the following steps:
as shown in fig. 2, the unmanned platform obtains information of water surface targets such as ships, obstacles, coastlines and the like around the unmanned platform in real time by a navigation radar, a photoelectric turret, a millimeter wave radar and a navigation type laser radar, and accurately measures the distance, the direction and the size of a near-distance water surface target, so as to detect the water surface obstacle; the multi-beam depth sounder measures the water depth information of the unmanned platform in the underwater preset range in real time, and when the water depth of a navigation area is smaller than the navigation safe water depth of the unmanned platform, the underwater obstacle in the area is determined to exist, and the distance and the direction of the underwater obstacle are calculated. When the distance between a plurality of obstacles is less than 1.5 times the length of the unmanned platform, the obstacles are fused into one obstacle, so that the underwater obstacle is detected.
Converting the coordinates of the obstacle into a two-dimensional plane rectangular coordinate system of the unmanned platform, wherein the coordinate conversion formula is as follows:
in the formula, (x, y) is coordinate values in the coordinate system of the unmanned platform, and rho is the distance between the obstacle and the unmanned platform; theta is the orientation of the obstacle relative to the unmanned platform, and is unit rad;
the unit rad is the course of the unmanned platform; delta phi is the installation angle deviation of the detection equipment, and is unit rad; Δ x and Δ y are detection device installation distance deviations.
The step S104: performing grid map modeling on the unmanned platform two-dimensional plane rectangular coordinate system, determining a corresponding grid value based on a barrier and the coordinate of the initial global route, and constructing a local grid map of the water surface and underwater navigation environment with the unmanned platform as the center, wherein the grid map modeling comprises the following steps of:
carrying out grid map modeling on a two-dimensional plane rectangular coordinate system of the unmanned platform, wherein the grid map is a square area with the size of L multiplied by L, and discretizing the whole square area into grid units with the same size, wherein L is the length of the grid map, the distance between every grid unit is D, the unmanned platform is positioned at the central grid of the grid map, the first grid at the upper left of the grid map is the starting point of the grid map, and the grid map G can be expressed as:
G＝{g ij |g ij ＝{0,1,2,3,4,5,6,7}i∈N,j∈N}
wherein i represents the number of columns of the grid in the grid map, and j represents the number of rows of the grid in the grid map; g ij Reflecting the grid map assignment rule, g ij A value of 0 indicates that the grid is a free grid; gi is j A value of 1 indicates that the grid is an obstacle grid; gi (Gi) j A value of 2 indicates that the grid is a waypoint grid; g is a radical of formula ij A value of 3 indicates that the grid is a coincidence grid of the waypoint and the obstacle; g ij A value of 4 indicates that the grid is a course grid; g ij A value of 5 indicates that the grid is a course and obstacle coincidence grid; g ij A number 6 indicates that the grid is the starting grid of the course; g ij A number 7 indicates that the grid is the destination grid of the route, and N is the number of rows/columns of the grid map.
According to the coordinates in the unmanned platform two-dimensional plane rectangular coordinate system, a calculation formula for calculating the grid unit corresponding to the coordinates in the grid map is as follows:
wherein, (i, j) represents the number of columns and rows in the grid map where the grid is located; l is the length of the grid map, D is the spacing of the grid units, N is the number of grid rows (rows), and odd numbers are taken; and (x, y) is a coordinate value in the coordinate system of the unmanned platform.
And calculating to obtain grid units corresponding to the obstacles in a grid map according to the coordinates of the obstacles in the unmanned platform two-dimensional plane rectangular coordinate system, and marking the grid units corresponding to the obstacles according to the assignment rule of the grid map.
And rasterizing the initial global air route loaded in the unmanned platform, and marking the corresponding grid unit according to a grid map assignment rule. For example, the tag is a waypoint grid, a waypoint and obstacle coincidence grid, a course grid, or a course and obstacle coincidence grid. And constructing a local map of the water surface and underwater navigation environment by taking the unmanned platform as the center.
Step S105: based on the local grid map of the underwater navigation environment on the water surface, performing collision analysis and dynamically adjusting the air route in real time, comprising the following steps:
in the grid map, whether an airway and an obstacle coincident grid exist on the airway is searched along the navigation direction, if the airway and the obstacle coincident grid exist, an airway grid closest to the airway and the obstacle coincident grid is selected on the current airway as a new airway point grid, the original airway and the obstacle coincident grid are replaced, navigation is performed according to a new airway, whether the airway and the obstacle coincident grid exist on the airway is continuously searched in the grid map, and if the airway and the obstacle coincident grid exist, the airway needs to be re-planned to avoid the obstacle.
The re-planning of the route comprises:
searching a first intersection point grid and a last intersection point grid which are intersected by the route and the barrier along the navigation direction in a grid map, searching a grid which is 1.5 times the length of the unmanned platform away from the first intersection point grid along the reverse direction of the navigation direction, and taking the grid as a starting point grid; and searching a grid 1.5 times the length of the unmanned platform from the last intersection point grid along the navigation direction, using the grid as an end grid, and searching a plurality of free grids between the start point grid and the end point grid in the grid map by using a search algorithm.
The search algorithm is an A-algorithm improved type, the search directions are four directions of 0 degrees, 90 degrees, 180 degrees and 270 degrees, adjacent grids in the four directions are searched from a starting grid, the distance between the adjacent grids and the starting grid is calculated, and a loop operation is executed, namely, the grid with the minimum distance is selected as a next traversal node in each time until the end grid is reached; and then, starting from the ending grid, and finding the starting grid according to the shortest distance rule in turn, thereby searching a new flight path consisting of a plurality of free grids.
The formula of the search algorithm is:
wherein G (i, j) represents the distance from the grid (i, j) to the starting grid; g (i) p ,j p ) Represents the distance from the previous grid (i.e., parent grid) of grid (i, j) to the origin grid; g (i) s ,j s ) Represents the origin grid (i) s ,j s ) The distance value of (d) is 0;
represents when the grid (i) n ,j n ) When the grid is an obstacle grid, a route grid coincident with the obstacle or a route grid coincident with the obstacle, the grid arrives at a starting point grid (i) s ,j s ) Is infinite, i.e., no traffic can be passed.
And converting the row and column coordinates of the free grids forming the new route into coordinate points in a two-dimensional plane rectangular coordinate system of the unmanned platform. Calculating slopes between every two coordinate points along the navigation direction, and eliminating middle coordinate points with the same slopes; calculating the distance between every two rest coordinate points, eliminating the two coordinate points when the distance is less than 1.5 times of the length of the unmanned platform, and taking the middle point of the two coordinate points as a new coordinate point; and inserting the acquired coordinate points into a two-dimensional plane rectangular coordinate system of the unmanned platform, and forming a new operation route together with the route. Therefore, the dynamic route planning of autonomous navigation of the unmanned water platform is completed.
The method for planning the autonomous navigation route of the unmanned platform on the water surface in the narrow water channel is described in the following with reference to a specific embodiment.
Take an environmental detection unmanned boat with a length of 4m as an example.
A narrow water channel is found on a navigation electronic chart, an operation area with the length of about 3km and the width of no less than 30m is defined, a global course is established in the operation area and is formed by connecting 4 discrete waypoints, and the global course comprises 1 navigation starting point, two waypoints and 1 terminal point.
The unmanned ship is loaded with a preset operation area and a preset air route, and sails in the operation area according to the preset air route. The unmanned ship obtains information of water surface targets such as ships, obstacles, coastlines and the like around the unmanned ship in real time through a navigation radar, a photoelectric turret, a millimeter wave radar and a navigation type laser radar, and accurately measures the distance, the direction and the size of the close-distance water surface targets; the water depth information of the unmanned ship in a certain underwater range is measured in real time by the multi-beam bathymeter, when the water depth of a navigation area is less than 1m of the navigation safe water depth of the unmanned ship, the underwater obstacle in the area is determined, and the distance and the direction of the underwater obstacle are measured and calculated. For safety in navigation, the barrier edge is suitably extended 5m outwards. When the distance between obstacles is less than 6m, the obstacles are fused into one obstacle.
And establishing a two-dimensional plane rectangular coordinate system of the unmanned ship by taking the center of the unmanned ship as an O point, the true north direction as a Y axis and the east direction as an X axis, and converting the coordinates of the water surface and the underwater obstacles into the coordinate system. The coordinate conversion formula is as follows:
in the formula, (x, y) is coordinate values in the coordinate system of the unmanned ship, and rho is the distance between the obstacle and the unmanned ship; theta is the direction of the obstacle relative to the unmanned ship and is unit rad;
the unit rad is the course of the unmanned ship; delta phi is the installation angle deviation of the detection equipment, and the actual value is 0.03rad; the Δ x and Δ y mounting distance deviations were 20cm and 30cm, respectively.
The unmanned ship acquires the position of a geodetic coordinate by combining a Beidou positioning system and a Doppler log, and maps the route into a plane rectangular coordinate system by combining the preset route longitude and latitude.
Carrying out grid map modeling on an unmanned ship coordinate system, selecting a square area with the grid map size of 502m multiplied by 502m, discretizing the whole square area into grid units with the size of 2m multiplied by 2m, wherein the unmanned ship is positioned on a central grid of the grid map, and the first grid on the upper left is a grid starting point. The grid map may be represented as:
G＝{g ij |g ij ＝{0,1,2,3,4,5,6,7}i∈N,j∈N}
wherein i represents the number of columns where the grid is located in the grid map, and j represents the number of rows where the grid is located in the grid map; g is a radical of formula ij A value of 0 indicates that the grid is a free grid; g ij A value of 1 indicates that the grid is an obstacle grid; g, g ij A value of 2 indicates that the grid is a waypoint grid; g ij A value of 3 indicates that the grid is a coincidence grid of the waypoint and the obstacle; g ij A value of 4 indicates that the grid is a course grid; g ij A value of 5 indicates that the grid is a course and obstacle coincidence grid; g ij A number 6 indicates that the grid is the starting grid of the course; g ij And 7, the grid is the destination grid of the flight path.
And obtaining grid units of the corresponding ground of the obstacles in the grid map through calculation according to the coordinates of the obstacles on the water surface and underwater, and assigning the grids into obstacle grids according to the assignment rule of the grid map, namely the grid value is 1. The calculation formula is as follows:
where, (i, j) represents the number of columns and rows in the grid map where the grid is located; and (x, y) are coordinate values in the coordinate system of the unmanned ship.
And rasterizing the routes in the rectangular plane coordinate system according to a grid map assignment rule, and assigning corresponding grids, such as a waypoint grid or a waypoint and obstacle coincident grid or a route and obstacle coincident grid. So far, a water surface and underwater navigation environment local map taking an unmanned boat as a center is constructed.
In the grid map, whether an airway and an obstacle coincident grid exist on the airway is searched along the navigation direction, if the airway and the obstacle coincident grid exist, an airway grid closest to the airway and the obstacle coincident grid is selected on the current airway as a new airway point grid, the original airway and the obstacle coincident grid are replaced, navigation is performed according to a new airway, whether the airway and the obstacle coincident grid exist on the airway is continuously searched in the grid map, and if the airway and the obstacle coincident grid exist, the airway needs to be re-planned to avoid the obstacle.
Searching a first intersection point grid and a last intersection point grid which are intersected by the route and the barrier along the navigation direction in a grid map, searching a grid which is 6m away from the first intersection point grid along the reverse direction of the navigation direction, and taking the grid as a starting point grid S; and searching a grid 6m away from the last intersection grid in the navigation direction, taking the grid as an end grid E, and searching a series of free grids from the grid S to the grid E in the grid map, namely, the grid with the grid value of 0.
The search algorithm is an A-algorithm improved type, the search directions are four directions of 0 degrees, 90 degrees, 180 degrees and 270 degrees, adjacent grids in the four directions are searched from the starting grid, the distance between the grids and the starting grid is calculated, and the grid with the minimum distance is selected as a node of the next traversal until the ending grid is reached. And finally, starting from the end grid, and finding the starting point grid according to the shortest distance rule in turn, thereby searching a new flight path formed by the free grid.
The search algorithm formula is as follows:
wherein G (i, j) represents the distance from the grid (i, j) to the starting grid; g (i) p ,j p ) Representing the distance from the grid (i, j) previous to the grid (parent grid) to the origin grid; g (i) s ,j s ) Represents the grid of origin (i) s ,j s ) The distance value of (d) is 0;
represents when the grid (i) n ,j n ) When the grid is an obstacle grid, a route point and obstacle coincidence grid or a route and obstacle coincidence grid, it reaches the starting point grid (i) s ,j s ) Is infinite, here assigned a value of 100000m.
The row-column coordinates (i, j) of these free grids that make up the new flight are converted into coordinate points in the coordinate system of the unmanned boat. Calculating slopes between every two coordinate points along the navigation direction, and rejecting middle coordinate points with the same slopes; and (4) calculating the distance between every two rest coordinate points, eliminating the two coordinate points when the distance is less than 6m, and taking the middle point of the two coordinate points as a new coordinate point. And after the operation is finished, inserting the finally reserved coordinate points into the plane rectangular coordinate system, and forming a new operation route together with the route. Therefore, the route planning of the autonomous navigation of the environment detection type unmanned ship is completed.
An embodiment of the present invention further provides a device for planning autonomous navigation routes of an unmanned platform on a water surface in a narrow water channel, as shown in fig. 3, the device includes:
an initial route acquisition module: determining an operation area on a navigation electronic map before the narrow water channel sails, and planning an initial global air route;
a first coordinate conversion module: configuring the unmanned platform to load the operation area and the initial global air route, and establishing a two-dimensional plane rectangular coordinate system of the unmanned platform, wherein the coordinate system takes the center of the unmanned platform as an origin, takes the east direction as an x axis, and takes the true north direction as a y axis; the unmanned platform maps the initial global route to a two-dimensional plane rectangular coordinate system of the unmanned platform;
a second coordinate conversion module: the unmanned platform is configured to detect an obstacle in navigation, and the coordinates of the obstacle are converted into a two-dimensional plane rectangular coordinate system of the unmanned platform;
a grid map modeling module: the method comprises the steps that a grid map modeling is carried out on a two-dimensional plane rectangular coordinate system of the unmanned platform, a corresponding grid value is determined based on a barrier and the coordinate of the initial global route, and a water surface and underwater navigation environment local grid map with the unmanned platform as the center is constructed;
the air route dynamic adjustment module: and performing collision analysis and dynamically adjusting the air route in real time based on the local grid map of the water surface underwater navigation environment.
The embodiment of the invention further provides an autonomous navigation route planning system for the unmanned platform on the water surface of the narrow water channel, which comprises the following steps:
a processor for executing a plurality of instructions;
a memory to store a plurality of instructions;
the instructions are stored in the memory, and loaded and executed by the processor to implement the method for planning the autonomous navigation route of the unmanned platform on the water surface of the narrow channel.
The embodiment of the invention further provides a computer readable storage medium, wherein a plurality of instructions are stored in the storage medium; the instructions are used for loading and executing the self-navigation route planning method of the unmanned platform on the water surface of the narrow water channel by the processor.
It should be noted that the embodiments and features of the embodiments may be combined with each other without conflict.
In the embodiments provided in the present invention, it should be understood that the disclosed system, apparatus and method may be implemented in other ways. For example, the above-described apparatus embodiments are merely illustrative, and for example, the division of the units is only one logical division, and there may be other divisions in actual implementation, for example, a plurality of units or components may be combined or integrated into another system, or some features may be omitted, or not executed. In addition, the shown or discussed mutual coupling or direct coupling or communication connection may be an indirect coupling or communication connection through some interfaces, devices or units, and may be in an electrical, mechanical or other form.
The units described as separate parts may or may not be physically separate, and parts displayed as units may or may not be physical units, may be located in one position, or may be distributed on multiple network units. Some or all of the units can be selected according to actual needs to achieve the purpose of the solution of the embodiment.
In addition, functional units in the embodiments of the present invention may be integrated into one processing unit, or each unit may exist alone physically, or two or more units are integrated into one unit. The integrated unit may be implemented in the form of hardware, or in the form of hardware plus a software functional unit.
The integrated unit implemented in the form of a software functional unit may be stored in a computer readable storage medium. The software functional unit is stored in a storage medium, and includes several instructions to enable a computer device (which may be a personal computer, a physical machine Server, or a network cloud Server, etc., and needs to install a Windows or Windows Server operating system) to perform some steps of the method according to various embodiments of the present invention. And the aforementioned storage medium includes: various media capable of storing program codes, such as a usb disk, a removable hard disk, a Read-Only Memory (ROM), a Random Access Memory (RAM), a magnetic disk, or an optical disk.
The above description is only a preferred embodiment of the present invention, and is not intended to limit the present invention in any way, and any simple modification, equivalent change and modification made to the above embodiment according to the technical spirit of the present invention are still within the scope of the technical solution of the present invention.
Claims (8)
1. An autonomous navigation route planning method for an unmanned platform on a narrow water channel water surface is characterized by comprising the following steps:
step S101: before the narrow water channel sails, determining an operation area on a sea-going electronic map, and planning an initial global air route;
step S102: the unmanned platform loads the operation area and the initial global air route, and establishes a two-dimensional plane rectangular coordinate system of the unmanned platform, wherein the coordinate system takes the center of the unmanned platform as an origin, takes the east direction as an x axis and takes the true north direction as a y axis; the unmanned platform maps the initial global route to a two-dimensional plane rectangular coordinate system of the unmanned platform;
step S103: detecting an obstacle in the navigation of the unmanned platform, and converting the coordinate of the obstacle into a two-dimensional plane rectangular coordinate system of the unmanned platform;
step S104: performing grid map modeling on the unmanned platform two-dimensional plane rectangular coordinate system, determining a corresponding grid value based on the barrier and the coordinate of the initial global air route, and constructing a local grid map of the water surface and underwater navigation environment with the unmanned platform as the center;
step S105: performing collision analysis and dynamically adjusting a route in real time based on the local grid map of the water surface underwater navigation environment;
step S105: based on the local grid map of the underwater navigation environment on the water surface, performing collision analysis and dynamically adjusting the air route in real time, comprising the following steps:
in a grid map, searching whether an airway and an obstacle coincident grid exist on the airway along a navigation direction, if the airway and the obstacle coincident grid exist, selecting an airway grid closest to the airway and the obstacle coincident grid on the current airway as a new airway point grid, replacing the original airway and the obstacle coincident grid, navigating according to a new airway, and meanwhile, continuously searching whether the airway and the obstacle coincident grid exist on the airway, if the airway and the obstacle coincident grid exist, re-planning the airway to avoid the obstacle;
the re-planning a route includes:
searching a first intersection point grid and a last intersection point grid, which are intersected by a route and an obstacle, in a grid map along the navigation direction, searching a grid which is 1.5 times the length of the unmanned platform away from the first intersection point grid along the direction opposite to the navigation direction, and taking the grid as a starting point grid; searching a grid which is 1.5 times the length of the unmanned platform away from the last intersection point grid along the navigation direction, using the grid as an end grid, and searching a plurality of free grids between the start point grid and the end point grid in a grid map by using a search algorithm;
the search algorithm is an A-algorithm improved type, the search directions are four directions of 0 degrees, 90 degrees, 180 degrees and 270 degrees, adjacent grids in the four directions are searched from a starting grid, the distance between the adjacent grids and the starting grid is calculated, and a loop operation is executed, namely, the grid with the minimum distance is selected as a next traversal node in each time until the end grid is reached; starting from the ending grid, and finding a starting grid according to the shortest distance rule in turn, thereby searching a new flight path formed by a plurality of free grids;
the formula of the search algorithm is:
wherein G (i, j) represents the distance from the grid (i, j) to the starting grid; g (i) p ,j p ) Represents the distance of the previous grid of grid (i, j) to the starting grid; g (i) s ,j s ) Represents the origin grid (i) s ,j s ) The distance value of (d) is 0;
represents when the grid (i) n ,j n ) When the grid is an obstacle grid, a route point and obstacle coincidence grid or a route and obstacle coincidence grid, it reaches the starting point grid (i) s ,j s ) The distance of (2) is infinite, namely the traffic cannot be performed;
converting row and column coordinates of free grids forming a new route into coordinate points in a two-dimensional plane rectangular coordinate system of the unmanned platform;
calculating slopes between every two coordinate points along the navigation direction, and eliminating middle coordinate points with the same slopes; calculating the distance between every two rest coordinate points, eliminating the two coordinate points when the distance is less than 1.5 times of the length of the unmanned platform, and taking the middle point of the two coordinate points as a new coordinate point; and inserting the acquired coordinate points into a two-dimensional plane rectangular coordinate system of the unmanned platform to form a new operation route.
2. The method of claim 1, wherein the unmanned platform mapping the initial global route into the unmanned platform two-dimensional planar rectangular coordinate system comprises:
the unmanned platform acquires the geodetic coordinate position of the unmanned platform by the integrated navigation positioning system, combines the latitude and longitude of a route point in a pre-established route, and maps the route to the unmanned platform two-dimensional plane rectangular coordinate system through the conversion of the geodetic coordinate and the unmanned platform two-dimensional plane rectangular coordinate system.
3. The method for autonomous navigation routing of an unmanned water surface platform according to claim 2, wherein detecting an obstacle while the unmanned platform is underway comprises:
the unmanned platform acquires water surface target information of ships, obstacles and coastlines around the unmanned platform in real time through a navigation radar, a photoelectric turret, a millimeter wave radar and a navigation laser radar, accurately measures water surface targets, and acquires the distance, the direction and the size of the water surface targets and the unmanned platform so as to detect the water surface obstacles; the method comprises the steps that water depth information in an underwater preset range of an unmanned platform is measured in real time by a multi-beam depth sounder, when the water depth of a navigation area is smaller than the navigation safe water depth of the unmanned platform, an underwater obstacle in the area is determined, and the distance and the direction of the underwater obstacle are measured; when the distance between the obstacles is less than 1.5 times the length of the unmanned platform, the obstacles are fused into one obstacle, and thus the underwater obstacle is detected.
4. The method for autonomous navigation routing of an unmanned surface platform in a narrow channel of claim 3, wherein said transforming the coordinates of said obstacle to a rectangular coordinate system of a two-dimensional plane of said unmanned platform comprises:
the coordinate transformation formula is as follows:
in the formula, (x, y) is coordinate values in the coordinate system of the unmanned platform, and rho is the distance between the obstacle and the unmanned platform; theta is the orientation of the obstacle relative to the unmanned platform, and is the unit rad;
the unit rad is the course of the unmanned platform; delta phi is the installation angle deviation of the detection equipment, and is unit rad; Δ x and Δ y are detection device installation distance deviations.
5. The method for planning autonomous navigation routes for unmanned underwater vehicles with narrow water surfaces as claimed in claim 4, wherein said step S104: performing grid map modeling on the two-dimensional plane rectangular coordinate system of the unmanned platform, determining a corresponding grid value based on the coordinates of the barrier and the initial global route, and constructing a local grid map of the water surface and underwater navigation environment with the unmanned platform as the center, wherein the grid map comprises the following steps:
carrying out grid map modeling on a two-dimensional plane rectangular coordinate system of the unmanned platform, wherein the grid map is a square area with the size of L multiplied by L, and discretizing the whole square area into grid units with the same size, wherein L is the length of the grid map, the distance between every two grid units is D, meanwhile, the unmanned platform is positioned at the central grid of the grid map, the first grid at the upper left of the grid map is the starting point of the grid map, and the grid map G can be expressed as:
G＝{g ij |g ij ＝{0,1,2,3,4,5,6,7} i∈N,j∈N}
wherein i represents the number of columns of the grid in the grid map, and j represents the number of rows of the grid in the grid map; g ij Reflecting the grid map assignment rule, g ij A value of 0 indicates that the grid is a free grid; g ij A value of 1 indicates that the grid is an obstacle grid; g ij A value of 2 indicates that the grid is a waypoint grid; g ij The grid is denoted by 3Coinciding grids for waypoints and obstacles; g ij A value of 4 indicates that the grid is a course grid; g ij A value of 5 indicates that the grid is a course and obstacle coincident grid; g ij A starting grid of 6 indicates that the grid is a course; g ij 7 represents that the grid is an end point grid of the air route, and N is the row number or the column number of the grid map;
according to the coordinates in the unmanned platform two-dimensional plane rectangular coordinate system, a calculation formula for calculating the grid unit corresponding to the coordinates in the grid map is as follows:
wherein, (i, j) represents the number of columns and rows in the grid map where the grid is located; l is the length of the grid map, D is the spacing of the grid units, N is the number of grid columns or rows, and odd numbers are taken; (x, y) are coordinate values in the coordinate system of the unmanned platform;
calculating to obtain grid units corresponding to the obstacles in a grid map according to the coordinates of the obstacles in the unmanned platform two-dimensional plane rectangular coordinate system, and marking the grid units corresponding to the obstacles according to the assignment rule of the grid map;
rasterizing an initial global route loaded in the unmanned platform, and marking a corresponding grid unit according to a grid map assignment rule; and constructing and finishing a local map of the water surface and underwater navigation environment by taking the unmanned platform as a center.
6. An autonomous navigation route planning device for an unmanned platform on a water surface in a narrow water course, the device comprising:
an initial route obtaining module: before the narrow water channel sails, determining an operation area on a sea electronic map, and planning an initial global air route;
a first coordinate conversion module: configuring the unmanned platform to load the operation area and the initial global air route, and establishing a two-dimensional plane rectangular coordinate system of the unmanned platform, wherein the coordinate system takes the center of the unmanned platform as an origin, takes the east direction as an x axis, and takes the true north direction as a y axis; the unmanned platform maps the initial global route to a two-dimensional plane rectangular coordinate system of the unmanned platform;
a second coordinate conversion module: the unmanned platform is configured to detect an obstacle in navigation, and the coordinates of the obstacle are converted into a two-dimensional plane rectangular coordinate system of the unmanned platform;
the grid map modeling module: the unmanned platform is configured to be subjected to grid map modeling on a two-dimensional plane rectangular coordinate system, corresponding grid values are determined based on the barriers and the coordinates of the initial global air route, and a water surface and underwater navigation environment local grid map taking the unmanned platform as the center is constructed;
the air route dynamic adjustment module: performing collision analysis and dynamically adjusting a course in real time based on the local grid map of the water surface underwater navigation environment;
the collision analysis and real-time dynamic adjustment of the air route based on the local grid map of the water surface underwater navigation environment comprises the following steps:
in a grid map, searching whether an airway and an obstacle coincident grid exist on the airway along a navigation direction, if the airway and the obstacle coincident grid exist, selecting an airway grid closest to the airway and the obstacle coincident grid on the current airway as a new airway point grid, replacing the original airway and the obstacle coincident grid, navigating according to a new airway, and meanwhile, continuously searching whether the airway and the obstacle coincident grid exist on the airway, if the airway and the obstacle coincident grid exist, re-planning the airway to avoid the obstacle;
the re-planning a route includes:
searching a first intersection point grid and a last intersection point grid, which are intersected by a route and an obstacle, in a grid map along the navigation direction, searching a grid which is 1.5 times the length of the unmanned platform away from the first intersection point grid along the direction opposite to the navigation direction, and taking the grid as a starting point grid; searching a grid which is 1.5 times the length of the unmanned platform away from the last intersection point grid along the navigation direction, using the grid as an end grid, and searching a plurality of free grids between the start point grid and the end point grid in a grid map by using a search algorithm;
the search algorithm is an A-algorithm improved type, the search directions are four directions of 0 degrees, 90 degrees, 180 degrees and 270 degrees, adjacent grids in the four directions are searched from a starting grid, the distance between the adjacent grids and the starting grid is calculated, and a loop operation is executed, namely, the grid with the minimum distance is selected as a next traversal node in each time until the end grid is reached; starting from the ending grid, and finding a starting grid according to the shortest distance rule in turn, thereby searching a new flight path formed by a plurality of free grids;
the formula of the search algorithm is:
wherein G (i, j) represents the distance from the grid (i, j) to the starting grid; g (i) p ,j p ) Represents the distance of the previous grid of grid (i, j) to the starting grid; g (i) s ,j s ) Represents the grid of origin (i) s ,j s ) The distance value of (d) is 0;
represents when the grid (i) n ,j n ) When the grid is an obstacle grid, a route point and obstacle coincidence grid or a route and obstacle coincidence grid, it reaches the starting point grid (i) s ,j s ) The distance of (a) is infinite, namely, the traffic cannot be passed;
converting the row and column coordinates of the free grids forming the new air route into coordinate points in a two-dimensional plane rectangular coordinate system of the unmanned platform;
calculating slopes between every two coordinate points along the navigation direction, and rejecting middle coordinate points with the same slopes; calculating the distance between every two rest coordinate points, eliminating the two coordinate points when the distance is less than 1.5 times of the length of the unmanned platform, and taking the middle point of the two coordinate points as a new coordinate point; and inserting the acquired coordinate points into a two-dimensional plane rectangular coordinate system of the unmanned platform to form a new operation route.
7. The utility model provides a narrow water course surface of water unmanned platform is navigation route planning system independently which characterized in that includes:
a processor for executing a plurality of instructions;
a memory to store a plurality of instructions;
wherein the plurality of instructions for being stored by the memory and loaded and executed by the processor to perform the method for autonomous navigation routing by the unmanned platform on a water surface of a narrow water course according to any of claims 1 to 5.
8. A computer-readable storage medium having a plurality of instructions stored therein; the plurality of instructions for loading and executing by a processor the method for autonomous navigation routeing for the unmanned platform on a water surface of a narrow channel according to any of claims 1 to 5.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011493165.9A CN112747760B (en) | 2020-12-16 | 2020-12-16 | Autonomous navigation route planning method and device for unmanned platform on water surface of narrow water channel |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011493165.9A CN112747760B (en) | 2020-12-16 | 2020-12-16 | Autonomous navigation route planning method and device for unmanned platform on water surface of narrow water channel |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112747760A CN112747760A (en) | 2021-05-04 |
| CN112747760B true CN112747760B (en) | 2022-10-28 |
Family
ID=75648058
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202011493165.9A Active CN112747760B (en) | 2020-12-16 | 2020-12-16 | Autonomous navigation route planning method and device for unmanned platform on water surface of narrow water channel |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112747760B (en) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114088100B (en) * | 2022-01-11 | 2022-05-06 | 中科星图智慧科技有限公司 | Path planning method and device based on complex conditions |
| CN115061470B (en) * | 2022-06-30 | 2023-03-21 | 哈尔滨理工大学 | Unmanned vehicle improved TEB navigation method suitable for narrow space |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2017173990A1 (en) * | 2016-04-07 | 2017-10-12 | 北京进化者机器人科技有限公司 | Method for planning shortest path in robot obstacle avoidance |
| CN110488871A (en) * | 2019-09-03 | 2019-11-22 | 滁州学院 | A kind of unmanned plane path dynamic programming method based on two three-dimensional integratedization |
| CN110530390A (en) * | 2019-09-16 | 2019-12-03 | 哈尔滨工程大学 | A kind of non-particle vehicle path planning method under narrow environment |
| CN110849370A (en) * | 2019-11-14 | 2020-02-28 | 中国船舶重工集团公司第七0七研究所 | Dynamic route planning method based on unmanned surface vehicle |
| CN111536962A (en) * | 2020-05-19 | 2020-08-14 | 智慧航海（青岛）科技有限公司 | Intelligent ship route planning method and device, storage medium and computer equipment |
| CN111721296A (en) * | 2020-06-04 | 2020-09-29 | 中国海洋大学 | Data driving path planning method for underwater unmanned vehicle |
-
2020
- 2020-12-16 CN CN202011493165.9A patent/CN112747760B/en active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2017173990A1 (en) * | 2016-04-07 | 2017-10-12 | 北京进化者机器人科技有限公司 | Method for planning shortest path in robot obstacle avoidance |
| CN110488871A (en) * | 2019-09-03 | 2019-11-22 | 滁州学院 | A kind of unmanned plane path dynamic programming method based on two three-dimensional integratedization |
| CN110530390A (en) * | 2019-09-16 | 2019-12-03 | 哈尔滨工程大学 | A kind of non-particle vehicle path planning method under narrow environment |
| CN110849370A (en) * | 2019-11-14 | 2020-02-28 | 中国船舶重工集团公司第七0七研究所 | Dynamic route planning method based on unmanned surface vehicle |
| CN111536962A (en) * | 2020-05-19 | 2020-08-14 | 智慧航海（青岛）科技有限公司 | Intelligent ship route planning method and device, storage medium and computer equipment |
| CN111721296A (en) * | 2020-06-04 | 2020-09-29 | 中国海洋大学 | Data driving path planning method for underwater unmanned vehicle |
Non-Patent Citations (2)
| Title |
|---|
| UUV自主航行路径规划方法;温志文 等;《制造业自动化》;20161130;第38卷(第11期);第1-5页 * |
| 水下无人潜航器避障与导航仿真系统设计与应用;郑为 等;《系统仿真学报》;20160131;第28卷(第1期);第91-98页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112747760A (en) | 2021-05-04 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Song et al. | Smoothed A* algorithm for practical unmanned surface vehicle path planning | |
| KR101799216B1 (en) | Ship navigation apparatus and method for providing route information for ship | |
| KR101894674B1 (en) | Ship navigation apparatus and method for providing route information for ship | |
| CN112747760B (en) | Autonomous navigation route planning method and device for unmanned platform on water surface of narrow water channel | |
| CN108681321A (en) | A kind of undersea detection method that unmanned boat collaboration is formed into columns | |
| KR101719142B1 (en) | Ship navigation apparatus and method for providing route information for ship | |
| EP3885705B1 (en) | Systems and associated methods for generating navigation charts and navigable routes in an open environment | |
| RU2483280C1 (en) | Navigation system | |
| CN111309010B (en) | Control method implemented by unmanned ship seabed terrain surveying and mapping control system based on bionic robot | |
| CN111307160B (en) | Correction method and device for long-endurance flight path of autonomous underwater vehicle | |
| CN111090283B (en) | Unmanned ship combined positioning and orientation method and system | |
| CN113010958A (en) | Simulation system of self-propelled ship and operation method thereof | |
| CN112880678A (en) | Unmanned ship navigation planning method in complex water area environment | |
| Marchel et al. | Optimisation of the Position of Navigational Aids for the Purposes of SLAM technology for Accuracy of Vessel Positioning | |
| Reed et al. | Providing nautical chart awareness to autonomous surface vessel operations | |
| CN111412918B (en) | Unmanned ship global safety path planning method | |
| Lee et al. | Generation of Ship’s passage plan using data-driven shortest path algorithms | |
| CN108241368B (en) | Unmanned ship route navigation method and equipment | |
| Huang et al. | A rapid globe-wide shortest route planning algorithm based on two-layer oceanic shortcut network considering great circle distance | |
| CN114047514A (en) | Unmanned ship navigation method based on millimeter wave radar | |
| Naus et al. | The idea of using the A* algorithm for route planning an unmanned vehicle “Edredon” | |
| TWI838007B (en) | A watercrafts environment risk real-time control and navigation system and its operation method | |
| Lee et al. | Passage Planning in Coastal Waters for Maritime Autonomous Surface Ships using the D* Algorithm | |
| Blaich et al. | Trajectory generation for vessels using a reachability graph | |
| Kohlbrenner et al. | Toward a Marine Road Network for Ship Passage Planning and Monitoring |
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 |