JP6844205B2 - Mobile control method and mobile control system - Google Patents

Description

The present invention relates to a mobile control method and a mobile control system used to move a plurality of mobiles in a formation.

Autonomous underwater vehicles (AUVs) or UUVs (Unmanned Underwater Vehicles) are used for underwater resource exploration, mines exploration and removal, and marine victims exploration (exploration). It is considered to do.

When conducting the above-mentioned exploration, it is desirable to carry out a wide range of exploration in a short time in order to improve the efficiency of the exploration.

Therefore, as a method for carrying out efficient exploration, it is considered to make a plurality of underwater vehicles navigate in formation.

As one of the methods for moving a plurality of moving bodies in a formation, there is a method in which a main engine and a follower are defined among a plurality of moving bodies and the follower is moved by following the movement of the main engine (for example). , Patent Document 1).

Japanese Unexamined Patent Publication No. 2004-25971

However, in the method shown in Patent Document 1, among the moving bodies forming a formation, those set as followers need to communicate with the main engine. Therefore, the follower needs to be located in an area where it can communicate with the main engine.

Therefore, in the method shown in Patent Document 1, the size of the formation composed of the main engine and the follower is limited depending on the distance that can be communicated with the main engine.

In water, communication is mainly performed using ultrasonic waves, but in general, the distance that can be communicated underwater is shorter than that of wireless communication. Therefore, when the method shown in Patent Document 1 is applied to a formation of a moving body that moves underwater such as an underwater navigator, the actual situation is that the size of the formation that can be configured becomes more limited.

Further, in the method shown in Patent Document 1, since the follower requires communication with the main engine, if the number of followers forming the formation increases, the communication load of the main engine becomes large.

Therefore, in the method shown in Patent Document 1, the actual situation is that the number of followers is limited depending on the communication load of the main engine.

In particular, underwater communication limits the amount of information that can be transmitted and received compared to wireless communication. Therefore, when the method shown in Patent Document 1 is applied to a formation of moving bodies moving underwater such as an underwater navigator, the limit on the number of moving bodies constituting the formation becomes large.

Therefore, the present invention has been made to provide a mobile body control method and a mobile body control system capable of moving a plurality of mobile bodies in a formation state without determining a main engine or a follower for the plurality of mobile bodies. To do.

In order to solve the above-mentioned problems, in order to solve the above-mentioned problems, for a plurality of moving bodies moving in a formation, each moving body is set as an apex in graph theory, and positioning of other moving bodies by a positioning means provided for each moving body is used. A process of setting a possible state as an arc in graph theory and setting a moving body of an essential positioning target, which is another moving body that must be positioned, to a state in which a whole area tree or a strongly connected graph structure is formed. A process of storing the movement plan of the own machine and the movement plan of another moving body including the essential positioning target positioned by the positioning means is performed in the storage unit provided for each moving body. When moving in a formation of each mobile body, a process of positioning other mobile bodies including the essential positioning target by the positioning means of each mobile body and positioning of the positioning means by each mobile body are performed. From the result, at the positioning time point, the positioning is based on the process of obtaining the measured value of the relative position between the own machine and the other moving body including the essential positioning target, and the movement plan stored in the storage unit. Regarding the process of obtaining the target value of the relative position to be taken by the own machine and other moving objects including the essential positioning target at the time of formation correction set after the time point, and the measured value of the relative position, the relative position This is a mobile body control method that performs a process of obtaining a deviation from the target value of the above and a process of giving a control input to the actuator of the own machine so that the deviation becomes zero at the time of the formation correction.

There is a method of forming a formation formed by the plurality of mobile bodies into a series of formations.

In addition, a moving body that moves in a formation is provided with a position detecting means for detecting the position of its own machine, a positioning means for positioning other moving bodies, an actuator, a control device, and a storage unit. In the control device, each mobile body forming the formation is set as the apex in the graph theory, and the positionable state of the other moving body by the positioning means of the moving body is set as an arc in the graph theory, and is a spanning tree or strongly connected. An essential positioning target mobile body, which is another moving body that must be positioned so as to form a graph structure, is set, and the storage unit is positioned by the movement plan of the own machine and the positioning means. The control device has a function of storing a movement plan of another mobile body including the essential positioning target, and further, the control device obtains the own device and another mobile body including the essential positioning target based on the positioning result of the positioning means. Based on the process of obtaining the measured value of the relative position of the above and each movement plan stored in the storage unit, the own machine and the essential positioning target are included at the formation correction time point set after the positioning time point. The process of finding the target value of the relative position to be taken by another moving body, the process of finding the deviation of the measured value of the relative position from the target value of the relative position, and the deviation being zero at the time of the formation correction. Therefore, the mobile control system has a configuration having a function of giving a control input to the actuator.

According to the moving body control method and the moving body control system of the present invention, it is possible to move a plurality of moving bodies in a formation state without determining a main engine or a follower for the plurality of moving bodies.

It is a schematic diagram which shows the 1st Embodiment of a mobile body control system. It is a figure which shows the example of the shape of the formation formed by the underwater vehicle. It is a figure for demonstrating the graph theory. It is a figure which shows the state which the deviation occurs in the target value of a relative position, and the present position of a relative position.

Hereinafter, embodiments for carrying out the present invention will be described with reference to the drawings.

[First Embodiment]
1 to 4 show an example in which a plurality of moving bodies to be controlled are underwater navigating bodies as a first embodiment of a moving body control method and a moving body control system.

FIG. 1 is a schematic view showing an underwater vehicle to be controlled in the present embodiment. FIG. 2 shows an example of the shape of a formation formed by a plurality of moving bodies. FIG. 2A shows an I-shaped formation in a horizontal row, FIG. 2B shows a V-shaped formation, and FIG. c) is a schematic plan view showing each O-shaped formation. FIG. 3 is a diagram for explaining an outline of graph theory. FIG. 4 is a diagram showing a state in which a deviation occurs between the target value of the relative position between the underwater vehicles and the current position of the relative position.

The mobile control system of the present embodiment is composed of a plurality of underwater vehicles X as shown in FIG.

The underwater vehicle X is configured to include a position detecting means 1, a positioning means 2, an actuator 3, and a control device 4.

The position detecting means 1 has a function of detecting the position of its own machine of the underwater vehicle X. The position detecting means 1 includes, for example, a receiving device of a global navigation satellite system (GNSS) that detects the position of its own aircraft when it is floating on the water surface, for example, as a position in geographic coordinates, a radio positioning device, and the like. In addition, it may be composed of an inertial navigation system that detects its own position when submerged in water, a Doppler velocimeter, and the like.

The positioning means 2 has a function of detecting the relative position of another underwater vehicle X with respect to its own aircraft. The positioning means 2 may be composed of, for example, an acoustic positioning device, a positioning device that uses radio waves in the case of a short range, an optical positioning device based on a captured image of a camera, or the like.

When the positioning means 2 includes an acoustic communication device (not shown) between the own unit and the other underwater vehicle X and is arranged in an environment capable of acoustic communication, the other underwater vehicle X detects the position. Information on the position detected as the position of the own machine by the means 1 is acquired by acoustic communication, and based on the difference between the information and the position of the own machine detected by the position detecting means 1, another underwater vehicle X It may further have a function of detecting the relative position of the machine with respect to its own machine.

The actuator 3 has a function of controlling the traveling direction and speed of the underwater vehicle X. The actuator 3 is composed of, for example, various thrusters and rudders.

The control device 4 has a function of giving a command (control input) to the actuator 3 by performing processing according to a control rule described later based on signals input from the position detecting means 1 and the positioning means 2 to the control device 4. ..

Further, the control device 4 includes a storage unit 5 that stores a waypoint file as a movement plan. Here, the waypoint file is a file for setting a course in which the underwater vehicle X moves as a moving body. The waypoint file is coordinate information of a plurality of waypoints numbered according to the order to be followed, and the underwater vehicle X passes through each waypoint set in the waypoint file for its own aircraft in the order of numbers. By doing so, you can move according to the set course. Further, in the waypoint file, the passage target time when the underwater vehicle X passes each waypoint is also set for each waypoint. As a result, the underwater vehicle X can move on the set course at the set time by passing each waypoint at the set passage target time.

Next, the function of the control device 4 will be described, and the moving body control method will also be described.

In the present embodiment, as shown in FIG. 2A, when a plurality of underwater vehicles X, for example, seven underwater vehicles X move in a horizontal row of I-shaped formations. An example will be described.

First, as a preparation, an individual identification code is set for each underwater vehicle X. Here, for convenience of explanation, each underwater vehicle X is numbered consecutively by a natural number from 1 to 7 as an identification code. In FIG. 2A, the numbers 1 to 7 of the respective identification codes are attached to the reference numerals X in order from the underwater vehicle X located at one end (left end in the figure) of the formation.

The control device 4 (see FIG. 1) of each of the underwater vehicles X1 to X7 is set with other underwater vehicles X1 to X7 that need to be reliably positioned by the positioning means 2 (see FIG. 1) of the own machine. Will be done. Hereinafter, the other underwater vehicles X1 to X7 that need to be reliably positioned by the positioning means 2 are referred to as underwater vehicles X1 to X7 that are essential positioning targets. At this time, the i-th underwater vehicle Xi targets the (i-1) -th underwater vehicle X (i-1) as an essential positioning target (i = 2, 3, ..., 7). This is to ensure at least the conditions for forming the graph structure of the spanning tree in the graph theory described later. The relationship between the underwater vehicles X1 to X7 and the essential positioning targets is shown by arrows in FIG. 2 (a). The tip side of the arrow is the essential positioning target.

In the case of a series of formations (a shape capable of writing with a single stroke) such as the I-shape shown in FIG. 2A, the control device 4 of the underwater vehicle X1 located at one end of the formation may be used. The essential positioning target may or may not be set to the second underwater vehicle X2.

Further, the control devices 4 of the underwater vehicles X1 to X7 have a waypoint file for their own machine and a waypoint file for setting the course of the underwater vehicles X1 to X7, which are the essential positioning targets, in the storage unit 5. And remember.

Further, when it is assumed that the underwater vehicles X1 to X7 other than the essential positioning targets are located within the range where the positioning means 2 of each underwater vehicle X1 to X7 can be positioned, the storage unit 5 Also stores waypoint files for course setting of underwater vehicles X1 to X7 that can be positioned other than the essential positioning targets.

In the present embodiment, the underwater vehicles X1 to X7 are moved in a state of forming a horizontal row of formations as shown in FIG. 2 (a). Therefore, in the waypoint files of the underwater vehicles X1 to X7, the movement courses of the underwater vehicles X1 to X7 are set in a substantially parallel arrangement with a set interval. Further, the waypoints on the moving course of the underwater vehicles X1 to X7 are set to pass target times so that the underwater vehicles X1 to X7 travel side by side.

The storage of these waypoint files in the storage unit 5 is adjusted before each underwater vehicle X1 to X7 is navigated, such as on land or on a mother ship.

Next, the graph theory will be described.

The graph is composed of a plurality of vertices v1, v2, v3 and arcs a1, a2, a3, for example, as shown in FIG.

In this embodiment, the vertices v1, v2, v3 correspond to individual underwater vehicles X1 to X7. For convenience of explanation, the number of vertices is set to 3 in FIG.

Further, in the present embodiment, the arc a1 indicated by the bidirectional arrows can be positioned between the underwater vehicles X1 to X7 at the apex v1 and the underwater vehicles X1 to X7 at the apex v2. It shows that there is.

On the other hand, in the arc a2 indicated by the one-way arrow, the underwater vehicles X1 to X7 at the apex v2 on the base end side of the arrow are positioned from the underwater vehicles X1 to X7 at the apex v3 on the tip side of the arrow. It shows that it is possible. Similarly, in the arc a3 indicated by the one-way arrow, the underwater vehicle X1 to X7 at the apex v3 on the base end side of the arrow can be positioned from the underwater vehicle X1 to X7 at the apex v1 on the tip side of the arrow. It shows that there is.

Therefore, in the present embodiment, the adjacency in the graph indicates that the positioning from the apex vi to the apex vj is possible.

Also, the connectivity in the graph indicates that there is a way from vertex vi to vertex vj.

The graph structure can be represented by a matrix, and the adjacency matrix A is indicated by 1 if it is adjacent and 0 otherwise. Therefore, the adjacency matrix A in the case of the graph structure of FIG. 3 is as follows.

The degree matrix D represents an adjacent number, and in the case of the graph structure of FIG. 3, it is as follows.

Therefore, the graph Laplacian L is as follows.

In graph theory, looking at the Graph Laplacian L shows whether a system with that graph structure can be agreed upon. Here, the consensus is that the values of all vertices match a certain constant α. In the case of a distributed system that is not a so-called leader-follower type, it is known that the agreement condition is that the graph structure is a spanning tree or a strong connection.

With the above preparations in place, the underwater navigation bodies X1 to X7 form a formation shown in FIG. 2A and start sailing.

At this time, in each of the underwater vehicles X1 to X7, the waypoint file for the own machine is stored in the storage unit 5. Therefore, the control device 4 of each of the underwater vehicles X1 to X7 reads out the coordinates of the waypoint to be moved at the present time from the waypoint file for the own machine stored in the storage unit 5.

Further, the control device 4 obtains information on the current position of its own machine based on the signal from the position detecting means 1.

Therefore, when the control device 4 simply performs waypoint control, the actuator 3 may be provided with a control input for moving from the current position of the own machine to the waypoint that is the current movement target. The control input in this case is the following equation (1).
(Control input) = (Input obtained from the waypoint file of the own machine) ... (1)

For the right side of the above equation (1), a calculation method generally used in the past may be adopted when controlling a moving body such as an underwater vehicle by waypoints. This may be, for example, a calculation method in which the deviation from the current movement target of the current position of the own machine is obtained and the control input of the actuator 3 is calculated so that the obtained deviation becomes zero. This kind of calculation method is shown in, for example, Japanese Patent Application Laid-Open No. 2007-160972, Japanese Patent Application Laid-Open No. 2013-139185, and the like.

By the way, when actually operating each underwater vehicle X1 to X7, for example, one of the underwater vehicles X1 to X7 is set in a waypoint file for obstacle avoidance or the like. If you sail off the moving course you are in, you may not be able to maintain the shape of your formation.

Further, when the inertial navigation system is used as the position detecting means 1, if the navigation is continued for a long time, an error occurs in the position of the own aircraft detected by the inertial navigation system. Therefore, even in this case, the shape of the formation may not be maintained.

Therefore, in the present embodiment, each control device 4 of each underwater vehicle X1 to X7 is based on the detection result of the relative position of the other underwater vehicle X1 to X7 including the essential positioning target positioned by the positioning means 2. In addition, at the positioning time point, the measured value of the relative position between the own aircraft and other underwater vehicles X1 to X7 including the essential positioning target is obtained.

Further, each control device 4 is based on the waypoint file stored in the storage unit 5, and at the formation correction time set after the positioning time, the other control device 4 includes its own aircraft and other underwater navigation including the essential positioning target. Find the target value of the relative position that the bodies X1 to X7 should take.

Further, each control device 4 obtains a deviation from the target value of the relative position with respect to the measured value of the obtained relative position, and controls and inputs to the actuator 3 of the own machine so that the deviation becomes zero at the time of the formation correction. It is designed to perform the process of giving.

At the time of the correction correction, if the passage target times of the waypoints currently set as the movement targets of the underwater vehicles X1 to X7 are the same time, the passage target time of the waypoints is set at the formation correction time. You can do it. In this case, in each control device 4, the target value of the relative position to be taken by the own machine and the other underwater vehicles X1 to X7 including the essential positioning target is the own machine and the essential positioning stored in the storage unit 5. It may be obtained as the difference between the coordinates of the waypoints of other underwater vehicles X1 to X7 including the target.

If the passage target times of the waypoints currently set as the current movement targets of the underwater vehicles X1 to X7 do not match, the passage target times of the waypoints of any of the underwater vehicles X1 to X7 are set. It may be set at the time of formation correction. Further, for example, when the current positions of the underwater vehicles X1 to X7 are close to the waypoints that are the current movement targets, the formation correction time is irrelevant to the passage target time of the waypoints. , It may be set after a certain T seconds.

In these cases, each control device 4 has its own machine at the time of the formation correction based on the waypoint files of the own machine and other underwater vehicles X1 to X7 including the essential positioning target stored in the storage unit 5. And the coordinates of the planned movement positions of the other underwater vehicles X1 to X7 including the essential positioning target are obtained. Next, in each control device 4, the target value of the relative position to be taken by the own aircraft and the other underwater vehicles X1 to X7 including the essential positioning target can be obtained from the obtained difference between the planned movement positions. Good.

Specifically, the control device 4 of each underwater vehicle X1 to X7 is based on, for example, information on the detection result of the relative position of the other underwater vehicle X1 to X7 positioned by the positioning means 2 with respect to its own machine. , The process of adding a correction term to the control input obtained by the above equation (1) is performed. The control input to which this correction term is added is given by the following equation (2).
(Control input) = (Input obtained from the waypoint file of the own machine)
+ (Correction input obtained from acoustic positioning and graph theory) ... (2)

The first term on the right side of the equation (2) is the same as the right side of the equation (1). The second term on the right side is a correction term, and in the present embodiment, the correction term is PI control as shown in the following equation (3).
u _i = J _i * 1 / D _ii * Σ _{j = 1 to n} [A _ij * {K _i * (d _ij − p _ij ))
+ I _i * ∫ ( _dig − p _ij ) dt}]… (3)
Correction term = transformation matrix * (proportional control + integral control)

u _i, as a correction term, an input obtained from a positioning result and graph theory by the positioning means 2, which is a command to become the value of the control input to the actuator 3.

J _i is the earth coordinate system detected by the position detecting means 1 underwater vehicle X1~X7 and (inertial coordinate system when using an inertial navigation system), a transformation matrix between the actuators coordinate system.

Here, in the underwater vehicles X1 to X7, when there are a plurality of actuators 3 such as various thrusters and steering wheels, the arrangement and orientation of the respective actuators 3 are involved in the translational motion and the rotational motion, and the translational motion is involved. It is involved in the direction and speed of travel of the aircraft, which is the result of rotational movement. Therefore, in the underwater vehicles X1 to X7, when the individual control amounts of the plurality of actuators 3 are determined by a matrix and combined, as a result of the control of the plurality of actuators 3, the underwater vehicles X1 to X7 in the earth coordinate system The direction of movement and speed are determined.

Therefore, a matrix that synthesizes the control amount of the actuator 3 defined from the arrangement and orientation of the actuator 3 is called an actuator coordinate system. This is a matrix of the number of actuators × 6th order.

Di _i is an i-row i-column component of the degree matrix D of the graph, and means the number of adjacent numbers in the graph. The number of other underwater vehicles X1 to X7 for which the i-th underwater vehicle X1 to X7 can be positioned is substituted.

A _ij is a component of the i-th row and j-column of the adjacency matrix A of the graph, and if the i-th underwater vehicle X1 to X7 can position the j-th underwater vehicle X1 to X7, it must be 1. If 0 is substituted.

_dij is a target value of the relative position between the i-th underwater vehicle X1 to X7 and the j-th underwater vehicle X1 to X7.

p _ij is the current value of the relative positions of i-th underwater vehicle X1 to X7 and j th underwater vehicle X1 to X7.

_Ki is a proportional gain. This proportional gain may be determined by a commonly used method such as the limit sensitivity method.

I _i is the integral gain and may be determined by a general method. If the positioning is interrupted for a certain period of time or longer, the integrated value shall be reset to zero.

In the mobile control system of the present embodiment including the underwater navigation bodies X1 to X7 having the above configuration, each underwater navigation body X1 to X7 is arranged in a horizontal row I-shaped formation shown in FIG. 2 (a). Assemble and move.

At this time, if any one of the underwater vehicles X1 to X7 sails off the movement course set in the waypoint file for obstacle avoidance, etc., the vehicle deviates from the movement course. In the underwater vehicles X1 to X7, the measured values of the relative positions of the own aircraft and other underwater vehicles X1 to X7 including the essential positioning target deviate from the target values of the relative positions.

Further, in the other underwater navigation bodies X1 to X7 whose essential positioning targets are the underwater navigation bodies X1 to X7 off the moving course, the other underwater navigation bodies X1 to X7 including the own aircraft and the essential positioning target are also included. The measured value of the relative position of is deviated from the target value of the relative position.

In this way, in the underwater vehicles X1 to X7 in which the measured value of the relative position deviates from the target value of the relative position, the control device 4 commands the actuator 3 to control input for eliminating the change. Will be given as.

As a result, as shown in the outline in FIG. 4, the target value of the relative position of the underwater vehicle Xi and the underwater vehicle Xj at the time of formation modification is as shown by the alternate long and short dash line P0 in FIG. When the current value of the relative position is in the state shown by the solid line P1 in FIG. 4, the current value P1 of this relative position is corrected so as to match the target value P0 of the relative position at the time of formation correction.

Therefore, each of the underwater vehicles X1 to X7 can move while maintaining the relative positional relationship, and can hold the formation of the predetermined shape as a whole. Alternatively, even if the formation of the predetermined shape temporarily collapses, each of the underwater vehicles X1 to X7 can move by reassembling the formation of the predetermined shape.

Therefore, according to the mobile body control method and the mobile body control system of the present embodiment, it is possible to move a plurality of underwater vehicles X1 to X7 in a state of forming a formation having a predetermined shape. At this time, it is not necessary to determine the main engine and the follower of each of the underwater vehicles X1 to X7.

Further, in the mobile body control method and the mobile body control system of the present embodiment, the underwater vehicle X1 to X7 need only perform positioning of the other underwater vehicle X1 to X7 by the positioning means 2, and communication is required. Do not. In general, the signal used for positioning may have a smaller amount of information than the signal used for communication, so that the distance at which positioning is possible is larger than the distance at which communication is possible.

Therefore, in the mobile body control method and the mobile body control system of the present embodiment, it is possible to increase the size of the formation composed of the underwater navigation bodies X1 to X7.

Further, since it is sufficient that the individual underwater vehicles X1 to X7 can perform positioning of other underwater vehicles X1 to X7 existing in the positioning range of the positioning means 2 equipped in the own machine. It is possible to eliminate the limitation on the number of underwater navigation bodies X1 to X7 that form a formation.

It is preferable that the correction term of the control input of the actuator 3 obtained by the control device 4 is PI control as in the equation (3). This is because both the proportional control term and the integral control term are feedback control of the target value and the current value, and when the difference between the target value and the current value of the relative position becomes large, the influence of the correction term becomes large.

Further, the correction term is existing or conventionally proposed as a control rule for following the target value, such as P control, PID control, and feedback control as shown below, instead of the PI control. Any control law may be applied.

In the case of P control, the correction term may be as shown in Eq. (4) below.
u _i = J _i * 1 / D _ii * Σ _{j = 1 to n} [A _ij * {K _i * ( _dig − p _ij )}]… (4)
Correction term = transformation matrix * proportional control

In the case of PID control, the correction term may be as shown in Eq. (5) below.
u _i = J _i * 1 / D _ii * Σ _{j = 1 to n} [A _ij * {K _i * (d _ij − p _ij ))
+ I _i * ∫ ( _dig − p _ij ) dt + Di * (0-v _ij )}]… (5)
Correction term = transformation matrix * (proportional control + integral control + differential control)

In equation (5), Di is a differential gain. v _ij is the relative speed between the i-th underwater vehicle X1 to X7 and the j-th underwater vehicle X1 to X7. The goal of the differential control term is to set the relative velocity to zero.

In the case of feedback control, the correction term may be as shown in Eq. (6) below.
u _i = J _i * 1 / D _ii * Σ _{j = 1 to n} {A _ij * F _ij }… (6)
Correction term = transformation matrix * feedback control

(6) In the _{formula, F ij} is the i-th underwater vehicle X1~X7 and j th underwater vehicle X1~X7 feedback control of the relative positional relationship to be taken.

In the case of P control, the position detecting means 1 may be provided with a function of detecting the position of its own machine of the underwater vehicle X, as in the case of the PI control described above. On the other hand, in the case of PID control, the position detecting means 1 may be provided with a function of detecting the position and speed of the underwater vehicle X. Further, in the case of feedback control, the position detecting means 1 may be provided with a function of detecting information necessary for feedback control of the underwater vehicle X itself.

By the way, in the present embodiment, by setting the underwater navigation bodies X1 to X7 to be essential positioning targets in each of the underwater navigation bodies X1 to X7, a graph structure of a spanning tree from which consensus conditions can be obtained in the graph theory is formed. ing.

Therefore, if it becomes completely impossible for each underwater vehicle X1 to X7 to perform positioning of the essential positioning target underwater vehicle X1 to X7, the agreement condition (graph connection condition) of the graph theory is broken. It will be impossible to maintain the formation.

As one of the methods for avoiding such a situation, the target value of the relative position between each underwater vehicle X1 to X7 and the essential positioning target underwater vehicle X1 to X7 can be determined by the positioning means 2. It should be set sufficiently small compared to the limit of.

Alternatively, the control device 4 of each underwater vehicle X1 to X7 may further perform the processing as shown in the following application example.

[Application example of the first embodiment]
In this application example, in addition to the functions described in the first embodiment, the control device 4 of each underwater vehicle X1 to X7 becomes unstable in positioning of the underwater vehicle X1 to X7, which is an essential positioning target. , It has a function to control its own machine to reestablish the positioning before it becomes completely impossible to position.

Here, the unstable positioning means that the positioning process by the positioning means 2 is repeated at a certain interval, and the positioning process is performed for a set period in the past or a set number of times in the past. This means that the ratio of the number of successful positionings for the underwater vehicles X1 to X7, which are the essential positioning targets, is equal to or less than the set threshold value. For example, in the control device 4 in the past 10 positioning processes by the positioning means 2, the positioning becomes unstable when the ratio of the number of successful positionings of the underwater vehicles X1 to X7, which are essential positioning targets, is 50% or less. You just have to judge.

Alternatively, when the positioning means 2 has a function of outputting information about the positioning status and the positioning accuracy, the control device 4 sets the positioning status and the positioning accuracy based on the information. If it falls below the threshold value, it may be determined that the positioning is unstable.

When the positioning of the underwater vehicles X1 to X7, which are the essential positioning targets, becomes unstable, the control device 4 of the underwater vehicles X1 to X7 moves in the direction of approaching the underwater vehicles X1 to X7, which are the essential positioning targets. The control input to be performed is commanded to the actuator 3.

At this time, the control device 4 has not sufficiently obtained positioning information about the current positions of the underwater vehicles X1 to X7, which are essential positioning targets. Therefore, the control device 4 may predict the positions of the underwater vehicles X1 to X7, which are essential positioning targets, based on the following two pieces of information.

The first is the positioning result of the underwater vehicles X1 to X7, which are essential positioning targets until the positioning becomes unstable. The second is the information of the waypoint files of the underwater vehicles X1 to X7, which are the essential positioning targets, stored in the storage unit 5. As the prediction method, a general method such as linear approximation or quadratic approximation may be used.

The control device 4 commands the actuator 3 to give a control input to move the aircraft in the direction of approaching the positions of the underwater vehicles X1 to X7, which are the essential positioning targets predicted in this way.

As a result, each of the underwater vehicles X1 to X7 moves in a direction approaching the underwater vehicles X1 to X7, which are essential positioning targets.

With this movement, when the unstable situation of positioning of the underwater vehicles X1 to X7, which are the essential positioning targets, is resolved, the control device 4 resumes the control of the first embodiment described above.

As a result, according to the moving body control method and the moving body control system of the present application example, the positioning means 2 of each underwater vehicle X1 to X7 can position the underwater vehicle X1 to X7, which is an essential positioning target. Since the state can be maintained, the formation by the underwater navigation bodies X1 to X7 can be more reliably or more stably maintained.

[Modified example of the first embodiment]
In the first embodiment, as shown in FIG. 2A, an example in which seven underwater vehicles X1 to X7 move in a horizontal row of I-shaped formations has been described. However, it may be applied when moving in a single row extending diagonally or in a vertical single row formation.

Examples of a series of formations (a shape that allows one-stroke writing) include a V-shaped formation as shown in FIG. 2 (b) and a shape in which the front and back of the formation in FIG. 2 (b) are interchanged (inverted V). It may be applied when moving in a formation of (character type). In this case, the number of the underwater vehicles X1 to X7 arranged in one straight line portion of the V shape and the other straight line portion may be different.

Further, it may be applied when a plurality of underwater navigation bodies X as shown in FIG. 2C, for example, eight underwater navigation bodies X1 to X8 sail in an O-shaped formation. ..

In this way, when the formation has a circular configuration, the first underwater vehicle X1 may be configured to set the last underwater vehicle X8 as an essential positioning target. In this case, the consensus condition in graph theory is that the graph structure is strongly connected. In this case as well, the same effect as that of the first embodiment can be obtained.

The present invention is not limited to the embodiment, and in the embodiment, the application example, and the modified example, any one of the underwater vehicles X1 to X7 and X8 positions the unmanned surface boat. It may be provided with a function. In this case, if the unmanned surface boat is equipped with a receiver of the Global Navigation Satellite System (GNSS), the position information in the earth coordinate system can be obtained. Therefore, this unmanned surface boat is correctly set in the earth coordinate system. If you move the movement course, in addition to maintaining the shape of the formation of the underwater navigation bodies X1 to X7, X8, the position of the formation is set in the earth coordinate system at the waypoints of each underwater navigation body X1 to X7, X8. It can be corrected according to the course.

Further, a landmark whose position in the earth coordinate system is known may be installed on the seabed, and any of the underwater vehicles X1 to X7 and X8 may have a function of positioning the landmark. In this case, when the inertial navigation system is used as the position detecting means 1 of the underwater navigation bodies X1 to X7 and X8, the underwater navigation bodies X1 to X7 and X8 pass near the landmark. If the positioning is possible, the position error of the inertial navigation system as the position detecting means 1 can be corrected. The underwater vehicles X1 to X7 and X8 having the positional error corrected in this way can move correctly and therefore to the waypoints set in the earth coordinate system. Therefore, when the other underwater vehicles X1 to X7 and X8 position the underwater vehicles X1 to X7 and X8 in which this position error is corrected, the shape of the formation of the underwater vehicles X1 to X7 and X8 is formed. In addition to holding, the position of the formation can be corrected to match the course set in the Earth coordinate system at the waypoints of each underwater vehicle X1 to X7, X8.

The mobile body control method and the mobile body control system of the present invention may be applied to a moving body other than the underwater navigating body as long as it is a moving body to be moved in a formation.

Of course, various other changes can be made without departing from the gist of the present invention.

1 Position detection means, 2 Positioning means, 3 Actuator, 4 Control device, 5 Storage unit, X, X1, X2, X3, X4, X5, X6, X7, X8 Underwater vehicle (moving body), v1, v2 v3 vertex, a1, a2, a3 arc, P0 relative position target value, P1 relative position current value

Claims (3)

For multiple mobiles that move in formation
Each moving body is set as the apex in the graph theory, and the positionable state of the other moving body by the positioning means provided for each moving body is used as an arc in the graph theory. The process of setting a certain essential positioning target mobile object to a state in which a spanning tree or a strongly connected graph structure is formed.
A process of storing the movement plan of the own machine and the movement plan of another moving body including the essential positioning target to be positioned by the positioning means is performed in the storage unit provided for each moving body.
When moving in formation of each of the above moving bodies,
A process of positioning another mobile body including the essential positioning target by the positioning means of each mobile body, and
In each of the moving bodies, a process of obtaining an actually measured value of a relative position between the own machine and another moving body including the essential positioning target at the positioning time point from the positioning result of the positioning means.
Based on each movement plan stored in the storage unit, the relative position to be taken by the own aircraft and other moving objects including the essential positioning target at the formation correction time set after the positioning time. The process of finding the target value and
A process of obtaining a correction target value by adding the actually measured value of the relative position and the correction value obtained from the graph theory to the target value.
With respect to the measured value of the relative position, the process of obtaining the deviation from the correction target value and
A mobile body control method characterized in that a process of giving a control input to an actuator of the own machine is performed so that the deviation becomes zero at the time of the formation correction. The mobile body control method according to claim 1, wherein the formation formed by the plurality of moving bodies is formed into a series of formations. For mobiles that move in formation
Position detection means to detect the position of your own machine,
Positioning means for positioning other mobile objects and
Actuator and
Control device and
With a memory
In the control device, each mobile body forming the formation is set as the apex in the graph theory, and the positionable state of the other moving body by the positioning means of the moving body is set as an arc in the graph theory, and is a spanning tree or strongly connected. Indispensable positioning target mobiles, which are other mobiles that must be positioned, are set so that a graph structure is formed.
The storage unit has a function of storing a movement plan of the own machine and a movement plan of another moving body including the essential positioning target positioned by the positioning means.
Further, the control device is
From the positioning result of the positioning means, a process of obtaining an actually measured value of a relative position between the own machine and another moving body including the essential positioning target, and
Based on each movement plan stored in the storage unit, the relative position to be taken by the own aircraft and other moving objects including the essential positioning target at the formation correction time set after the positioning time. The process of finding the target value and
The process of obtaining the correction target value by adding the actually measured value of the relative position and the correction value obtained from the graph theory to the target value, and
With respect to the measured value of the relative position, the process of obtaining the deviation from the correction target value and
A mobile control system characterized by having a function of performing a process of giving a control input to the actuator so that the deviation becomes zero at the time of the formation correction.

Images