CN112513747B

CN112513747B - Control device

Info

Publication number: CN112513747B
Application number: CN201980051701.9A
Authority: CN
Inventors: 恵木守
Original assignee: Omron Corp
Current assignee: Omron Corp
Priority date: 2018-09-27
Filing date: 2019-09-27
Publication date: 2022-05-24
Anticipated expiration: 2039-09-27
Also published as: JP2020052810A; EP3859456A1; JP6992719B2; WO2020067460A1; CN112513747A; EP3859456A4; US11467547B2; US20210302924A1

Abstract

A control device for performing model predictive control, wherein the control device sets a position of a virtual obstacle based on a position of an actual obstacle acquired by a first acquisition unit such that the virtual obstacle related to the actual obstacle is positioned at a position substantially symmetrical to the actual obstacle with reference to a target trajectory for control by a predetermined target command, and wherein a stage cost calculated by a predetermined evaluation function in the model predictive control includes: a first stage cost associated with a first probability potential field representing a probability that an actual obstacle may exist based on a location of the actual obstacle; and a second stage cost associated with a second probability potential field that is a probability potential field representing a probability that a virtual obstacle may exist based on the position of the virtual obstacle and has a probability value that is greater than or equal to the first probability potential field. When the output of the controlled object is made to follow the target command by the model predictive control, collision with the moving obstacle is avoided.

Description

Control device

Technical Field

The present invention relates to a control device for causing a control target to follow a predetermined target command.

Background

In order to move the control target following the command trajectory, feedback control is generally used. For example, in an articulated robot, a control device of the robot controls a servo motor of each joint axis so that a position of a finger tip of the robot follows a preset (taught) command trajectory by using feedback control. However, in general feedback control, since each servo motor causes a response delay in any way, there is a problem that the actual trajectory of the robot deviates from the commanded trajectory. In order to suppress such a deviation from the command trajectory, a technique related to model predictive control is used (for example, see non-patent document 1).

Further, if there is an obstacle that may interfere with the control target when the control target follows the command trajectory and moves, the control target must be controlled so as to avoid a collision with the obstacle. For example, in the technique shown in patent document 1, a probability potential field (potential field) is formed based on the existence probability of an obstacle existing around the control object, and a path along which the control object should travel is determined based on the gradient of the probability potential field. Similarly, in the collision avoidance control of the vehicle shown in patent document 2, a probabilistic potential field based on the existence probability of an obstacle is also used.

Documents of the prior art

Patent document

Patent document 1: japanese patent laid-open No. 2003-241836

Patent document 2: japanese patent laid-open publication No. 2011-186878

Non-patent document

Non-patent document 1: yu Tai and Ohtsuka (Yuta Sakurai and Toshiyuki Ohtsuka): offset Compensation of Continuous-Time Model Predictive Control By interference Estimation (Offset Compensation of Continuous Time Model Predictive Control By Disturbance Estimation); journal of society of System control information, vol.25, No.7, pp.10-18(2012)

Disclosure of Invention

Problems to be solved by the invention

In order to make it possible to make the output of the control target follow the target command in a satisfactory manner by the model predictive control and to avoid a collision with a moving obstacle existing around the control target in a satisfactory manner, it is preferable to use a probabilistic potential field formed based on the existence probability of the obstacle as in the conventional art. However, according to the path generated for collision avoidance by the conventional technique, the control target tends to avoid the moving obstacle ahead of the control target. In this case, since the controlled object and the moving obstacle are eventually in parallel, the possibility of collision with the obstacle continues for a long time, and the time required for avoidance becomes long. In particular, in the case of a control target such as a robot whose movable range is limited due to a limited length of a structure such as an arm, if the possibility of collision is delayed, the control target may fall into a state in which collision with an obstacle cannot be avoided.

The present invention has been made in view of the above problems, and an object of the present invention is to provide a technique capable of preferably avoiding a collision with a moving obstacle when an output of a control target follows a target command by model predictive control.

Means for solving the problems

In order to solve the above problem, the present invention includes a stage cost (stage cost) calculated in model predictive control for follow-up control on a target command: phase costs associated with the probabilistic potential field of an actual obstacle (actual obstacle); and a phase cost associated with a probabilistic potential field of a virtual obstacle corresponding to the actual obstacle. Thus, the following of the target command and the avoidance of a better obstacle are realized.

The present invention as understood from the first aspect is a control device including a model predictive control unit that has a predictive model that defines a correlation between a predetermined state variable relating to a control target and a control input to the control target in the form of a predetermined state equation, and that causes the output of the control target to follow a predetermined target command, wherein the model predictive control unit performs model predictive control based on the predictive model in a predetermined time-width prediction section for the predetermined target command to which the output of the control target is to follow according to a predetermined evaluation function, and outputs at least a value of the control input at an initial time of the prediction section. Further, the control device includes: a first acquisition unit that acquires a position of an actual obstacle, which is an actual obstacle, with respect to the control target; and a setting unit that sets a position of a virtual obstacle based on the position of the actual obstacle acquired by the first acquisition unit, and that causes the virtual obstacle associated with the actual obstacle to be positioned at a position substantially symmetrical to the actual obstacle, based on a tracking target trajectory of the control target based on the predetermined target command. The phase cost calculated by the predetermined evaluation function includes: a state quantity cost, which is a phase cost associated with the specified state variable; a control input cost that is a phase cost associated with the control input; a first stage cost associated with a first probability potential field representing a probability that the actual obstacle may exist based on a location of the actual obstacle; and a second stage cost associated with a second probabilistic potential field representing a probability that the virtual obstacle may exist based on a position of the virtual obstacle and having a probability value above the first probabilistic potential field.

The control device of the present invention is a control device that causes an output of a control target to follow a predetermined target command, and is configured such that the model predictive control unit generates a control input for the control target. Then, a prediction model provided in the model prediction control unit is formed based on the control target. In the model predictive control, a prediction section having a predetermined time width is set at each control time, and an arithmetic process is performed in the prediction section according to a predetermined evaluation function, so that the calculated control input value at least at the initial time in the prediction section is generated in real time and output. In the model predictive control, a prediction section moves with the passage of control time, and a so-called rolling Horizon (Receding Horizon) control is executed. Then, the correlation between the predetermined state variable associated with the control target and the control input is reflected on the prediction model. With this configuration, it is possible to generate a control input for following a predetermined target command in real time and output the control input to a control target.

Here, when the control target is made to follow the target command, if there is an obstacle moving in the vicinity thereof, it is necessary to achieve the following of the target command while avoiding collision with the obstacle. Therefore, the control device acquires the position of the actual obstacle, which is the actual obstacle, by the first acquisition unit, and sets the position of the virtual obstacle corresponding to the actual obstacle by the setting unit. The virtual obstacle is not an actually existing obstacle, but a virtual obstacle set in correspondence with an actual obstacle for calculation of model predictive control following a target command. The position of the virtual obstacle is set to a position substantially symmetrical to the actual obstacle with respect to the tracking target trajectory of the control target. This is to allow the probability potential field starting from the actual obstacle and the probability potential field starting from the virtual obstacle to be arranged symmetrically with respect to the tracking target trajectory. In addition, this does not mean that the two potential fields must be formed to be identical.

After the positions of the actual obstacle and the virtual obstacle are grasped in this manner, the stage cost calculated by the predetermined evaluation function for model predictive control is made to include a first stage cost corresponding to the actual obstacle and a second stage cost corresponding to the virtual obstacle in addition to the state quantity cost and the control input cost. Therefore, in the model predictive control for following the target command, the first probability potential field relating to the actual obstacle and the second probability potential field relating to the virtual obstacle set to be substantially symmetrical with respect to the target trajectory can be made to act. As a result, the repulsive force generated by the first potential field of the actual obstacle (action for moving the control target away) and the repulsive force generated by the second potential field of the virtual obstacle act on the control target, so that the following speed of the control target with respect to the target command is preferably reduced, and the control target can move away from the actual obstacle so as to be positioned behind the actual obstacle (in the direction opposite to the traveling direction). In this way, it is possible to suppress the situation where the control target and the obstacle run in parallel as in the conventional art, and to avoid a collision with the actual obstacle.

In particular, in the control device, the two potential fields are set such that the second potential field has a probability value equal to or greater than the first potential field. This means that the repulsive force formed by the second potential field of the virtual obstacle is higher than the repulsive force formed by the first potential field of the actual obstacle. Therefore, by the model predictive control for calculating the stage cost as described above, the following speed of the control target to the target command is preferably reduced, and avoidance of the actual obstacle is preferably achieved.

Here, it is understood from the second aspect of the present invention that the problems are solved. The present invention as understood from the second aspect is a control device including a model predictive control unit that has a predictive model that defines a correlation between a predetermined state variable relating to a control target and a control input to the control target in the form of a predetermined state equation, and that causes the output of the control target to follow a predetermined target command, and that performs model predictive control based on the predictive model in a predetermined time-width prediction section according to a predetermined evaluation function with respect to the predetermined target command to which the output of the control target is to follow, and that outputs at least a value of the control input at an initial time of the prediction section. Further, the control device includes: a servo integrator for inputting a deviation between the predetermined target command and an output of the control target; a first acquisition unit that acquires a position of an actual obstacle, which is an actual obstacle, with respect to the control target; and a setting unit that sets a position of a virtual obstacle based on the position of the actual obstacle acquired by the first acquisition unit, and that causes the virtual obstacle associated with the actual obstacle to be positioned at a position substantially symmetrical to the actual obstacle, based on a tracking target trajectory of the control target based on the predetermined target command. The phase cost calculated by the predetermined evaluation function includes: a state quantity cost, which is a phase cost associated with the specified state variable; a control input cost that is a phase cost associated with the control input; a first stage cost associated with a first probability potential field representing a probability that the actual obstacle may exist based on a location of the actual obstacle; and a second stage cost associated with a second probabilistic potential field representing a probability that the virtual obstacle may exist based on a position of the virtual obstacle and having a probability value above the first probabilistic potential field. Further, the state variable associated with the control target includes a predetermined integral term expressed by a product of the deviation and a predetermined integral gain, and the predetermined integral gain is smaller as the distance between the control target and the actual obstacle is shorter.

The control apparatus of the second aspect is different from the control apparatus of the first aspect in that it further includes a servo integrator. With this configuration, the model prediction control based on the deviation is performed. This makes it possible to effectively eliminate steady-state variations without unnecessarily deteriorating the follow-transition response to a predetermined target command. In addition, since the control device eliminates the steady-state deviation by including a predetermined integral term in the prediction model, the load required for designing the control system can be greatly reduced, and the control device can realize the preferable follow-up control of the controlled object. In the case of using an observer (observer) or the like that estimates disturbance that is a factor of steady-state deviation as in the conventional art, the parameter design is difficult and the calculation load is relatively large, and therefore the configuration of the present invention is also useful from such a viewpoint.

However, even if the repulsive force generated by the first potential field of the actual obstacle and the repulsive force generated by the second potential field of the virtual obstacle are caused to act on the control target in the model predictive control by including such a predetermined integral term in the state variables, the influence of the predetermined integral term is relatively strong if the deviation between the position of the control target and the target position becomes large as time passes or as the target command passes. Therefore, the effect of the repulsive force by the second potential field of the virtual obstacle is relatively weak, and as a result, it is difficult to control the control target so as to avoid behind the actual obstacle. Therefore, in the control device according to the second aspect, the shorter the distance between the control target and the actual obstacle, the smaller the predetermined integral gain included in the predetermined integral term. With this, when the possibility of collision of the control target with the actual obstacle is increased, the effect of the repulsive force generated by the potential fields of the actual obstacle and the virtual obstacle can be relatively increased, and the collision with the actual obstacle can be avoided while the target command is followed.

In the control device described above, there is a tendency that the closer the entry angle of the actual obstacle to the tracking target trajectory is to 90 degrees, the weaker the action of the repulsive force formed by the first potential field of the actual obstacle and the repulsive force formed by the second potential field of the virtual obstacle on the control target in the model prediction control is. As a result, even when the control target is relatively close to the actual obstacle, the following speed cannot be reduced appropriately, and it is difficult to avoid collision between the control target and the actual obstacle. Therefore, the control device may further include a second acquisition unit that acquires an entry angle of the actual obstacle with respect to the tracking target trajectory, which is defined as an angle between a moving direction of the control target and a moving direction of the actual obstacle, and in this case, the second stage cost may be calculated to be larger as the entry angle approaches 90 degrees in the stage cost calculated by the predetermined evaluation function. By calculating the second-stage cost depending on the entrance angle in this manner, it is possible to avoid collision between the actual obstacle and the control target without being affected by the entrance angle of the actual obstacle.

In the control device, the entrance angle may be calculated based on a past position of the actual obstacle acquired by the first acquisition unit. With this, the entry angle to the following target trajectory can be predicted for an actual obstacle whose travel route cannot be known in advance originally. In addition, as an example, the entry angle to the tracking target trajectory can be calculated by performing linear approximation on the movement of the actual obstacle from the nearest two points or performing polynomial approximation on the movement of the actual obstacle from the nearest multiple points.

In the control device described above, the second-stage cost may be calculated to be smaller as the position of the control target passes through the position of the intersection where the tracking target trajectory intersects with the obstacle trajectory along which the actual obstacle follows, and the position of the control target is farther from the position of the intersection in the stage cost calculated by the predetermined evaluation function. When the position of the control object passes the position of the intersection while the control object is advancing on the tracking target trajectory, collision of the control object with the actual obstacle has been substantially avoided. Therefore, as described above, the second-stage cost is calculated to be smaller as the distance from the intersection increases after the position of the control target passes through the position of the intersection, so that the repulsive force generated by the second potential field of the virtual obstacle is prevented from being inadvertently applied to the control target, and thus the preferable follow-up control of the control target can be realized. As another method, in the control device described above, the second stage cost may be set to zero in the stage cost calculated by the predetermined evaluation function after the position of the control target passes through the position of the intersection where the tracking target trajectory and the obstacle trajectory along which the actual obstacle follows intersect.

In the control device described above, when a plurality of actual obstacles exist with respect to the control target that performs the following of the target command, the setting unit sets the positions of the virtual obstacles corresponding to the respective actual obstacles, and the respective actual obstacles and the respective virtual obstacles are reflected in the calculation of the stage cost of the model predictive control, whereby even if the number of actual obstacles is large, the control target can preferably avoid them and realize the following of the target command by the control target. However, if the model predictive control is to be performed in accordance with a large number of actual obstacles, the processing load of the control device required for the model predictive control increases, and therefore it is preferable to specify the number of actual obstacles that can be handled by the control device as the predetermined number.

In this way, the number of actual obstacles that can be handled in the model predictive control in the control device is limited to a predetermined number, and the predetermined evaluation function may calculate the first stage cost and the second stage cost corresponding to each of two or more predetermined numbers of actual obstacles. Further, when the positions of the actual obstacles exceeding the predetermined number of excesses are acquired by the first acquisition unit, the predetermined number of actual obstacles may be extracted from the predetermined exceedance of actual obstacles based on the separation distance between the control target and each of the predetermined exceedance of actual obstacles, and the first stage cost may be calculated based on the extracted positions of the predetermined number of actual obstacles and the second stage cost may be calculated based on the extracted positions of the predetermined number of virtual obstacles corresponding to the extracted predetermined number of actual obstacles, in the stage cost calculated by the predetermined evaluation function. That is, when there are more than a predetermined number of actual obstacles, the actual obstacles that are preferentially reflected in the model predictive control are extracted based on the separation distance from the control target. This is because the separation distance between the control object and the actual obstacle is an important factor for collision avoidance. With this configuration, the processing load of the control device can be suppressed as much as possible, and the target command can be controlled to follow the target command while avoiding collision between the control target and the actual obstacle.

ADVANTAGEOUS EFFECTS OF INVENTION

When the output of the control target follows the target command by the model predictive control, it is possible to preferably avoid a collision with the moving obstacle.

Drawings

Fig. 1 is a first diagram showing a schematic configuration of a control system including a servo driver as a control device.

Fig. 2 is a first diagram showing a control configuration of the servo driver according to the embodiment.

Fig. 3 is a diagram for explaining an outline of collision avoidance with an actual obstacle in model prediction control performed by the servo driver of the embodiment.

Fig. 4 is a first diagram showing the result of the follow-up control of the actual device (plant) by the servo driver according to the embodiment.

Fig. 5 is a second diagram showing a control configuration of the servo driver according to the embodiment.

Fig. 6 is a diagram showing transition of the first and second integral gains set in the model predictive control of the servo driver shown in fig. 5.

Fig. 7 is a second diagram showing the result of the follow-up control of the actual device by the servo driver according to the embodiment.

Fig. 8 is a third diagram showing the result of the follow-up control of the actual device by the servo driver according to the embodiment.

Fig. 9 is a fourth diagram showing the result of the follow-up control of the actual device by the servo driver according to the embodiment.

Fig. 10 is a fifth diagram showing the result of the follow-up control of the actual device by the servo driver according to the embodiment.

Description of the symbols

1: network

2: motor with a stator having a stator core

3: load device

4: servo driver

5: standard PLC

6: actual equipment

41: servo integrator

42: state acquisition unit

43: model prediction control unit

45: acquisition unit

46: setting part

61: physical obstacle

62: virtual obstacle

Detailed Description

< application example >

An application example of the control device according to the embodiment is described with reference to fig. 1 to 3. Fig. 1 is a schematic configuration diagram of a control system. The control system includes a network 1, a servo driver 4, and a standard Programmable Logic Controller (PLC) 5. The servo driver 4 is a control device for servo-controlling an actual device (hereinafter simply referred to as "actual device") 6 including the motor 2 and the load device 3. The actual device 6 becomes a control target. In the control system, the servo driver 4 performs feedback control of the actual device 6 so that the output of the actual device 6 follows the target command sent from the standard PLC 5. The servo driver 4 generates a control input for performing the follow-up control of the actual device 6 based on the target command received from the standard PLC 5. The generation of the control input by the servo driver 4 will be described later. Here, various mechanical devices (for example, an arm of an industrial robot or a conveying device) can be exemplified as the load device 3 constituting the actual equipment 6, and the motor 2 is incorporated in the load device 3 as an actuator for driving the load device 3. For example, the motor 2 is an Alternating Current (AC) servo motor. An encoder (not shown) is attached to the motor 2, and a parameter signal (a position signal, a velocity signal, or the like) relating to the operation of the motor 2 is fed back to the servo driver 4 by the encoder.

The standard PLC 5 generates a target command related to the operation (movement) of the actual device 6, and transmits the target command to the servo driver 4. The servo driver 4 receives the servo command from the standard PLC 5 via the network 1, and receives a feedback signal output from an encoder connected to the motor 2. The servo driver 4 supplies a drive current to the motor 2 based on the servo command and a feedback signal from the encoder so that the output of the actual device 6 follows a predetermined command. The supply current is ac power supplied from an ac power supply to the servo driver 4. In the present embodiment, the servo driver 4 is of a type that receives three-phase alternating current, but may be of a type that receives single-phase alternating current. In addition, for servo control of the actual plant 6, the servo driver 4 executes model prediction control by the model prediction control unit 43 as shown in fig. 2.

Here, a control structure of the servo driver 4 will be described based on fig. 2. The target command supplied from the standard PLC 5 to the servo driver 4 is referred to as "r", and the control input to the actual device 6 is referred to as "u". The servo driver 4 includes a state acquisition unit 42, a model prediction control unit 43, an acquisition unit 45, and a setting unit 46. The respective processes performed by the state acquisition unit 42, the model prediction control unit 43, the acquisition unit 45, and the setting unit 46 are calculated and executed by an arithmetic processing device mounted on the servo driver 4.

In the present embodiment, a state acquisition unit 42 and a model prediction control unit 43 are formed. The state acquisition unit 42 is for model prediction control by the model prediction control unit 43, and acquires values of state variables included in the state x of the actual plant 6. The model prediction control unit 43 executes model prediction control (rolling time domain control) using the state x of the actual plant 6 acquired by the state acquisition unit 42 and the control input u to the actual plant 6 output by itself.

More specifically, the model prediction control unit 43 has a prediction model that defines the correlation between the state x relating to the actual plant 6 and the control input u to the actual plant 6 by using the following state equation (equation 1). In addition, the following formula 1 is a nonlinear equation of state. The prediction model may reflect, for example, predetermined physical characteristics of the actual plant 6.

[ number 1]

Here, the model prediction control unit 43 receives the state x concerning the actual plant 6 and the control input u to the actual plant 6 as inputs, and performs model prediction control based on the prediction model expressed by the following expression 1 in accordance with the evaluation function expressed by the following expression 2 in a prediction section having a predetermined time width T.

[ number 2]

The first term on the right of equation 2 is the termination cost, and the second term on the right is the stage cost. The stage cost can be expressed by the following formula 3.

[ number 3]

Where xref (k) represents the target state quantity at time k, x (k) represents the calculated state quantity at time k, uref (k) represents the target control input at time k in a steady state, and u (k) represents the calculated control input at time k. Q and R are coefficients (weight coefficients) representing the weight of the state quantity in the stage cost and coefficients (weight coefficients) representing the weight of the control input, respectively. Therefore, the first term on the right side of equation 3 refers to the phase cost associated with the state quantity, referred to as the "state quantity cost", and the second term on the right side refers to the phase cost associated with the control input, referred to as the "control input cost". The third term, i.e., the first-stage cost, and the fourth term, i.e., the second-stage cost, on the right side will be described later.

Further, in the present embodiment, the acquisition unit 45 and the setting unit 46 are formed. The acquisition unit 45 acquires a predetermined parameter related to an actual obstacle (actual obstacle) recognized through imaging by the camera 8. The predetermined parameter is defined as the position of the actual obstacle or the angle between the moving direction of the actual obstacle when the actual obstacle is moving and the moving direction of the controlled object when the follow-up control is performed, and includes an entry angle θ (hereinafter, simply referred to as an "entry angle") of the actual obstacle with respect to the follow-up target trajectory.

For example, since the region captured by the camera 8 is known, the acquisition unit 45 can acquire the position of the actual obstacle by performing conventional techniques such as image processing on the captured image. Also, the entry angle of the actual obstacle can be acquired based on the acquired past position of the actual obstacle. For example, the movement of the two nearest points of the actual obstacle is linearly approximated by the positions OP1 and OP2 acquired by the acquisition unit 45, and the movement of the actual apparatus 6 is linearly approximated by the two points of the positions TP1 and TP2 at the corresponding timings. The entrance angle θ can be calculated by the cosine law. As another method, the entry angle θ to the tracking target trajectory can be calculated by performing polynomial approximation on the movement of the nearest multipoint actual obstacle based on the position thereof.

The setting unit 46 sets the position of the virtual obstacle based on the position of the actual obstacle. The virtual obstacle is an obstacle that is set virtually for the purpose of calculating the stage cost, particularly the second stage cost, according to equation 3 for avoiding a collision between the actual device 6 as a control target and the actual obstacle. Therefore, the virtual obstacle is not an actually existing object, unlike the actual device 6 and the actual obstacle. The setting unit 46 sets the position of the virtual obstacle to be substantially symmetrical with respect to the tracking target trajectory of the real device 6, which will be described later in detail.

The value of the control input u at the initial time t of the prediction section calculated in the model prediction control based on the above is output as the control input u to the actual plant 6 corresponding to the target command r at this time t. In the model predictive control, a prediction interval of a predetermined time width T is set every time this control time, and a control input u to the actual plant 6 at the control time is calculated based on the evaluation function of equation 2 and sent to the actual plant 6. The problem of obtaining the operation amount that optimizes the value of the evaluation function J of the form as expressed by equation 2 is a problem widely known as an optimal control problem, and an algorithm for calculating a numerical solution thereof is disclosed as a known technique. As such a technique, a continuous deformation method is exemplified, and details are disclosed in, for example, a high-speed algorithm (a connection/GMRES method for fast calculation of nonlinear rolling time domain control) which is a combination of the continuous deformation method and the GMRES method, which is a publicly known document, { great density (t.ohtsuka), automation (automation), 40 th container, p563 to 574, 2004 }.

In the continuous deformation method, the input u (t) in the model predictive control is calculated by solving a simultaneous linear equation relating to the input u (t) shown in the following formula 4. Specifically, equation 4 is solved by numerically integrating dU/dt and updating the input u (t). In this way, since the continuous deformation method does not repeat the calculation, the calculation load for calculating the input u (t) at each time can be suppressed as much as possible.

[ number 4]

Here, F, U (t) is represented by the following formula 5.

[ number 5]

U(t)＝[u₀ ^*T(t),μ₀ ^*T(t),...u_{N_1} ^*T(t),μ_{N_1} ^*T(t)]

… (formula 5)

Where H is a Hamiltonian (Hamiltonian), λ is a common state, and μ is a Lagrange multiplier (Lagrange multiplier) with the constraint C ═ 0.

Here, the calculation of the stage cost according to the above equation 3 in the present embodiment will be described in detail with reference to fig. 3. Fig. 3 shows a state in which a moving actual obstacle 61 is present around the actual device 6 controlled so as to follow the target command r. The actual obstacle 61 is recognized by the shooting of the camera 8 for its existence, and the position of the actual obstacle 61 is acquired by the acquisition section 45. In addition, the acquired position of the actual obstacle 61 is referred to as "p 61". Here, based on the fact that the actual obstacle 61 moves relative to the actual device 6, a probability potential field indicating the probability that the actual obstacle 61 may exist around the actual device 6 can be calculated (the probability potential field due to the actual obstacle 61 is set as the first potential field). The calculation of the probability potential field itself is a known technique, and therefore can be calculated by using, for example, the technique described in japanese patent laid-open No. 2003-241836.

In the present embodiment, regardless of whether or not the first potential field of the actual obstacle 61 is directly calculated, the first stage cost associated with the first potential field is included in the stage cost calculated in the model prediction control by the model prediction control unit 43 (see expression 3 above). The first-stage cost can be expressed by the following equation 6.

[ number 6]

n_obs＝|pt-p61|

… (formula 6)

Where pt is the position of the actual device 6 and k is the coefficient associated with the first potential field.

By generating the first-stage cost in this manner, when the probability value of the first potential field becomes high, the first-stage cost increases, and the model prediction control unit 43 calculates the control input u so that the "repulsive force" finally generated by the first potential field of the actual obstacle 61 moves the actual device 6 that is performing the follow-up control on the target command r away from the actual obstacle 61 while performing the follow-up control on the target command r. For example, the actual device 6 will follow the avoidance trajectory shown in figure 3 with a chain of dots, according to such a control input u. However, in such an avoidance trajectory, since the actual device 6 itself will avoid in front of the moving actual obstacle 61, the time during which the actual device 6 is affected by the first potential field of the actual obstacle 61 becomes long, and in some cases, the actual device 6 may have difficulty avoiding the actual obstacle 61 (fig. 3 shows a state in which the actual device 6 following the avoidance trajectory collides with the actual obstacle 61 at a position indicated by "x").

Therefore, in the present embodiment, in order to avoid a collision between the moving actual obstacle 61 and the actual device 6, the position of the virtual obstacle 62 is set by the setting unit 46. Specifically, the position of the virtual obstacle 62 is set to be substantially symmetrical to the actual obstacle 61 with respect to the tracking target trajectory of the actual device 6. In addition, the position of the virtual obstacle 62 will be referred to as "p 62". Further, similarly to the case of the actual obstacle 61, a probability potential field indicating a probability that the virtual obstacle 62 may exist around the actual device 6 can be calculated (the probability potential field due to the virtual obstacle 62 is set as the second potential field). Then, as in the case of the actual obstacle 61, regardless of whether or not the second potential field of the virtual obstacle 62 is directly calculated, the second stage cost associated with the second potential field is included in the stage cost calculated in the model prediction control by the model prediction control unit 43 (see the above-described expression 3). The second-stage cost can be expressed by the following formula 7.

[ number 7]

n_pobs＝|pt-p62|

… (formula 7)

Where pt is the position of the actual device 6, and k is a coefficient relating to the second potential field, and is the same as k shown in equation 6 in the present embodiment.

By including the first stage cost and the second stage cost in the stage cost calculated by the model prediction control unit 43 in this manner, it is possible to cause the repulsive force of the first potential field of the actual obstacle 61 to act on the actual device 6 from one side of the target trajectory and the repulsive force of the second potential field of the virtual obstacle 62 to act on the actual device 6 from the other side of the target trajectory. Fig. 4 depicts (plot) the trajectories of the actual device 6, the actual obstacle 61, and the virtual obstacle 62 when the model prediction control of the present embodiment is performed in the case where the actual obstacle 61 travels at the entrance angle θ of 30 degrees with respect to the tracking target trajectory followed by the actual device 6. Note that, in fig. 4, the control axes for model predictive control are two axes, i.e., the horizontal axis and the vertical axis. As can be understood from this, the actual device 6 is preferably decelerated when approaching the actual obstacle 61 based on the control input u calculated by the model predictive control of the present embodiment, and the follow-up control on the follow-up target trajectory can be preferably maintained so as to avoid behind the actual obstacle 61.

< first structural example >

The servo control performed by the servo driver 4 of the present configuration example will be described with reference to fig. 5. In the servo driver 4 of the present configuration example, the model predictive control is performed by the model predictive control unit 43 as in the above-described application example, but in this case, the output z of the servo integrator 41 is acquired by the state acquisition unit 42 and subjected to the model predictive control. Specifically, the deviation e (e-r-y) between the target command r transmitted from the standard PLC 5 and the output y of the real plant 6 fed back by the feedback system 44 is input to the servo integrator 41. The output z of the servo integrator 41 is input to the model prediction control unit 43 via the state acquisition unit 42. Therefore, the state acquisition unit 42 adds an output z to the state variable relating to the actual plant 6, and supplies the output z to the model prediction control unit 43 for the model prediction control.

In this way, based on the control configuration including the servo integrator 41, the prediction model of the model prediction control unit 43 in this configuration can be expressed by, for example, the following equation 8.

[ number 8]

In the formula 8, (r-y) represents the deviation e. It is to be understood that the prediction model includes a deviation e (r-y) and a first integral gain K _iAnd a second integral gain ζ. First integral gain K_iAnd the product of the second integral gain ζ corresponds to a predetermined integral gain. Thus, in the servo control by the servo driver 4 using the model predictive control, in addition to the effect of avoiding collision with an actual obstacle and providing good followability to a target command shown in the application example, the integral amount to be the servo control drive source is easily adjusted, and the servo control in which overshoot (overshoot) is suppressed is easily realized without using a disturbance observer which requires an inconvenient adjustment such as expansion of a disturbance model or design of an observer gain as in the conventional art.

Here, the first integral gain K constituting the predetermined integral gain of the integral term included in the prediction model shown in equation 8_iAs shown in the upper stage (a) of fig. 6, the adjustment can be performed based on the deviation e. Specifically, the first integral gain K is adjusted in the following manner_iI.e. as the magnitude of the deviation e becomes smaller, the first integral gain K_iThe value of (c) becomes large. For example, when the magnitude of the deviation e is equal to or larger than a predetermined value e0, the first integral gain K_iBecomes 0, and the first integral gain K is set in a range where the magnitude of the deviation e is smaller than e0 _iThe value is set to be greater than 0 and 1 or less. Furthermore, the first integral gain K_iIs set so that the first integral gain K becomes closer to 0 as the magnitude of the deviation e becomes closer to 0_iIs abruptly close to 1, and when the magnitude of the deviation e is 0, the first integration gain K is obtained_iBecomes 1. Thus, the first integral gain K can be adjusted based on the magnitude of the deviation e_iThus, in the case where the output y of the actual device 6 is relatively deviated from the target command r, the first integral gain K_iIs adjusted to be small so that the amount of integration for servo control is adjusted not to be unnecessarily backlogged. Also, when the deviation of the output y of the actual device 6 from the target command r becomes small, that is, when the magnitude of the deviation e becomes small, the first integral gain K_iSince the value of (d) is adjusted to be large, the following ability in the servo control can be effectively improved. By making the first integral gain K in this way_iThe value of (2) is varied, so that both vibration suppression and overshoot suppression can be achieved, and a good follow-up property of servo control can be realized.

On the other hand, when the relationship between the actual device 6 and the actual obstacle 61 is considered, the collision avoidance between the two is basically preferable to the followability to the target command when the distance (norm) between the two is small. In this case, as described above, if the first integral gain K is passed _iIn addition, if the influence of the predetermined integral term of the prediction model is relatively larger than the influence of the first-stage cost and the second-stage cost associated with the actual obstacle 61 and the virtual obstacle 62, it may become difficult to avoid collision with the actual obstacle 61. Therefore, the predetermined integral gain includesTwo integral gains ζ. The second integral gain ζ can be adjusted based on a parameter related to the possibility of collision of the actual device 6 with the actual obstacle 61, for example, a separation distance (norm) ncol between the actual device 6 and the actual obstacle 61, as shown in the lower stage (b) of fig. 6. Specifically, when the norm ncol is equal to or larger than a predetermined ncol0, the second integral gain ζ becomes 1, and the second integral gain ζ is set to a value equal to or larger than 0 and equal to or smaller than 1 in a range where the norm ncol is smaller than ncol 0. The transition of the second integration gain ζ is set such that the value of the second integration gain ζ approaches 0 abruptly as the magnitude of the norm ncol approaches 0.

In this way, in the predetermined integral term of the prediction model shown in equation 8, the predetermined integral gain includes the first integral gain K_iAnd a second integral gain ζ. Thus, basically, the first integral gain K corresponding to the deviation e is passed _iThe integration effect can be improved in the following ability to the target command r, but when the norm of the actual device 6 and the actual obstacle 61 becomes smaller and the possibility of collision increases, the integration effect can be reduced by the effect of the second integration gain ζ. Fig. 7 shows the trajectories of the actual device 6, the actual obstacle 61, and the virtual obstacle 62 when the model predictive control of the present embodiment is performed in the case where the actual obstacle 61 travels at the entrance angle θ of 30 degrees with respect to the tracking target trajectory followed by the actual device 6. Note that, in fig. 7, the control axes for model predictive control are two axes, i.e., the horizontal axis and the vertical axis. The upper part (a) of fig. 7 depicts that the predetermined integral term of the prediction model shown in equation 8 includes only the first integral gain K_iThe lower part (b) of the time-course trajectory depicts the first integral gain K included in the predetermined integral term as shown in equation 8_iAnd a second integral gain ζ. From this, it can be understood that in the upper stage (a), the following performance to the target command is good by the integration effect, but the collision avoidance between the actual device 6 and the actual obstacle 61 is not sufficient. On the other hand, in the lower stage (b), the integration effect is relaxed, and as a result, the deceleration of the actual equipment 6 effectively acts so as to be at the actual obstacle 61 Avoiding the rear.

In addition, with respect to the first integral gain K_iAnd adjustment of the second integral gain ζ, data relating to the correlation between the deviation e or the norm ncol shown in fig. 6 and each integral gain may be stored in the memory of the servo driver 4, and at this time, the model prediction control unit 43 accesses the data to adjust each integral gain.

< second structural example >

The servo control performed by the servo driver 4 of the present configuration example will be described. If the model predictive control of the virtual obstacle 62 is continuously taken into consideration after the collision between the actual device 6 and the actual obstacle 61 is avoided, there is a possibility that the repulsive force generated by the second potential field of the virtual obstacle 62 may adversely affect the followability of the actual device 6 to the target command r. Therefore, in the model prediction control of the model prediction control unit 43 in the servo driver 4 of the present configuration example, the second stage cost is calculated based on, for example, the following expression 9. The control configuration applicable to this configuration example may be any of fig. 2 and 5.

[ number 9]

n_pobs＝|pt-p62|

pseudoPot＝(tanh((Pcross-pt)×kz)+1)/2

… (formula 9)

The Pcross is a position where the actual equipment 6 may collide with the actual obstacle 61, and is, for example, a position of an intersection where a tracking target trajectory of the actual equipment 6 intersects with an obstacle trajectory assumed to be traveled by the actual obstacle 61. It should be noted that Pcross is not a position where the actual apparatus 6 actually collides with the actual obstacle 61, but a position where the collision can be avoided but the collision is likely to be caused by the present embodiment is assumed. Therefore, after the actual equipment 6 passes through the position indicated by Pcross, it basically means that the possibility of collision of the actual equipment 6 with the actual obstacle 61 becomes small.

The intersection point position Pcross can be acquired based on the past position of the acquired actual obstacle. For example, the movement of the two nearest points of the actual obstacle is linearly approximated by the positions OP1 and OP2 acquired by the acquisition unit 45, and the movement of the actual apparatus 6 is linearly approximated by the two points of the positions TP1 and TP2 at the corresponding timings. The position of the intersection of the two straight lines is defined as an intersection position Pcross.

The second-stage cost is calculated by the equation 9 such that the second-stage cost decreases as the position of the actual plant 6 is farther from the intersection position Pcross in the calculation of the stage cost in the model predictive control at least after the actual plant 6 passes the intersection position Pcross. As a result, after the actual device 6 passes through the intersection position Pcross, the effect of carelessness on the actual device 6 due to the second potential field of the virtual obstacle 62 can be reduced, and the decrease in the following ability can be avoided.

Fig. 8 depicts the trajectories of the actual device 6, the actual obstacle 61, and the virtual obstacle 62 when the model predictive control of the present embodiment is performed in the case where the actual obstacle 61 travels at the entrance angle θ of 30 degrees with respect to the tracking target trajectory followed by the actual device 6. Note that, in fig. 8, the control axes for model predictive control are two axes, i.e., the horizontal axis and the vertical axis. As can also be understood from fig. 8, the size of the drawing representing the virtual obstacle 62 becomes smaller after the actual equipment 6 passes through the intersection point position Pcross. This means that the value of the second stage cost is calculated small by the pseudoPot of said equation 9. As a result, after the actual device 6 passes through the intersection position Pcross, the influence of the virtual obstacle 62 is alleviated, and it is possible to avoid a decrease in the following ability of the actual device 6 with respect to the target command r.

In the above equation 9, the value of the second-stage cost is calculated to be smaller as the actual equipment 6 is farther from the intersection position Pcross after the actual equipment 6 passes the intersection position Pcross, but the value of the second-stage cost may be calculated to be smaller as it is before the actual equipment 6 approaches the intersection position Pcross. Further, as another method, the value of the second-stage cost may be set to zero after the actual plant 6 passes through the intersection position Pcross.

< third structural example >

The servo control performed by the servo driver 4 of the present configuration example will be described. As the entrance angle of the actual obstacle 61 approaches 90 degrees, the repulsive force formed by the first potential field of the actual obstacle 61 and the repulsive force formed by the second potential field of the virtual obstacle corresponding to the actual obstacle 61 are less likely to act on the actual device 6, and it is more difficult to achieve collision avoidance with the actual obstacle 61. Therefore, in the model prediction control of the model prediction control unit 43 in the servo driver 4 of the present configuration example, the second stage cost is calculated, for example, according to the following expression 10. Note that the control configuration applicable to this configuration example may be any of fig. 2 and 5.

[ number 10]

n_pobs＝|pt-p62|

pseudoPot＝1+sinθ

… (formula 10)

The second-stage cost is calculated by using the equation 10, in such a manner that the closer the entrance angle of the actual obstacle 61 is to 90 degrees, the larger the second-stage cost becomes. As a result, even if the entrance angle of the actual obstacle 61 approaches 90 degrees, the actual device 6 can ensure the following ability to the target command r and can avoid the collision with the actual obstacle 61 well.

Fig. 9 depicts the trajectories of the actual device 6, the actual obstacle 61, and the virtual obstacle 62 when the model predictive control of the present embodiment is performed in the case where the actual obstacle 61 travels at the entrance angle θ of 90 degrees with respect to the tracking target trajectory followed by the actual device 6. Note that, in fig. 9, the control axes for model predictive control are two axes, i.e., the horizontal axis and the vertical axis. In addition, the upper part (a) of fig. 9 depicts a trajectory when pseudoPot of the prediction model shown in equation 10 is fixed to a fixed value (1), and the lower part (b) depicts a trajectory when the pseudoPot is adjusted by 90 degrees according to the entry angle according to equation 10. Accordingly, in the upper stage (a), since the entrance angle is 90 degrees, as described above, the repulsive force generated by each potential field is hard to act on the actual device 6, and as a result, the actual device 6 is avoided in front of the moving actual obstacle 61, and the avoidance amount is relatively large. On the other hand, in the lower stage (b), the second stage cost is calculated in consideration of the entrance angle θ. That is, when the entrance angle is 90 degrees, the value of pseudoPot is 2, which is twice as large as that of the upper stage (a). As a result, the influence of the repulsive force generated by the second potential field of the virtual obstacle 62 can be made to act relatively largely, and the actual device 6 can be avoided behind the moving actual obstacle 61 as shown in the lower stage (b), and the avoidance amount is also made relatively small.

In the above example, the entry angle θ is reflected in the calculation of the second stage cost, but the entry angle θ may be further reflected in the calculation of the first stage cost as shown in the following equation 11.

[ number 11]

n_obs＝|pt-p61|

… (formula 11)

Further, the technical idea disclosed in the second configuration example may be used together, and the entrance angle θ may be reflected in the calculation of the second stage cost as shown in the following equation 12.

[ number 12]

n_pobs＝|pt-p62|

pseudoPot

＝(1+sinθ)×(tanh((Pcross-pt)×kz)+1)/2

… (formula 12)

< fourth structural example >

The servo control performed by the servo driver 4 of the present configuration example will be described. In the present configuration example, model prediction control is mentioned in which, when the actual device 6 performs the follow-up control on the target command r, the presence of a plurality of (for example, two)

actual obstacles

61a and 61b (see fig. 10 described later) is recognized and each position is acquired by the acquisition unit 45. In the model prediction control performed by the model prediction control unit 43 of the present configuration example, the first-stage cost is calculated for each of the two recognized

actual obstacles

61a and 61b, and the second-stage cost is calculated for each of the

virtual obstacles

62a and 62b set by the setting unit 46, respectively, according to the following equation 12.

[ number 13]

The calculation of each first stage cost and each second stage cost is as shown in the application examples and the configuration examples described so far.

By calculating the stage cost in the model predictive control based on equation 13 as described above, even when a plurality of

actual obstacles

61a and 61b exist, it is possible to achieve both the following ability of the actual plant 6 to the target command r and the avoidance of collision with each actual obstacle. Fig. 10 depicts the trajectories of the actual device 6, the actual obstacle 61a, the actual obstacle 61b, the virtual obstacle 62a, and the virtual obstacle 62b when the model prediction control of the present embodiment is performed in the case where the two

actual obstacles

61a and 61b travel with respect to the tracking target trajectory followed by the actual device 6. Note that, in fig. 10, the control axes for model predictive control are two axes, i.e., the horizontal axis and the vertical axis. The virtual obstacle 62a and the virtual obstacle 62b are set by the setting unit 46 so as to correspond to the actual obstacle 61a and the actual obstacle 61b, respectively. In the embodiment shown in fig. 10, as shown in configuration example 2, after the actual device 6 has passed through the intersection with each actual obstacle, the value of the second-stage cost associated with the virtual obstacle corresponding to the actual obstacle is set to zero. As can be understood from fig. 10, by performing the model predictive control according to the present embodiment based on equation 13, the actual plant 6 can preferably maintain the following ability to the target command r while avoiding behind each of the two

actual obstacles

61a and 61 b.

Even when the number of actual obstacles is three or more, the virtual obstacles corresponding to the number are set by the setting unit 46, and the first-stage cost and the second-stage cost associated with each virtual obstacle are calculated to perform the model predictive control, whereby a collision with each actual obstacle can be avoided and a preferable follow-up to the target command r can be realized.

However, if the number of actual obstacles considered in the model predictive control is increased, the processing load of the servo driver 4 increases by the number, and there is a possibility that the real-time follow-up control of the actual plant 6 is hindered. Therefore, the number of actual obstacles considered in the model predictive control and the number of virtual obstacles set in accordance with the actual obstacles may be limited to a predetermined number (for example, two each) in consideration of the processing load of the servo driver 4. In this case, when the presence of a predetermined number of excess actual obstacles (for example, three obstacles) exceeding the predetermined number is recognized and the position of the predetermined number of excess actual obstacles is acquired by the acquisition unit 45, the predetermined number of actual obstacles that can be considered in the model predictive control is extracted from the predetermined number of excess actual obstacles on the basis of a predetermined reference. As the predetermined criterion, the separation distance between the actual equipment 6 and the actual obstacle may be used in consideration of the possibility of collision between the actual equipment 6 and the actual obstacle. For example, since the smaller the separation distance, the higher the possibility of collision, meaning that the collision should be avoided with priority, the predetermined number of actual obstacles may be extracted in descending order of the separation distance. In the model predictive control, the first-stage costs associated with the extracted actual obstacles are calculated based on the positions of the extracted actual obstacles, and the second-stage costs associated with the extracted actual obstacles are calculated based on the positions of the virtual obstacles corresponding to the extracted virtual obstacles. With this configuration, the processing load of the servo driver 4 is appropriately suppressed, and then the real-time and preferable follow-up control of the actual device 6 is realized.

< Note 1 >

A control device 4 that includes a model prediction control unit 43 and causes an output of a control target to follow a predetermined target command, wherein the model prediction control unit 43 has a prediction model that defines a correlation between a predetermined state variable relating to the control target 6 and a control input to the control target 6 in the form of a predetermined state equation, and the model prediction control unit 43 performs model prediction control based on the prediction model in a predetermined time-width prediction section for the predetermined target command to be followed by the output of the control target 6 according to a predetermined evaluation function, and outputs at least a value of the control input at an initial time of the prediction section, and the control device 4 includes:

a first acquisition unit 45 that acquires a position of an actual obstacle 61, which is an actual obstacle, with respect to the control object 6; and

a setting unit 46 that sets the position of a virtual obstacle 62 such that the virtual obstacle 62 associated with the actual obstacle 61 is positioned substantially symmetrically with respect to the actual obstacle 61 with respect to a target trajectory of the control object 6 based on the predetermined target command, based on the position of the actual obstacle 61 acquired by the first acquisition unit 45,

The phase cost calculated by the predetermined evaluation function includes: a state quantity cost, which is a phase cost associated with the specified state variable; a control input cost that is a phase cost associated with the control input; a first stage cost associated with a first probability potential field representing a probability that the actual obstacle 61 may exist based on a location of the actual obstacle 61; and a second stage cost associated with a second probabilistic potential field representing a probability that the virtual obstacle 62 may exist based on the position of the virtual obstacle 62 and having a probability value that is greater than or equal to the first probabilistic potential field.

< Note 2 >

A servo integrator 41 for inputting a deviation between the predetermined target command and the output of the control target;

the phase cost calculated by the predetermined evaluation function includes: a state quantity cost, which is a phase cost associated with the specified state variable; a control input cost that is a phase cost associated with the control input; a first stage cost associated with a first probability potential field representing a probability that the actual obstacle 61 may exist based on a location of the actual obstacle 61; and a second stage cost associated with a second probabilistic potential field that is a probabilistic potential field representing a probability that the virtual obstacle 62 may exist based on the position of the virtual obstacle 62 and has a probability value that is greater than or equal to the first probabilistic potential field,

The state variable associated with the controlled object 6 includes a predetermined integral term expressed by a product of the deviation and a predetermined integral gain,

the shorter the distance between the control target and the actual obstacle is, the smaller the predetermined integral gain is.

Claims

1. A control device that includes a model prediction control unit that specifies a correlation between a predetermined state variable relating to a control target and a control input to the control target in the form of a predetermined state equation, and that outputs a value of the control input at least at an initial time of a prediction section, the model prediction control unit having a prediction model that performs model prediction control based on the prediction model for the predetermined target command to be followed by the output of the control target in accordance with a predetermined evaluation function within the prediction section having a predetermined time width, the control device comprising:

a first acquisition unit that acquires a position of an actual obstacle, which is an actual obstacle, with respect to the control target; and

a setting unit that sets a position of a virtual obstacle based on the position of the actual obstacle acquired by the first acquisition unit, such that the virtual obstacle associated with the actual obstacle is positioned substantially symmetrically with respect to the actual obstacle, based on a target trajectory for following the control target based on the predetermined target command,

The phase cost calculated by the predetermined evaluation function includes: a state quantity cost, which is a phase cost associated with the specified state variable; a control input cost, which is a phase cost associated with the control input; a first stage cost associated with a first probabilistic potential field representing a probability that the actual obstacle may be present based on a location of the actual obstacle; and a second stage cost associated with a second probabilistic potential field representing a probability that the virtual obstacle may exist based on a position of the virtual obstacle and having a probability value above the first probabilistic potential field.

2. A control device that includes a model prediction control unit that specifies a correlation between a predetermined state variable relating to a control target and a control input to the control target in the form of a predetermined state equation, and that outputs a value of the control input at least at an initial time of a prediction section, the model prediction control unit having a prediction model that performs model prediction control based on the prediction model for the predetermined target command to be followed by the output of the control target in accordance with a predetermined evaluation function within the prediction section having a predetermined time width, the control device comprising:

A servo integrator for inputting a deviation between the predetermined target command and an output of the control target;

the phase cost calculated by the predetermined evaluation function includes: a state quantity cost, which is a phase cost associated with the specified state variable; a control input cost that is a phase cost associated with the control input; a first stage cost associated with a first probability potential field representing a probability that the actual obstacle may exist based on a location of the actual obstacle; and a second stage cost associated with a second probabilistic potential field representing a probability that the virtual obstacle may exist based on a position of the virtual obstacle and having a probability value above the first probabilistic potential field,

The state variable associated with the control target includes a predetermined integral term expressed by a product of the deviation and a predetermined integral gain,

3. The control device according to claim 1 or 2, further comprising:

a second acquisition unit that acquires an entry angle of the actual obstacle with respect to the tracking target trajectory, the entry angle being defined as an angle between a movement direction of the control target and a movement direction of the actual obstacle,

in the stage cost calculated by the predetermined evaluation function, the second stage cost is calculated to be larger as the entrance angle approaches 90 degrees.

4. The control device of claim 3, wherein

The entrance angle is calculated based on the past position of the actual obstacle acquired by the first acquisition unit.

5. The control device according to claim 1 or 2, wherein

After the position of the control target passes through the position of the intersection where the tracking target trajectory and the obstacle trajectory along which the actual obstacle follows intersect, the second stage cost is calculated to be smaller as the position of the control target is farther from the position of the intersection in the stage cost calculated by the predetermined evaluation function.

6. The control device according to claim 1 or 2, wherein

The second stage cost is set to zero in the stage cost calculated by the predetermined evaluation function after the position of the control target passes through the position of the intersection where the tracking target trajectory and the obstacle trajectory along which the actual obstacle follows intersect.

7. The control device according to claim 1 or 2, wherein

The predetermined evaluation function is configured to calculate the first stage cost and the second stage cost corresponding to each of two or more predetermined numbers of the actual obstacles,

when the positions of the actual obstacles exceeding the predetermined number of the excesses are acquired by the first acquisition unit, the predetermined number of the actual obstacles is extracted from the predetermined exceedance of the actual obstacles based on the separation distance between the control target and each of the predetermined exceedance of the actual obstacles, and further, the first stage cost is calculated based on the extracted positions of the predetermined number of the actual obstacles among the stage costs calculated by the predetermined evaluation function, and the second stage cost is calculated based on the positions of the predetermined number of the virtual obstacles corresponding to the extracted predetermined number of the actual obstacles.