Mobile object control device and mobile object control system

Abstract

To provide a moving body control device and a moving body control system capable of optimizing a control volume.SOLUTION: A moving body control device 10 controls a moving body by using a control volume, and includes: a calculation unit 13 which calculates the control volume so that a state quantity of the moving body becomes closer to a target state quantity being a target value of the state quantity, under a constraint condition to keep maintaining a state where a distance between the moving body and an obstruction is larger than an avoidance distance for avoiding the obstruction; and an output unit 14 for outputting the control volume to the moving body. The calculation unit 13 calculates the avoidance distance on the basis of a disturbance direction in which the disturbance is exerted to the moving body and a positional relation between the obstruction and the moving body.SELECTED DRAWING: Figure 2

Description

[Technical Field]

[0001] The present disclosure relates to a mobile object control device and a mobile object control system. [Background technology]

[0002] Autonomous-controlled mobile bodies, such as USVs (Unmanned Surface Vehicles) and ASVs (Autonomous Surface Vehicles), are known. Such mobile bodies are required to have the ability to avoid obstacles. For example, Patent Document 1 describes a mobile body control system that causes an autonomous-controlled mobile body to avoid obstacles. This mobile body control system calculates a control amount that minimizes an evaluation function of the control amount using a collision avoidance constraint function as a constraint condition, and controls the mobile body using the control amount. The collision avoidance constraint function specifies that the distance between the center position of an obstacle and the mobile body must be equal to or greater than the radius of a circle that approximates the size of the obstacle plus a margin corresponding to the uncertainty of the state quantity of the mobile body. [Prior art documents] [Patent documents]

[0003] [Patent Document 1] Japanese Patent Application Publication No. 2018-181166 Summary of the Invention [Problem to be solved by the invention]

[0004] The mobile object control system described in Patent Document 1 calculates a margin based on a disturbance acting on the mobile object. However, the mobile object control system includes a constant value based on the disturbance in the margin, regardless of the direction in which the disturbance acts on the mobile object. In general, the position of the mobile object may fluctuate in the direction in which the disturbance acts on the mobile object. Therefore, there is room for improvement in the method for calculating the control amount in the mobile object control system.

[0005] The present disclosure describes a mobile object control device and a mobile object control system that are capable of optimizing a control amount. [Means for solving the problem]

[0006] A mobile body control device according to one aspect of the present disclosure is a device for controlling a mobile body using a control variable. This mobile body control device includes: a calculation unit that calculates a control variable so that a state variable of the mobile body approaches a target state variable, which is a target value of the state variable, under a constraint that the distance between the mobile body and an obstacle is continuously maintained greater than an avoidance distance for avoiding the obstacle; and an output unit that outputs the control variable to the mobile body. The calculation unit calculates the avoidance distance based on the direction of the disturbance acting on the mobile body and the positional relationship between the obstacle and the mobile body. [Effects of the Invention]

[0007] The present disclosure can optimize the control amount. [Brief explanation of the drawings]

[0008] [Figure 1] FIG. 1 is a schematic configuration diagram of a mobile object control system according to an embodiment. [Figure 2] FIG. 2 is a functional block diagram of the mobile object control device shown in FIG. [Figure 3] FIG. 3 is a flowchart showing a series of processes of a mobile object control method performed by the mobile object control device shown in FIG. [Figure 4] FIG. 4 is a diagram for explaining the direction of a disturbance and the positional relationship between an obstacle and a moving body. [Figure 5] Fig. 5(a) is a diagram showing an example of a movement trajectory of a moving object controlled by the moving object control system shown in Fig. 1. Fig. 5(b) is a diagram showing an example of a movement trajectory of a moving object controlled by a moving object control system of a comparative example. DETAILED DESCRIPTION OF THE INVENTION

[0009] [1] Overview of the embodiment A mobile body control device according to one aspect of the present disclosure is a device for controlling a mobile body using a control variable. This mobile body control device includes: a calculation unit that calculates a control variable so that a state variable of the mobile body approaches a target state variable, which is a target value of the state variable, under a constraint that the distance between the mobile body and an obstacle is continuously maintained greater than an avoidance distance for avoiding the obstacle; and an output unit that outputs the control variable to the mobile body. The calculation unit calculates the avoidance distance based on the direction of the disturbance acting on the mobile body and the positional relationship between the obstacle and the mobile body.

[0010] In the above-described mobile body control device, an avoidance distance is calculated based on the direction of the disturbance acting on the mobile body and the positional relationship between the obstacle and the mobile body. When the disturbance acts on the mobile body, the mobile body receives a force that moves the mobile body in the disturbance direction. At this time, if the disturbance direction is a direction in which the mobile body moves away from the obstacle, the disturbance acting on the mobile body may cause the mobile body to move away from the obstacle. On the other hand, if the disturbance direction is a direction in which the mobile body moves toward the obstacle, the disturbance acting on the mobile body may cause the mobile body to approach the obstacle. Therefore, by taking into account the disturbance direction and the positional relationship between the obstacle and the mobile body, it is possible to appropriately calculate the avoidance distance. Since the control amount is calculated under constraints using the avoidance distance calculated in this manner, it is possible to optimize the control amount.

[0011] The avoidance distance may include a correction distance according to the uncertainty of the state quantity, and the calculation unit may make the correction distance smaller when the disturbance direction is a direction in which the moving body moves away from the obstacle than when the disturbance direction is a direction in which the moving body moves toward the obstacle. When the disturbance direction is a direction in which the moving body moves away from the obstacle, the disturbance acting on the moving body may cause the moving body to move away from the obstacle. Therefore, the possibility that the moving body will approach the obstacle due to the uncertainty of the state quantity of the moving body, including the influence of the disturbance, is reduced. On the other hand, when the disturbance direction is a direction in which the moving body moves toward the obstacle, the disturbance acting on the moving body may cause the moving body to approach the obstacle. Therefore, the possibility that the moving body will approach the obstacle due to the uncertainty of the state quantity of the moving body is increased. According to the above configuration, when the possibility that the moving body will approach the obstacle due to the uncertainty of the state quantity of the moving body is low, the correction distance according to the uncertainty of the state quantity is reduced, and when the possibility that the moving body will approach the obstacle due to the uncertainty of the state quantity of the moving body is high, the correction distance according to the uncertainty of the state quantity is increased. Therefore, the avoidance distance is calculated appropriately according to the possibility that the moving object will approach the obstacle, and as a result, the control amount can be optimized.

[0012] The calculation unit may set the correction distance to zero if the disturbance direction is a direction in which the moving body is moving away from the obstacle. If the disturbance direction is a direction in which the moving body is moving away from the obstacle, the disturbance acting on the moving body may cause the moving body to move away from the obstacle. Therefore, the possibility that the moving body will approach the obstacle due to uncertainty in the state quantity of the moving body is reduced. Therefore, the moving body can avoid the obstacle without considering the correction distance according to the uncertainty in the state quantity of the moving body. According to the above configuration, if the possibility that the moving body will approach the obstacle due to uncertainty in the state quantity of the moving body is low, the avoidance distance does not include a component (correction distance) according to the uncertainty in the state quantity of the moving body, so there is no need to ensure an unnecessarily large distance between the moving body and the obstacle. As a result, it is possible to optimize the control amount.

[0013] The correction distance may include a component of process noise related to the disturbance, and the calculation unit may set the component to zero if the disturbance direction is a direction in which the moving body is moving away from the obstacle. If the disturbance direction is a direction in which the moving body is moving away from the obstacle, the disturbance acting on the moving body may cause the moving body to move away from the obstacle. Therefore, the possibility that the moving body will approach the obstacle due to uncertainty in the state quantity of the moving body is reduced. Therefore, the moving body can avoid the obstacle without considering the process noise related to the disturbance. According to the above configuration, if the possibility that the moving body will approach the obstacle due to uncertainty in the state quantity of the moving body is low, the avoidance distance does not include a component of process noise related to the disturbance, so there is no need to ensure an unnecessarily large distance between the moving body and the obstacle. As a result, it is possible to optimize the control amount.

[0014] A mobile object control system according to another aspect of the present disclosure includes the above-described mobile object control device, a state quantity detection device that detects a state quantity, a measurement device that measures a disturbance, and an obstacle detection device that detects an obstacle. Since this mobile object control system includes the above-described mobile object control device, it is possible to optimize the control quantity.

[0015] [2] Example of embodiment Hereinafter, embodiments of the present disclosure will be described with reference to the drawings. In the description of the drawings, the same elements are designated by the same reference numerals, and duplicated description will be omitted.

[0016] A mobile object control system according to one embodiment will be described with reference to FIG. 1. FIG. 1 is a schematic diagram of a mobile object control system according to one embodiment. The mobile object control system 1 shown in FIG. 1 is a system that controls an autonomously controlled mobile object M. Examples of the mobile object M include an Autonomous Surface Vehicle (ASV), a Remotely Operated Vehicle (ROV), and an Autonomous Underwater Vehicle (AUV). In this embodiment, an ASV is used as the mobile object M. The mobile object M is controlled by a control amount output from the mobile object control system 1. The mobile object M includes actuators such as thrusters and steering actuators as mobile devices to be controlled. The control amount is a value used to control the mobile object M. Examples of the control amount include a rudder angle and an engine rotation speed.

[0017] The mobile object control system 1 includes a state quantity detection device 2, a measurement device 3, an obstacle detection device 4, and a mobile object control device 10.

[0018] The state quantity detection device 2 is a device that detects the state quantity of the moving body M. The state quantity of the moving body M is a value that indicates the state of the moving body M. Examples of the state quantity of the moving body M include position information, velocity, angular velocity, and attitude of the moving body M. Note that the position information may be expressed by a two-dimensional XY Cartesian coordinate system (see FIG. 4) defined along the water surface. The state quantity detection device 2 includes, for example, a GNSS (Global Navigation Satellite System) and a Doppler ultrasonic velocimeter.

[0019] The measurement device 3 is a device that measures disturbances and generates disturbance information. The disturbance information is environmental information that affects the movement of the moving body M. Examples of the disturbance information include information about wind and tides. The measurement device 3 includes, for example, a wind vane and an anemometer.

[0020] The obstacle detection device 4 is a device that detects the state quantity of an obstacle. The state quantity of an obstacle is a value that indicates the state of the obstacle. Examples of the state quantity of an obstacle include the position information, traveling direction, and speed of the obstacle. The obstacle detection device 4 is, for example, a marine radar or a LiDAR (Light Detection And Ranging).

[0021] The mobile object control device 10 is a device that uses a control amount to control the mobile object M. The mobile object control device 10 calculates the control amount based on the state amount of the mobile object M, disturbance information, and the state amount of an obstacle.

[0022] The mobile object control device 10 is configured as a computer including hardware such as a CPU (Central Processing Unit), RAM (Random Access Memory) and ROM (Read Only Memory) as main storage devices, a communication module for communicating with other devices, and an auxiliary storage device such as a hard disk. The operation of these components realizes each of the functional components of the mobile object control device 10 shown in FIG. 2.

[0023] Next, functional components of the mobile body control device 10 will be described with reference to Fig. 2. Fig. 2 is a functional block diagram of the mobile body control device shown in Fig. 1. As shown in Fig. 2, the mobile body control device 10 includes, as functional components, an acquisition unit 11, an estimation unit 12, a calculation unit 13, and an output unit 14.

[0024] The acquisition unit 11 is a functional component that acquires various information. For example, the acquisition unit 11 acquires a state quantity of the moving body M, disturbance information, a state quantity of an obstacle, and a target state quantity. The target state quantity is a target value of the state quantity.

[0025] The estimation unit 12 is a functional component that estimates the center position and size of an obstacle based on the state quantities of the obstacle.

[0026] The calculation unit 13 is a functional component that calculates a control amount for bringing the state quantity of the moving body M closer to the target state quantity. The calculation unit 13 calculates a control amount such that the state quantity of the moving body M approaches the target state quantity under the constraint that the distance between the moving body M and the obstacle continues to be maintained in a state greater than the avoidance distance. The avoidance distance is the distance for the moving body M to avoid the obstacle.

[0027] The output unit 14 is a functional component that outputs a control amount. The output unit 14 outputs the control amount to the moving object M.

[0028] Next, a mobile body control method performed by the mobile body control device 10 will be described with reference to Fig. 3 and Fig. 4. Fig. 3 is a flowchart showing a series of processes of the mobile body control method performed by the mobile body control device shown in Fig. 1. Fig. 4 is a diagram for explaining the direction of a disturbance and the positional relationship between an obstacle and a mobile body.

[0029] The series of processes of the mobile body control method shown in Fig. 3 is started, for example, when the user performs an operation to control the mobile body M, and is then repeatedly performed every time a predetermined time elapses. The operation to control the mobile body M is to input a target state quantity x d In addition, the state quantity of the moving object M is input by the user as the target state quantity x d The target state quantities at a plurality of relay points located on the route to the moving object M are calculated in order of proximity to the moving object M, and the target state quantities x d The route may be input as follows. The route is calculated using a route calculation algorithm. Examples of the route calculation algorithm include A-Star search and Dijkstra's algorithm. Here, a mobile object control method implemented at time k will be described. Time k is an integer equal to or greater than 0, and is a discrete value that increments every time the above-mentioned predetermined time elapses. In the following description, when a variable is given a subscript "time k," it means the value of the variable at time k.

[0030] The state transition equation of the moving object M shown in equation (1) is calculated in advance and stored in a memory (not shown). The state transition equation shown in equation (1) is expressed as the state quantity x k and the control variable u input to the moving object M at time k. k and the state x of the moving object M at time k+1 k+1 The relationship between the state quantity x k includes variables such as the position, attitude, velocity, and angular velocity of the moving object M. k The number of variables included in may be selected appropriately. For convenience of explanation, time k is used in equation (1), but time k in equation (1) may be any time and is not limited to the time at which the mobile object control method is performed.

number

[0031] First, the acquisition unit 11 acquires the state quantity x of the moving object M. k , disturbance information, state of obstacle O, and target state x d Specifically, the acquisition unit 11 acquires the target state quantity x input to the mobile object control device 10 by the user (step S1). d The acquisition unit 11 acquires the state quantity x of the moving object M. k from the state quantity detection device 2. The acquisition unit 11 acquires disturbance information from the measurement device 3. The acquisition unit 11 acquires the velocity v of the disturbance W as the disturbance information. w , and the disturbance direction φ. As shown in FIG. 4, the disturbance direction φ is the direction of the disturbance, and in this embodiment, it is expressed as an angle with respect to north (positive direction of the X-axis). w is the velocity of the disturbance W in the disturbance direction φ. The acquisition unit 11 acquires the state quantity of the obstacle O from the obstacle detection device 4. Then, the acquisition unit 11 acquires the target state quantity x d , state quantity x k , disturbance information, and the state quantity of the obstacle O to the calculation unit 13, and also outputs the state quantity of the obstacle O to the estimation unit 12.

[0032] Next, the estimation unit 12 estimates the center position of the obstacle O and the size of the obstacle O (step S2). Specifically, when the estimation unit 12 receives the state quantity of the obstacle O from the acquisition unit 11, the estimation unit 12 estimates the obstacle O at time k based on the state quantity of the obstacle O. k and radius D Max Here, the estimation unit 12 approximates the obstacle O with a circle or sphere that can encompass the actual obstacle O. That is, the obstacle O is approximated with a circle or sphere of radius D Max is the minimum separation distance when the moving object M is at the center position o k from radius D Max This distance or more ensures that the moving body M will not come into contact with or collide with the obstacle O. Note that since the approximation method using a circle or a sphere is well known, a detailed description thereof will be omitted here.

[0033] When a marine radar is used as the obstacle detection device 4, the estimation unit 12 estimates the center position o k Alternatively, the estimation unit 12 may directly acquire the center position o of the obstacle O from the obstacle detection device 4. When a LiDAR is used as the obstacle detection device 4, the estimation unit 12 acquires point cloud information about the obstacle O from the obstacle detection device 4 and processes the point cloud information to estimate the center position o of the obstacle O. k Then, the estimation unit 12 calculates the center position o k , and radius D Max is output to the calculation unit 13.

[0034] Next, the calculation unit 13 calculates the control amount u k+1 In step S3, the calculation unit 13 first calculates the target state quantity x d , state quantity x k , disturbance information, and the state quantity of the obstacle O are received from the acquisition unit 11, and the center position o k , and radius D Max from the estimation unit 12. Then, the calculation unit 13 calculates the optimal controlled variable matrix u using, for example, model predictive control. As shown in equation (2), the controlled variable matrix u is c N up to c The control amount (control amount u k+1 ~Control amount u k+Nc) is a matrix of N c is the control horizon. The control horizon is the number of steps (positive integer) of the control variable matrix u that are considered in calculating the optimal control variable matrix u.

number

[0035] For example, the calculation unit 13 calculates the optimal control amount matrix u by solving the optimization problem shown in Equation (3). The optimization problem of Equation (3) is solved by solving the obstacle avoidance constraint function h k+i It specifies that the control variable matrix u that minimizes the evaluation function J(u) is to be found under the constraint that (u) is equal to or less than 0. i is 1 to N p is the integer value of N p is the forecast horizon. The forecast horizon is the number of steps (positive integer) of the state variables to be considered in calculating the optimal control variable matrix u. p is N c may be larger than N c It can be the same as:

number

[0036] The evaluation function J(u) is a function used to evaluate the controlled variable matrix u. As shown in equation (4), the evaluation function J(u) is calculated by multiplying the target state quantity x d and the state x at each time k+i The sum of the differences between the control variable u k+i The sum of the magnitudes of the target state x d and time k+N p State quantity x at +1 k+Np+i This function calculates the sum (evaluation value) of the values ​​obtained by multiplying the difference term between the two by the weighting matrices Q, R, and S. Therefore, the smaller the evaluation value, the more easily the state variable can be adjusted to the target state variable x while reducing the magnitude of the controlled variable. dAlthough a quadratic evaluation function is used as the evaluation function J(u) in equation (4), a general evaluation function used in model predictive control may also be used.

number

[0037] The calculation unit 13 adds the state quantity x k and the control variable u contained in the control variable matrix u k By inputting k+1 By repeating the same process, the state quantity x k+2 to the state quantity x k+Np+1 are calculated in order.

[0038] Obstacle avoidance constraint function h k+i (u) is a function for specifying the constraints for the moving body M to avoid the obstacle O. The obstacle avoidance constraint function h k+i (u) is the distance D from the avoidance distance k+i The avoidance distance is calculated by subtracting the corrected distance σ'. k+i Correction distance σ' k+i is a value calculated according to the uncertainty of the state quantity of the moving object M. Here, the avoidance distance is the radius D of the obstacle O. Max Correction distance σ' k+i The distance D is calculated by adding k+i is the distance between the moving object M and the obstacle O (center position o k+i ) is the distance between the radius D Max is the radius of the circle or sphere approximated to encompass the actual obstacle O, so the obstacle avoidance constraint function h k+i If the distance calculated by (u) is less than or equal to 0, it means that the moving object M does not come into contact with or collide with an obstacle.

number

[0039] The calculation unit 13 calculates the distance D using equation (6). k+i Calculate the distance D k+i is the position coordinate of the moving object M at time k+i (X k+i ,Y k+i ) and the center position o of obstacle O k+i coordinates (X ok+i ,Y ok+i ) is the distance between

number

[0040] Here, the calculation unit 13 calculates the avoidance distance based on the disturbance direction φ and the positional relationship between the obstacle O and the moving body M. Specifically, the calculation unit 13 calculates the corrected distance σ' based on the disturbance direction φ and the positional relationship between the obstacle O and the moving body M. k+i More specifically, as shown in equation (7), the calculation unit 13 calculates the direction θ k+i and the disturbance direction φ, the correction distance σ' k+i As shown in Figure 4, the direction θ k+i is the center position o of obstacle O k+i The direction is from the north to the position of the moving object M, and in this embodiment, it is expressed as an angle with north (positive direction of the X axis) as the reference.

number

[0041] As shown in equation (7), the calculation unit 13 calculates the position coordinates of the moving object M and the center position o k+i Based on the coordinates of the direction θ k+i Calculate the direction θ k+i and the angle difference between the disturbance direction φ (direction θ k+i and the disturbance direction φ) is smaller than 90 degrees (acute angle), the correction distance σ' k+i On the other hand, the calculation unit 13 sets the direction θ k+i and the angle difference between the disturbance direction φ (direction θ k+i and the disturbance direction φ) is 90 degrees or more, the correction distance σ' k+i the margin σk+i Set to.

[0042] where the margin σ k+i As shown in equation (8), the calculation unit 13 calculates the matrix B k+i and the error variance matrix CovX of the mobile unit M k+i and the margin σ k+i Calculate.

number

[0043] matrix B k+i is the collision avoidance constraint function g k+i As shown in equation (9), the calculation unit 13 calculates the collision avoidance constraint function g k+i By partially differentiating with respect to the state variables, we obtain the matrix B k+i Calculate.

number

[0044] Collision avoidance constraint function g k+i is a function for defining a constraint condition for the moving body M to avoid a collision with the obstacle O, assuming that no error occurs in the state quantity of the moving body M. In this embodiment, as shown in equation (10), the collision avoidance constraint function g k+i is the radius D Max Distance D from k+i is a function that specifies the subtraction of

number

[0045] error variance matrix CovX k+i represents the uncertainty of the position of the moving object M. The calculation unit 13 calculates the error variance matrix CovX using equation (11). k+i As shown in equation (11), the error variance matrix CovX at time k+i+1 is calculated. k+i+1 is matrix A k+i and the error variance matrix CovX at time k+ik+i and the process noise N proc and the process noise N f , the rotation matrix R(φ), and the step time dτ. The step time dτ is the step time of the model predictive control, and is the time required for the time k to increase by 1.

number

[0046] matrix A k+i is a partial differential matrix of the state transition equation of the moving object M shown in equation (1). As shown in equation (12), the calculation unit 13 partially differentiates the state transition equation shown in equation (1) with respect to the state quantity to obtain the matrix A k+i Calculate.

number

[0047] Process noise N proc is the process noise of the moving object M. The process noise N proc The same value as the process noise set in a general state estimation method such as a Kalman filter may be used as the noise factor.

[0048] Process noise N f is the process noise related to the disturbance W. The process noise N f is expressed by, for example, equation (13). The position change amount Δ(v w ) is the velocity v w is the amount of movement of the moving object M due to a disturbance W having the following characteristics. The parameter α indicates the ratio between the major axis and the minor axis of the error ellipse. The parameter α can take a value greater than 0 and less than 1. The smaller the parameter α, the longer the major axis of the error ellipse becomes relative to the minor axis.

number

[0049] The calculation unit 13 calculates the velocity v of the disturbance W.w The position change amount Δ(v w ) to convert the position change amount Δ(v w ) is calculated. This conversion function may be calculated in advance and stored in a memory (not shown). For example, if the disturbance W is wind, a conversion function with sufficient estimation accuracy can be obtained by regression analysis. w ) accurately, that is, when a conversion function with sufficient estimation accuracy cannot be obtained, the calculation unit 13 calculates the position change amount Δ(v w ) may be set to a large value.

[0050] The rotation matrix R(φ) is the process noise N f In other words, the rotation matrix R(φ) is a matrix for extending the process noise N f The calculation unit 13 calculates the rotation matrix R(φ) using equation (14). The rotation matrix R(φ) is used to rotate the major axis of the error ellipse caused by the process noise N f is converted into process noise taking into account the disturbance direction φ.

number

[0051] Using the above formulas (8) to (14), the calculation unit 13 calculates the margin σ k+i Calculate.

[0052] In addition, a general method such as SQP (Sequential Quadratic Programming) may be used to solve the optimization problem. The calculation unit 13 calculates the control amount u k+1 is output to the output unit 14.

[0053] Next, the output unit 14 receives the control amount u from the calculation unit 13. k+1 When receiving the control variable u k+1to the moving object M (step S4). Specifically, the output unit 14 outputs the control amount u k+1 Then, the moving device of the moving body M outputs the control amount u k+1 When receiving the control variable u k+1 It operates according to the

[0054] This completes the series of processes in the mobile object control method.

[0055] Next, the effects of the mobile object control system 1 and the mobile object control device 10 will be described with reference to (a) and (b) of Figure 5. (a) of Figure 5 is a diagram showing an example of the movement trajectory of a mobile object controlled by the mobile object control system shown in Figure 1. (b) of Figure 5 is a diagram showing an example of the movement trajectory of a mobile object controlled by a mobile object control system of a comparative example. In (a) and (b) of Figure 5, a mobile object M is navigating toward a destination Pd.

[0056] In the comparative example of the mobile body control system, the calculation unit of the mobile body control device calculates the direction θ k+i Regardless of the angle difference between the direction of the disturbance and the direction of the k+i the margin σ k+i Therefore, even when the angle difference is an acute angle, the correction distance σ' k+i is the margin σ k+i Therefore, as shown in FIG. 5(b), an error ellipse is set with the position of the moving object M at each time calculated by the state transition equation as its center. This error ellipse has a major axis along the disturbance direction φ and a radius D Max margin σ k+i That is, even though the disturbance W acts on the moving object M in a direction in which the moving object M moves away from the obstacle O, the moving object M moves away from the obstacle O. Max margin σ k+i The distance from the obstacle O is kept at least as long as the distance obtained by adding the above.

[0057] On the other hand, in the mobile object control system 1, the direction θ k+i and the disturbance direction φ is an acute angle, the correction distance σ′ k+i is set to zero. Therefore, as shown in FIG. 5(a), an error ellipse is set with its center at the position of the moving object M at each time calculated by the state transition equation. This error ellipse has a major axis along the disturbance direction φ and a radius D Max In other words, since the disturbance W acts on the moving object M in the direction in which the moving object M moves away from the obstacle O, the moving object M has a radius D Max As described above, a distance is maintained between the obstacle O and the mobile object M. As a result, the mobile object M controlled by the mobile object control system 1 navigates toward the destination Pd without maintaining an unnecessary distance between the obstacle O and the mobile object M, as compared to the mobile object M controlled by the mobile object control system of the comparative example.

[0058] As described above, in the mobile body control system 1 and the mobile body control device 10, the avoidance distance is calculated based on the disturbance direction φ in which the disturbance W acts on the mobile body M and the positional relationship between the obstacle O and the mobile body M. When the disturbance W acts on the mobile body M, the mobile body M receives a force that moves the mobile body M in the disturbance direction φ. At this time, if the disturbance direction φ is a direction in which the mobile body M moves away from the obstacle O, the disturbance W acting on the mobile body M may cause the mobile body M to move away from the obstacle O. On the other hand, if the disturbance direction φ is a direction in which the mobile body M approaches the obstacle O, the disturbance W acting on the mobile body M may cause the mobile body M to approach the obstacle O. Therefore, by taking into account the disturbance direction φ and the positional relationship between the obstacle O and the mobile body M, the avoidance distance can be appropriately calculated. Since the optimal control variable matrix u is calculated under the constraint conditions using the avoidance distance calculated in this manner, the control variable u k+1 It is possible to optimize the

[0059] When the disturbance direction φ is a direction in which the moving body M moves away from the obstacle O, the disturbance W acts on the moving body M, which may cause the moving body M to move away from the obstacle O. Therefore, the possibility that the moving body M will approach the obstacle O due to uncertainty in the state quantity of the moving body M including the influence of the disturbance W decreases. On the other hand, when the disturbance direction φ is a direction in which the moving body M moves towards the obstacle O, the disturbance W acts on the moving body M, which may cause the moving body M to approach the obstacle O. Therefore, the possibility that the moving body M will approach the obstacle O due to uncertainty in the state quantity of the moving body M increases. In the above embodiment, the calculation unit 13 calculates the corrected distance σ' when the disturbance direction φ is a direction in which the moving body M moves away from the obstacle O. k+i The corrected distance σ' when the disturbance direction φ is the direction in which the moving object M approaches the obstacle O k+i According to this configuration, when there is a small possibility that the moving object M will approach the obstacle O due to the uncertainty of the state quantity of the moving object M, the correction distance σ' according to the uncertainty of the state quantity is set to a value smaller than σ'. k+i becomes small and there is a high possibility that the moving object M will approach the obstacle due to the uncertainty of the state quantity of the moving object M, the correction distance σ' according to the uncertainty of the state quantity k+i Therefore, the avoidance distance is calculated appropriately according to the possibility that the moving object M will approach the obstacle O. As a result, the control amount u k+1 It is possible to optimize the

[0060] More specifically, when the disturbance direction φ is a direction in which the moving body M approaches the obstacle O, the calculation unit 13 calculates the corrected distance σ′ k+i the margin σ k+i When the disturbance direction φ is the direction in which the moving body M moves away from the obstacle O, the correction distance σ' k+i is set to zero. As described above, when the disturbance direction φ is a direction in which the moving object M moves away from the obstacle O, the possibility that the moving object M will approach the obstacle O due to the uncertainty of the state quantity of the moving object M decreases. Therefore, the corrected distance σ' k+iAccording to the above configuration, when the possibility that the moving object M will approach the obstacle O due to the uncertainty of the state quantity of the moving object M is low, the avoidance distance is calculated by subtracting the component corresponding to the uncertainty of the state quantity of the moving object M (corrected distance σ' k+i ), there is no need to ensure a greater distance than necessary between the moving body M and the obstacle O. As a result, the control variable u k+1 In other words, when the moving object M is moving in a direction away from the obstacle O, the avoidance distance becomes small, and the constraint condition is relaxed. Therefore, the number of options for the control amount matrix can be increased. Therefore, the calculation unit 13 can determine the control amount matrix u that further reduces the evaluation value of the evaluation function J(u) as the optimal control amount matrix u. As a result, the control amount u k+1 It is possible to optimize the

[0061] Although the embodiments of the present disclosure have been described above, the present disclosure is not limited to the above embodiments.

[0062] In the above embodiment, the direction θ k+i and the disturbance direction φ is an acute angle, the calculation unit 13 calculates the correction distance σ′ k+i is set to zero, but the margin σ k+i In this case, if there is a small possibility that the moving object M will approach the obstacle O due to the uncertainty of the state quantity of the moving object M, the correction distance σ' according to the uncertainty of the state quantity may be set to a smaller positive value. k+i becomes small and there is a high possibility that the moving object M will approach the obstacle due to the uncertainty of the state quantity of the moving object M, the correction distance σ' according to the uncertainty of the state quantity k+i Therefore, the avoidance distance is calculated appropriately according to the possibility that the moving object M will approach the obstacle O. As a result, the control amount u k+1 It is possible to optimize the

[0063] In the above embodiment, the direction θ k+i and the disturbance direction φ is an acute angle, the calculation unit 13 calculates the correction distance σ′ k+i is set to zero, but the process noise Nf In other words, when the disturbance direction φ is a direction in which the moving object M moves away from the obstacle O, the calculation unit 13 may set the process noise N f may be set to zero. k+i is the process noise N f Since the disturbance direction φ includes the component, the correction distance σ' when the moving body M is moving away from the obstacle O is k+i The corrected distance σ' when the disturbance direction φ is the direction in which the moving object M approaches the obstacle O k+i As described above, when the disturbance direction φ is a direction in which the moving object M moves away from the obstacle O, the possibility that the moving object M will approach the obstacle O due to the uncertainty of the state quantity of the moving object M decreases. Therefore, the process noise N f According to the above configuration, when the possibility that the moving object M will approach the obstacle O due to the uncertainty of the state quantity of the moving object M is low, the avoidance distance is determined by taking into account the process noise N f Therefore, it is not necessary to secure a larger distance than necessary between the moving body M and the obstacle O. As a result, the control variable u k+1 It is possible to optimize the

[0064] When the obstacle O is moving, the obstacle detection device 4 may detect the speed of the obstacle O as one of the state quantities of the obstacle O. The estimation unit 12 estimates the center position O at each time. k+1 may be calculated using a constant velocity motion model.

[0065] The obstacle detection device 4 detects an obstacle O at a center position o k , and radius D Max In this case, the mobile object control device 10 does not need to be equipped with the estimation unit 12.

[0066] The moving body M may be a surface vehicle (ASV). In this case, when the surface vehicle navigates on the water surface or sea surface where the wind blows from a fixed direction, the moving body control system 1 can calculate appropriate control variables. As a result, the navigation accuracy of the surface vehicle is improved.

[0067] The mobile object M may be a wheeled mobile robot equipped with crawler-type wheels. In this case, when the wheeled mobile robot travels on a soft slope, the mobile object control system 1 can calculate an appropriate control amount taking into consideration that a small landslide on the soft slope may cause the wheeled mobile robot to slide a certain distance in a certain time. As a result, the travel accuracy of the wheeled mobile robot is improved.

[0068] The moving body M may be an underwater mobile robot (AUV). In this case, when the underwater mobile robot navigates in an environment where ocean currents or tidal currents exist, the moving body control system 1 can calculate appropriate control variables. As a result, the navigation accuracy of the underwater mobile robot is improved.

[0069] The moving object M may be a drone. In this case, when the drone flies in an environment where the wind blows from a fixed direction, the moving object control system 1 can calculate an appropriate control amount. As a result, the accuracy of the flight of the drone is improved. [Explanation of symbols]

[0070] 1. Mobile control system 2. State quantity detection device 3. Measuring equipment 4 Obstacle detection device 10 Mobile control device 13 Calculation section 14 Output section M Mobile object O Obstacles W disturbance φ Disturbance direction

Claims

1. A mobile body control device that controls a mobile body using a control amount, a calculation unit that calculates a control amount such that a state quantity of the moving body approaches a target state quantity that is a target value of the state quantity, under a constraint that a distance between the moving body and an obstacle is continuously maintained greater than an avoidance distance for avoiding the obstacle; an output unit that outputs the control amount to the moving body; Equipped with the avoidance distance includes a correction distance according to uncertainty of the state quantity, The calculation unit calculates the avoidance distance by making the correction distance smaller when the disturbance direction acting on the moving body is a direction in which the moving body moves away from the obstacle than the correction distance when the disturbance direction is a direction in which the moving body moves toward the obstacle.

2. The mobile body control device according to claim 1 , wherein the calculation unit sets the correction distance to zero when the disturbance direction is a direction in which the mobile body moves away from the obstacle.

3. the corrected distance includes a process noise component related to the disturbance; The mobile body control device according to claim 1 , wherein the calculation unit sets the component to zero when the disturbance direction is a direction in which the mobile body moves away from the obstacle.

4. A mobile object control device according to any one of claims 1 to 3, a state quantity detection device for detecting the state quantity; a measuring device that measures the disturbance; an obstacle detection device that detects the obstacle; A mobile object control system comprising:

Images