JP2018181166A - Mobile body control method and mobile body control system - Google Patents

Abstract

PROBLEM TO BE SOLVED: To avoid collision of an underwater mobile body against an obstacle under environment where a tidal current is present.SOLUTION: An underwater mobile body 1 is configured to include a state amount detection part 2, an obstacle detection part 3, a mobile device 4, a calculation part 5, and a storage part 6. Upon reception of a current state amount of the own machine from the state amount detection part 2, based on information of a position of an obstacle 7 detected by the obstacle detection part 3, the calculation 5 calculates an obstacle avoidance restriction function as condition in which the own machine does not collide against the obstacle 7 and also calculates a margin function based on uncertainty of the state amount of the own machine and disturbance acting on the own machine, and then performs model prediction control with a function, obtained by adding the calculated margin function to the obstacle avoidance restriction function, provided as restriction to obtain an optimum control amount of the mobile device 4. Then the calculation part 5 provides the obtained optimum control amount to the mobile device 4 as a target value of a control amount of the mobile device 4.SELECTED DRAWING: Figure 1

Description

  The present invention relates to a method of controlling a moving body used to cause an autonomously controlled moving body to perform obstacle avoidance, and a moving body control system used to implement the method.

  An unmanned underwater vehicle called AUV (Autonomous Underwater Vehicle) is known as one of mobile vehicles performing autonomous movement.

  This type of underwater vehicle is considered to be used for seabed resource exploration, exploration and removal of mines, exploration of ocean distressed persons (exploration) and the like.

  One of the important techniques for operating such underwater vehicles is collision avoidance with other vehicles and obstacles such as reefs.

  As a technology for autonomously controlling a mobile body to perform obstacle avoidance, for example, a technology related to the following collision avoidance control of a vehicle has been conventionally proposed (for example, see Patent Document 1).

  This creates a vehicle model and a target model from data on the traveling state of the vehicle and data on the distance between the target and the vehicle and the relative velocity of the target, and the model predictive control allows the vehicle to avoid collisions. This is a technique of searching for the optimum operation amount regarding the actuator for traveling the optimum route, and inputting the obtained optimum operation amount into the actuator of the own vehicle, for example, the brake, the steering, and the accelerator. According to this technology of collision avoidance control, it is supposed that the vehicle can automatically travel on the optimum route to avoid a collision.

JP 2007-276508 A

  By the way, the vehicle obtains the propulsive force and the turning force using the frictional force between the wheels and the fixed ground. Therefore, in a vehicle traveling on a solid ground, if the angle of turn (steering angle) of the steered wheels operated by the steering and the number of rotations of the drive wheels operated by the brake or accelerator are determined, The amount of change, the moving speed, and the amount of movement are almost uniquely determined.

  However, when the underwater moving body is configured to include, for example, a propulsion unit such as a propeller and a steering unit such as a rudder as a moving device for changing the state quantity of its own aircraft, the number of revolutions of the propeller by the propulsion unit The movement speed and movement of the underwater moving body are due to the influence of the inertia of the underwater moving body and the movement of the water pushed by the body of the underwater moving body, rudder and propeller, etc. The amount is not fixed uniquely.

  In addition, even if the steering angle of the rudder is determined by the steering means, the speed of the change of the direction of the underwater moving body and the amount of change can not be uniquely determined because the rudder is easily influenced by the moving speed of the underwater moving body.

  Furthermore, in the underwater moving body, disturbances such as tidal current may exist in the environment to be used, in which case the state quantities such as the position and attitude of the underwater moving body, moving speed, and angular velocity related to attitude change are disturbances. Change under the influence.

  Moreover, in the underwater moving body, the inertial navigation system is often used as a means for detecting the state quantity of the own aircraft when moving under the water surface, but the inertial navigation system makes the use time of the underwater moving body longer. It has the characteristic that errors accumulate. Therefore, in the underwater moving body, an error may be included in the detection result of the state quantity of the own machine detected by the inertial navigation device.

  Therefore, since the underwater mobile body includes the uncertainty of the controlled state quantity and the detected state quantity, the collision avoidance control method disclosed in Patent Document 1 applies to collision avoidance of the underwater mobile body. It is not possible.

  In addition to the underwater moving body, autonomous moving ships that receive tidal currents and wind as disturbances, floating moving bodies called ASV, etc., are the moving bodies that include uncertainty and the state quantity to be controlled includes uncertainty. , Autonomously controlled aircraft that receive wind as disturbance, unmanned aerial vehicles called UAV, etc., spacecraft that receives radiated pressure of electromagnetic waves from the sun as disturbance, and autonomous control type that travels on a soft and sloping ground There are also crawler-type and wheel-type autonomous vehicles. Here, the uncertainty related to the state quantity is a true state because the detection accuracy of the state quantity detection means is limited when controlling the state quantity such as the position and posture of the moving body, moving speed, and angular velocity related to posture change. It means that the quantity can not be detected and the detected value can not be determined uniquely according to a probability distribution such as a normal distribution, for example, with respect to the true state quantity.

  Therefore, the present invention provides a control method and a mobile object control system of a mobile object capable of performing obstacle avoidance in an environment where there is a disturbance for a mobile object having uncertainty in control of the state quantity of the own aircraft. It is something to try.

  The present invention is a control method of a movable body that controls a movable body including a state quantity detection unit, an obstacle detection unit, and a movement device in order to solve the above-mentioned problem, wherein the state quantity detection unit Performs the process of detecting the current amount of state of the moving object, and the process of detecting the obstacle by the obstacle detection unit, and the calculation unit further detects the obstacle detected by the obstacle detection unit. A process of calculating an obstacle avoidance constraint function as a condition that the mobile does not collide with the obstacle based on the information on the position of the vehicle, the uncertainty of the state quantity of the mobile with respect to the state transition equation of the mobile, and A process of calculating a margin function based on a disturbance acting on a mobile object, and a function obtained by adding the margin function to the obstacle avoidance constraint function as a constraint, model prediction control is performed to determine an optimal control amount of the mobile device Processing and the above determined A control amount, to the mobile device, a control method for a moving body to perform the a process of giving a target value of the control amount of the mobile device.

  The moving body is an underwater moving body, and a disturbance acting on the underwater moving body is a current flow.

  The mobile device further includes a state amount detection unit, an obstacle detection unit, a movement device, and a calculation unit provided in the mobile object, and the calculation unit may calculate the current state amount of the mobile object detected by the state amount detection unit. And the condition that the moving object does not collide with the obstacle based on the function of receiving the information of the obstacle detected by the obstacle detection unit and the information of the position of the obstacle detected by the obstacle detection unit. The function of calculating an obstacle avoidance constraint function, the function of calculating a margin function based on the uncertainty of the amount of state of the moving body with respect to the state transition equation of the moving body, and the disturbance acting on the moving body, and the margin function A function of adding the obstacle avoidance constraint function to the function to perform model predictive control to obtain an optimal control amount of the mobile device, and the obtained optimal control amount for the mobile device to the mobile device Control of the device The mobile control system having a structure comprising a function of giving a target value.

  According to the mobile object control method and mobile object control system of the present invention, obstacle avoidance can be performed in an environment where there is a disturbance, for a mobile object having uncertainty in the control of the state quantity of the mobile object.

It is a schematic diagram showing a 1st embodiment of a mobile control system. It is a schematic diagram showing the state where a mobile performs obstacle avoidance. It is a figure for demonstrating the setting method of the azimuth | direction angle of a disturbance. It is a schematic diagram showing the modification of a 1st embodiment.

  Hereinafter, embodiments of the present invention will be described with reference to the drawings.

First Embodiment
FIGS. 1 to 3 show an example in which the present invention is applied to an underwater mobile body as a mobile body as a first embodiment of a mobile body control method and a mobile body control system. Further, in the present embodiment, the disturbance acting on the moving body is an example of the tidal current acting on the underwater moving body. In the present embodiment, a case in which the underwater moving body performs obstacle avoidance in the space of two-dimensional coordinates in the horizontal direction will be described.

  FIG. 1 is a schematic view showing a first embodiment of a mobile control system. FIG. 2 shows a moving route for obstacle avoidance set by the control method of the moving object, and FIG. 2 (a) is a schematic view showing the moving route set according to the present embodiment, FIG. b) is a schematic view showing a movement route set when the direction of disturbance is not considered as a comparative example. FIG. 3 is a schematic diagram for explaining the setting method of the azimuth angle of the tidal current.

  The mobile control system according to the present embodiment is shown in FIG. 1 and includes a state quantity detection unit 2 mounted on the underwater mobile 1, an obstacle detection unit 3, a mobile device 4, a calculation unit 5, and storage. And a unit 6.

  The state quantity detection unit 2 uses, as state quantities, the position of the underwater moving body 1, the moving speed, the attitude including the direction to which the machine is facing, and the angular velocity related to the change in the direction to which the machine is directed. It has a function to detect. As a configuration example of the state quantity detection unit 2 which realizes this function, for example, a configuration provided with an inertial navigation system or a configuration provided with an inertial navigation system and a Doppler ultrasonic speedometer may be used.

  The state quantity detection unit 2 may detect the position and the attitude of the underwater mobile unit 1 itself as coordinates in a two-dimensional coordinate system fixed on the surface of the earth. In addition, although it is suitable to use the coordinate system by the latitude and the longitude as this two-dimensional rectangular coordinate system, it is not limited to it.

  Of course, the state quantity detection unit 2 may adopt any device configuration other than that described above as long as it can realize the detection function of the state quantity.

  When the state quantity detection unit 2 detects the state quantity, it sends the detection result to the calculation unit 5.

  The obstacle detection unit 3 is located in front of the traveling direction of its own aircraft as shown in FIG. 2 (a) when the underwater moving vehicle 1 moves, and an obstacle 7 which becomes an obstacle to the movement of the own aircraft. It has a function to detect In addition, when the underwater moving body 1 moves along the moving path set as waypoint control, the obstacle detection unit 3 is downstream of the point where the underwater moving body 1 in the moving path is currently located. And a function of detecting an obstacle 7 present on the moving path of the vehicle. As a specific configuration example of the obstacle detection unit 3 for realizing these functions, for example, a configuration provided with an acoustic sonar may be used. In addition, as long as the obstacle detection unit 3 has the detection function of the obstacle 7 as described above, it may be configured to combine an acoustic sonar and another detection means (sensor), or other than the acoustic sonar. It is good also as composition provided with the detection means of.

  The obstacle detection unit 3 has a function of sending information of the detected obstacle 7 to the calculation unit 5 when the obstacle 7 is detected. At this time, the obstacle detection unit 3 may transmit, to the calculation unit 5, information on the relative position of the obstacle 7 based on the current position of the own device. In this case, the calculation unit 5 is fixed on the surface of the earth based on the information on the obstacle 7 sent from the obstacle detection unit 3 and the information on the position and attitude of the own machine sent from the state quantity detection unit 2 The position coordinates of the obstacle 7 in the two-dimensional coordinate system can be obtained by calculation. The function of this calculation may be provided in the obstacle detection unit 3 and information on the position coordinates of the obstacle 7 obtained by the calculation may be sent to the calculation unit 5.

  The moving device 4 is an actuator, such as a thruster or a rudder, provided in the underwater moving body 1, and the control output of the moving device 4 changes the state quantity of the position, velocity, attitude, and angular velocity of the underwater moving body 1 described above. .

  The mobile device 4 has a function of operating and operating according to the command when receiving the command (control input) from the calculation unit 5.

  Of course, as long as the moving device 4 can change the amount of state of the underwater moving body 1, the type of the moving device, the arrangement in the underwater moving body 1, the number, and the like may be arbitrary.

  The storage unit 6 has a function of storing the moving path of the underwater moving body 1 and data on the direction of the tidal current T (see FIG. 2A) existing in the environment where the moving path is set (hereinafter referred to as tidal current direction And a function to store data).

  When control of the movement of the underwater moving body 1 is performed by, for example, the waypoint method, the storage unit 6 may store the movement path as a waypoint file. In addition, the form which memorize | stores the movement path | route of the underwater mobile 1 by the memory | storage part 6 is not restricted to a waypoint file, Of course, according to the system which controls the movement of the underwater mobile 1, you may change suitably. It is.

  It is preferable to acquire the tidal current direction data stored in the storage unit 6 from, for example, the information on the tidal current T released for each sea area in a database of the Japan Coast Guard, etc. As long as it is tidal direction data about the environment where it has been stored, it may be from another information source.

  When the calculation unit 5 receives the detection result of the state amount of the underwater moving object 1 from the state amount detection unit 2 and receives the information of the obstacle 7 detected by the obstacle detection unit 3 from the obstacle detection unit 3, It has a function of giving a command to the moving device 4 by the processing of

  The state transition equation of the underwater vehicle 1 obtained in advance is given to the calculation unit 5 as the equation (1).

x _{i + 1} = f (x _i , u _i ) (1)
Where x _i is the amount of state of the underwater moving object, u _i is the amount of control (control output of moving device 4), i is discrete time

In this state, when the calculation unit 5 receives the detection result of the current state amount of its own mobile unit of the underwater moving body 1 from the state amount detection unit 2, it sets the current state amount as x ₁ .

Further, when the calculation unit 5 receives the information of the obstacle 7 from the obstacle detection unit 3, the central position of the obstacle 7 is o ₁ and the size is radius D based on the detected position and size of the obstacle 7. Approximate with _Max sphere or circle.

Furthermore, as shown in FIG. 2A, the calculation unit 5 acquires information on the target position 8 which is the movement target from the current position of the underwater mobile body 1 from the storage unit 6, and the underwater mobile body 1 A target state quantity x ^d which is a state quantity when existing at the target position 8 is obtained.

  Furthermore, the calculation unit 5 acquires the tidal current direction data on the current position of the underwater mobile body 1 from the storage unit 6 and, as shown in FIG. 3, the tidal current direction angle φ at the current position of the underwater mobile body 1 obtain. In FIG. 3, for example, in the two-dimensional coordinate system fixed to the same surface of the earth as used in the state quantity detection unit 2, the direction of the tidal current T is the reference (0 degree) X The angle is set to be inclined counterclockwise from the positive direction of the axis. Note that the tidal flow direction angle φ is an arbitrary two-dimensional coordinate system fixed on the same surface of the earth as used in the state quantity detection unit 2 as long as the direction of action on the underwater mobile body 1 can be clearly defined. Of course, it may be set based on the direction.

  Next, the calculation unit 5 calculates the control amount based on each of the above-mentioned information.

  Model predictive control is used to calculate the control amount in the calculation unit 5. In model predictive control, the optimal control amount matrix u of the next moving device 4 is obtained by solving the optimization problem shown in the following equation (2).

  Note that the solution to the optimization problem may be a solution generally used conventionally, such as the Newton method.

J (u) in the above equation (2) is an evaluation function, and h _i (u) is a collision avoidance constraint function, which are respectively described by the following equations (3) and (4).

ただし、ｇ_ｉ（ｕ）は、障害物回避制約関数であり、ｇ_ｉ（ｕ）＝Ｄ_Ｍａｘ−Ｄ_ｉ＞０で記述される。
Ｄ_ｉは、ｉステップ目の水中移動体１の位置と障害物７の中心位置ｏ_１との間の距離である。ｉステップ目の水中移動体１の位置の座標を（Ｘ_ｉ，Ｙ_ｉ）とし、障害物７の中心位置ｏ_１の座標を（Ｘ_ｏ，Ｙ_ｏ）とすると、Ｄ_ｉは、以下の式で記述される。

Where g _i (u) is an obstacle avoidance constraint function, and is described by g _i (u) = D _Max −D _i > 0.
D _i is a distance between the position of the i-th underwater vehicle 1 and the center position o ₁ of the obstacle 7. Assuming that the coordinates of the position of the underwater vehicle 1 at the i-th step are (X _i , Y _i ) and the coordinates of the center position o ₁ of the obstacle 7 are (X _o , Y _o ), D _i is Described in

  In addition, the evaluation function shown by said Formula (3) is an example, You may employ | adopt other evaluation functions generally used by model predictive control besides this format. Further, the obstacle avoidance constraint function described above is an example of a condition under which the underwater moving object 1 does not collide with the obstacle 7, and when the shape approximating the size of the obstacle 7 is other than a sphere or a circle, Therefore, a function representing a condition that the position of the underwater mobile 1 at the i-th step does not interfere with the obstacle 7 may be adopted appropriately.

The state quantity x _i of the underwater mobile body 1 is obtained from the state transition equation shown in the equation (1), the current state value x ₁ detected by the state quantity detection unit 2 and the optimal control amount matrix u.

Further, σ _i in the equation (4) is a margin function (margin term) calculated according to the uncertainty of the amount of state of the underwater mobile body 1.

Next, a method of deriving σ _i will be described.

sigma _i can be obtained from a partial derivative matrix _B i of the obstacle avoidance constraint function _{(i = 0 ... N p)} , and the error variance matrix CovX _i of the underwater vehicle _{1 (i = 0 ... N p} ).

The partial differential matrix B _i (i = 0... N _p ) of the obstacle avoidance constraint function is expressed by the following equation (5).

Further, the error dispersion matrix CovX _i (i = 0... N _p ) of the underwater mobile body 1 is expressed by the following equation (6).

Here, dτ in the equation (6) is a time per one horizon. Moreover, _Ai is a partial differential matrix of said Formula (1), and is given by the following (7) Formula.

N _proc in the equation (6) is the process noise of the underwater mobile 1 and may have the same value as the process noise set by a general state estimation method such as a Kalman filter.

  R (φ) in the equation (6) is a rotation matrix, and is expressed by the following equation (8).

N _f in the equation (6) is a process noise related to the current. N _f is given, for example, by the following equation (9), where an assumed flow velocity is v _f .

The value of v _f can be appropriately changed according to the flow velocity of the region in which the underwater moving body 1 is moved, which leads to the improvement of control performance. However, the tidal velocity of the region for moving the underwater mobile body 1 may be unknown. Thus, when the tidal velocity is unknown, if v _f is set to a value larger than the actual tidal velocity as a guideline for the set value, the possibility of the underwater moving object 1 colliding with the obstacle 7 is reduced. Can be implemented. Therefore, the tidal current T as a disturbance may have a constant tidal velocity, as long as the tidal flow direction angle φ is determined in a certain direction, or it may periodically change, for example, due to the influence of waves. . Furthermore, the periodic change of the flow velocity may include one in which the flow direction angle φ is constant and the intensity changes from the positive direction to the negative direction.

  Further, α in the equation (9) is a parameter that means the ratio of the major axis to the minor axis of the ellipse shown by the broken line in FIG. 2A, and the range is 0 <α <1. Therefore, the smaller the α, the longer the major axis with respect to the minor axis.

Based on the equations (5) to (9), σ _i can be defined by the following equation (10).

As described above, in the present embodiment, by defining σ _i by the equation (10), it is possible to set an appropriate margin in accordance with the uncertainty of the state quantity of the underwater mobile body 1.

When the calculation unit 5 solves the optimization problem shown in the equation (2) based on the above calculation and finds the optimum control amount matrix u of the mobile device 4, the first component u ₁ is calculated by the mobile device 4. It has a function of performing processing for giving the mobile device 4 as a target value of the control amount of

In the above, _xi has six variables of position / posture, velocity, and angular velocity as state quantities, but this is an example, and in the present embodiment, the state quantities do not depend on the number of variables.

When the control method of the underwater moving body 1 is implemented using the moving body control system of the present embodiment having the above configuration, the underwater moving body 1 moves along the moving path toward the target position 8 as shown in FIG. When the obstacle detection unit 3 (see FIG. 1) detects an obstacle 7 present in front of the traveling direction during movement of the vehicle, the calculation unit 5 performs the above-described processing, and the movement device 4 obtained as a result thereof The first component u ₁ of the optimal control amount matrix u of is applied to the mobile device 4 as a target value of the control amount of the mobile device 4.

  Therefore, the operation and operation of the mobile device 4 are controlled in accordance with the target value of the control amount received from the calculation unit 5.

Thus, as shown in FIG. 2 (a), for the location p _i of the underwater vehicle 1 of the i-th step _{(i = 0 ... N p)} , as shown by the dashed ellipse, respectively shown in FIG. 2 (a) A movement range 9 is set in consideration of the uncertainty of the state quantity and further in consideration of process noise related to the power flow, and the movement range 9 can predict and set a movement path not overlapping the obstacle 7.

  Therefore, according to the mobile object control method and mobile object control system of the present embodiment, for the underwater mobile object 1 having uncertainty in the control of the state quantity of the own vehicle, the obstacle 7 is avoided in the environment where disturbances exist. Can be done.

Incidentally, FIG. 2 (b), as a comparison, the process noise N _f about trends, without any adjustment based on the flow direction angle phi, i.e., those in the case of considering the process noise N _f about trends in all directions In this case, the movement range 9a taking into account the uncertainty of the state quantity for the position p _i (i = 0 ... Np) of the i-th step underwater moving body 1 is a circle as shown by a broken line in FIG. Become. This indicates that the movement range 9a is expanded also in the direction orthogonal to the power flow direction, and the process noise is estimated to an unnecessary area, and the uncertainty of the state quantity is overestimated. Become. As a result, the movement route set so that the movement range 9a does not overlap the obstacle 7 will bypass the obstacle 7 more largely, and as shown in FIG. 2 (b), the obstacle 7 and the target position Depending on the distance to 8, it may be difficult to set a movement path for the underwater mobile body 1 to reach the target position 8.

  On the other hand, in the present embodiment, as shown in FIG. 2A, the moving range 9 in consideration of the process noise related to the tidal current becomes an elliptical shape whose range is expanded only in the direction along the tidal direction. Therefore, in the present embodiment, the movement path of the underwater moving body 1 can be set closer to the obstacle 7 in the direction orthogonal to the power flow direction. For this reason, in the present embodiment, it is possible to shorten the detour distance for avoiding the collision with the obstacle 7.

  Further, according to the present embodiment, since estimation of the uncertainty of the state quantity from the obtained information can be suppressed to a smaller value, it is possible to achieve both of the robust performance and the control performance.

  In the present embodiment, the underwater moving object 1 is caused to avoid the obstacle 7 in the two-dimensional coordinate space in the horizontal direction. However, the control method and the moving object control of the moving object according to the present embodiment are described. The system may be applied to the case where the underwater moving object 1 is made to avoid the obstacle 7 in a three-dimensional coordinate space.

In this case, the state transition equation of the underwater moving body 1 is set as a state transition equation for the three-dimensional coordinate space, and the obstacle 7 is approximated by a sphere with a center position of o ₁ and a size of radius D _Max. You should do it. Furthermore, the flow direction angle φ may be set in a three-dimensional coordinate space.

  In the present embodiment, the tidal flow direction angle φ is described as being obtained from tidal flow direction data stored in the storage unit 6. However, the difference between the ground speed of the underwater moving body 1 and the ground speed, and others Alternatively, the tidal current direction angle φ may be estimated using the proposed tidal current estimation method.

Modified Example of First Embodiment
FIG. 4 is a schematic view showing another configuration of the mobile control system as a modification of the first embodiment.

  In FIG. 4, the same components as those in the first embodiment are denoted by the same reference numerals, and the description thereof will be omitted.

  The mobile control system of the present modification, as shown in FIG. 4, has the same configuration as the mobile control system of the first embodiment except that the underwater mobile 1 is provided with the calculation unit 5 and the storage unit 6. The calculation unit 5 and the storage unit 6 are provided in the control station 10 outside the underwater moving body 1.

  The control station 10 is, for example, a surface station or a ground station provided for a ship or a waterborne mobile.

  Furthermore, the underwater moving body 1 and the control station 10 are configured to include a communication device 11 and a communication device 12 for performing mutual communication.

  The communication method between the communication device 11 and the communication device 12 may be acoustic communication by acoustic signals such as ultrasonic waves, or wire communication by optical signals through optical fibers or electric signals through electric wires. Of course, any other communication method may be adopted.

  Furthermore, when the control station 10 is a ground station, it is difficult to directly communicate with the underwater moving body 1 by acoustic communication or wire communication. In this case, communication between the control station 10 and the underwater moving body 1 May be relayed by a relay provided on a ship or a waterborne mobile.

  In the mobile control system of the present modification, the underwater mobile 1 transmits the detection result of the state quantity by the state quantity detection unit 2 and the information of the obstacle 7 detected by the obstacle detection unit 3 to the communication device 11. It is sent to the calculation unit 5 of the control station 10 via the communication device 12.

  When the calculation unit 5 of the control station 10 receives the detection result of the state quantity from the underwater moving body 1 and the information of the obstacle 7, the flow direction data on the current position of the underwater moving body 1 is acquired from the storage unit 6 The same processing as the processing of the calculation unit 5 in the first embodiment is executed.

Thereafter, the control station 10 communicates with the communication device 12 with the first component u ₁ of the optimal control amount matrix u of the mobile device 4 obtained as a result of the processing of the calculation unit 5 as the target value of the control amount of the mobile device 4. It is sent to the underwater mobile 1 through the aircraft 11.

  In the underwater moving body 1, the target value of the control amount of the moving device 4 received from the control station 10 is given to the moving device 4.

  Thereby, according to this modification as well, similar effects can be obtained using the same control system as the mobile unit control system of the first embodiment.

First Application Example of First Embodiment
In the first embodiment, the obstacle 7 has been described as being fixed in position.

  On the other hand, the obstacle 7 may be, for example, a moving obstacle 7 such as another underwater vehicle operating at the same time.

In this case, when calculating the distance D _i between the position of the underwater vehicle 1 at the i-th step and the center position o ₁ of the obstacle in the calculation unit 5, the center position of the obstacle 7 at the i-th step The value of the x coordinate of o ₁ and the value of the y coordinate may be set based on the equation of motion of the obstacle 7.

  For example, when the obstacle 7 is another underwater vehicle, the information on the current position of the other underwater vehicle is obtained, and the difference between the current position and the position one step earlier makes the obstacle Obtain information on the speed of another underwater vehicle that is 7. Then, by substituting the obtained position information and velocity information into the state transition equation similar to the above-mentioned equation (1), information of the position of the i-th step of another underwater mobile body to be the obstacle 7 is obtained Just do it.

  As another method, communication is performed with another underwater mobile that becomes the obstacle 7 to obtain information on the position, velocity, attitude, and angular velocity of the other underwater mobile, and from the information, the obstacle 7 is obtained. Information on the position of the i-th step of another underwater vehicle that is

In addition, when the equation of motion of the obstacle 7 is not clear, the obstacle 7 performs uniform linear motion based on the information on the position of the obstacle 7 detected in a plurality of steps in the past by the obstacle detection unit 3 Assuming that the motion equation is established, the coordinates of the center position o ₁ at the time of the i-th step of the obstacle 7 may be obtained based on the equation of motion.

  According to this application example, in addition to the same effects as in the first embodiment can be obtained, the underwater mobile body 1 having uncertainty in the control of the state quantity of the own machine under an environment where disturbances exist, The moving obstacle 7 can be avoided.

Second Application Example of First Embodiment
In the first embodiment, as the state transition equation of the underwater moving body 1, the equation (1) is described in which x _i which is the amount of state of the underwater moving body and u _i which is the control amount are variables. Further, the influence of the tidal current is considered by introducing the process noise N _f related to the tidal current into the equation (6).

On the other hand, the state transition equation of the underwater moving body 1 is, in addition to the underwater moving body 1, the flow direction angle φ of the tidal current existing at the current position of the underwater moving body 1 and the tidal current It may be set as a state transition equation expanded in consideration of the influence of the velocity v _f .

x _{i + 1} = f (x _i , u _i , v _f , φ) (1A)

In this case, when performing the same processing as that of the first embodiment, the calculation unit 5 calculates the equation (6) indicating the error dispersion matrix CovX _i (i = 0... N _p ) of the underwater mobile body 1 by N _f The processing may be performed as an equation without.

  According to this application example, the same effect as that of the first embodiment can be obtained.

Third Application Example of First Embodiment
In the first embodiment, as shown in FIG. 2A, the position p _i (i = 0... N _p ) of the underwater mobile object 1 in the i-th step is the uncertainty of the state quantity of the underwater mobile 1 and It is set to pass through the center of the ellipse of moving range 9 in consideration of process noise related to the current.

On the other hand, the position p _i (i = 0... N _p ) of the i-th step underwater moving body 1 may be set to pass through a position shifted from the center of the moving range 9.

In this case, the amount assumed that the underwater mobile is moved downstream of the tidal current T by the tidal direction angle φ and the tidal velocity v _f is set as the shift amount (Xs, Ys), and the i th step underwater The following correction may be made to the equation for obtaining the distance D _i between the position of the mobile unit ₁ and the center position o ₁ of the obstacle.

  According to this application example, in addition to the same effect as the first embodiment, the underwater moving body 1 may move in the downstream direction of the tidal current T under the environment where the tidal current T exists, The movement path for avoiding the obstacle 7 of the underwater moving body 1 can be set in consideration of the fact that the possibility of moving in the direction upstream of T is higher.

[Application to other mobiles]
In the first embodiment and its modification and application example, the control method of the moving body and the moving body control system of the present invention are applied to the underwater moving body moving in the environment where the tidal current exists.

  On the other hand, in the method of controlling the mobile unit and the mobile unit control system according to the present invention, if the mobile unit is subjected to a disturbance in which the controlled state quantity includes uncertainty and the direction of the acting force becomes constant. As in the above, the present invention may be applied to moving bodies other than the underwater moving body 1.

  The control method and mobile control system of a mobile according to the present invention may be applied to an autonomously controlled floating mobile on the water which receives a current or wind as a disturbance. In addition, a mobile body on water includes a ship that performs automatic piloting.

  In this case, in the same configuration as shown in the first embodiment and its modification and application example, the tidal flow direction angle φ may be replaced with the direction in which the disturbance due to the tidal current and the wind acts on the water moving body.

  Note that the state quantity detection unit provided in the waterborne vehicle may be configured to include the reception unit of the Global Navigation Satellite System (GNSS). In addition, the obstacle detection unit and the moving device provided in the water moving body may be appropriately selected as the obstacle detecting unit and the moving device which are conventionally adopted in the water moving body or are proposed to be adopted. Of course.

  As a result, the obstacle avoidance can be performed in the environment where there is a disturbance in which the direction of the acting force becomes constant with respect to the mobile object on the water having uncertainty in control of the state quantity of the own aircraft.

  The control method and mobile control system of a mobile according to the present invention may be applied to an autonomously controlled aircraft that receives wind as a disturbance. The aircraft may be either a manned aircraft that performs autopilot or an unmanned aircraft.

  In this case, in the same configuration as shown in the first embodiment and its modification and application example, the tidal flow direction angle φ may be replaced with the direction in which the disturbance by the wind acts on the aircraft.

  The state quantity detection unit provided in the aircraft may be configured to include the reception unit of the Global Navigation Satellite System (GNSS). Also, the obstacle detection unit and the moving device provided in the aircraft may be appropriately selected as appropriate from the obstacle detection unit and the moving device which are conventionally adopted in the aircraft or have been proposed to be adopted. .

  As a result, for an aircraft having uncertainty in control of the state quantity of its own aircraft, obstacle avoidance can be performed in an environment where there is a disturbance in which the direction of the acting force becomes constant.

  The control method of a mobile body and the mobile body control system of the present invention may be applied to a spacecraft which receives the radiation pressure of electromagnetic waves from the sun or the like as a disturbance.

  In this case, in the same configuration as shown in the first embodiment and its modification and application example, the tidal flow direction angle φ may be replaced with the direction in which the disturbance due to the radiation pressure of the electromagnetic wave acts on the spacecraft.

  Note that the state quantity detection unit, the obstacle detection unit, and the moving device provided in the spacecraft are conventionally adopted in the space machine, or the state quantity detection unit, the obstacle detection unit, and the movement that are proposed to be adopted. Of course, the device may be selected appropriately.

  As a result, for a spacecraft having uncertainty in control of its own state quantity, obstacle avoidance can be performed in an environment where there is a disturbance in which the direction of the acting force becomes constant.

  The control method and mobile control system of a mobile according to the present invention may be applied to an autonomously controlled crawler-type or wheel-type autonomous mobile vehicle traveling on a soft ground with a slope. The autonomous traveling vehicle may be either a manned autonomous traveling vehicle that performs automatic steering or an unmanned autonomous traveling vehicle.

  In this case, in the same configuration as shown in the first embodiment and its modification and application example, the tidal flow direction angle φ is set to a direction at which the downward inclination angle of the ground on which the autonomous traveling vehicle is located becomes maximum. You just have to replace it.

  In addition, the state quantity detection unit, the obstacle detection unit, and the moving device provided in the autonomous traveling vehicle are conventionally adopted for the autonomous traveling vehicle, or the state quantity detection unit and the obstacle detection unit proposed to be adopted. Of course, the moving device may be selected appropriately.

  As a result, slippage occurs between the soft ground and the crawler and wheels during traveling, and the phenomenon of sliding down according to the slope of the ground occurs, so an obstacle occurs in an autonomous vehicle having uncertainty in the control of its own state quantity. Object avoidance can be performed.

  The present invention is not limited to only the above embodiment and its modifications and applications.

  The control method of the moving body and the moving body control system according to the present invention is any moving body other than those described above as long as the moving body is subjected to a disturbance in which the controlled state quantity includes uncertainty and the direction of the acting force becomes constant. It may be applied to mobiles.

  For example, the present invention may be applied to a mobile body in which the controlled state quantity includes uncertainty due to the low detection accuracy in the state quantity detection unit provided in the mobile body.

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

1 underwater moving body (moving body), 2 state quantity detecting unit, 3 obstacle detecting unit, 4 moving device, 5 computing unit, 7 obstacle

Claims (3)

It is a control method of a mobile that controls a mobile including a state quantity detection unit, an obstacle detection unit, and a mobile device,
A process in which the state quantity detection unit detects a current state quantity of the mobile object;
The obstacle detection unit performs a process of detecting an obstacle;
Furthermore, the calculation unit
A process of calculating an obstacle avoidance constraint function as a condition that the moving object does not collide with the obstacle based on the information of the position of the obstacle detected by the obstacle detection unit;
A process of calculating a margin function based on the uncertainty of the state quantity of the moving body with respect to the state transition equation of the moving body, and the disturbance acting on the moving body;
A process of performing model predictive control with a function obtained by adding the margin function to the obstacle avoidance constraint function as a constraint to obtain an optimal control amount of the mobile device;
Performing a process of giving the determined optimum control amount to the moving device as a target value of the control amount of the moving device. The method according to claim 1, wherein the moving body is an underwater moving body, and a disturbance acting on the underwater moving body is a tidal current. A state quantity detecting unit, an obstacle detecting unit, and a moving device provided on the moving body;
And a calculation unit,
The calculation unit
A function of receiving information on a current state amount of the mobile detected by the state amount detection unit and an obstacle detected by the obstacle detection unit;
A function of calculating an obstacle avoidance constraint function as a condition that the moving object does not collide with the obstacle, based on the information of the position of the obstacle detected by the obstacle detection unit;
A function of calculating a margin function based on the uncertainty of the state quantity of the moving body with respect to the state transition equation of the moving body, and the disturbance acting on the moving body;
A function of performing model predictive control with a function obtained by adding the margin function to the obstacle avoidance constraint function as a constraint to obtain an optimal control amount of the mobile device;
A function of providing the determined optimum control amount as a target value of the control amount of the movement device to the movement device.

Images