Control device

Abstract

A model prediction control part of a control device includes an obstacle avoidance control unit that operates when there are a plurality of actual obstacles to be avoided. The obstacle avoidance control unit decides the position of a virtual obstacle from the positions of the plurality of actual obstacles acquired by an acquisition part so as to be positioned between the plurality of actual obstacles, and performs model prediction control by using, as the stage cost, the addition result of a standard cost and a virtual obstacle evaluation term for which a prescribed function, which uses, as parameters, at least the position of the virtual obstacle and the position of a moving body, is multiplied by a virtual obstacle weight. Using this configuration, when the moving body is caused to follow with respect to a target trajectory by the model prediction control, a collision with an obstacle can be avoided suitably.

Claims

1. A control device which controls a driving device that moves a moving body, comprising: a processor, configured to acquire a position of each of actual obstacles which may obstruct movement of the moving body along a target trajectory; and perform a model prediction control to move the moving body along the target trajectory by calculating a control input and supplying the control input to the driving device based on a prediction model, determine presence/absence of the actual obstacle to be avoided according to information acquired from the processor, and use a standard cost as a stage cost when there is no actual obstacle to be avoided, wherein the control input is configured to minimize a value of an evaluation function comprising a sum of stage costs in a predicted section of a prescribed time width, and the prediction model is configured to be used for predicting a position of the moving body from the control input to the driving device, and wherein the model prediction control is further configured to perform an obstacle avoidance control that is configured to operate when there are a plurality of the actual obstacles to be avoided, decide a position of a virtual obstacle from the positions of the plurality of actual obstacles acquired by processor so as to be positioned between the plurality of actual obstacles, and calculate the control input that is configured to minimize the evaluation function by using, as the stage cost, an addition result of the standard cost and a virtual obstacle evaluation term for which a prescribed function, which uses, as parameters, at least the position of the virtual obstacle and the position of the moving body, is multiplied by a virtual obstacle weight.

2. The control device according to claim 1, wherein the obstacle avoidance control is configured to use, as the stage cost, an addition result of the standard cost, the virtual obstacle evaluation term, and an actual obstacle evaluation term for which the prescribed function is multiplied by the weight for each of the actual obstacles to be avoided.

3. The control device according to claim 2, wherein when there are two or three actual obstacles to be avoided, the obstacle avoidance control is configured to decide the position of the virtual obstacle to a position which is equidistant from the two or three actual obstacles and has the shortest distance to each of the actual obstacles.

4. The control device according to claim 2, wherein the model prediction control is further configured to determine whether or not the moving body comes into a speed reducing state, and the obstacle avoidance control is configured to start operations when the moving body is determined to come into the speed reducing state under a situation that there are the plurality of actual obstacles to be avoided by the model prediction control.

5. The control device according to claim 1, wherein the obstacle avoidance control is configured to use, as the stage cost, an addition result of the standard cost and the virtual obstacle evaluation term.

6. The control device according to claim 5, wherein the obstacle avoidance control is configured to decide a value of the virtual obstacle weight in a way that when starting operations, a calculation result of the virtual obstacle evaluation term at that time point matches the sum of values at that time point of the actual obstacle evaluation terms for which the prescribed function is multiplied by the weight for each of the actual obstacles to be avoided.

7. The control device according to claim 6, wherein When there are two actual obstacles to be avoided, the obstacle avoidance control is configured to use, as the prescribed function, a function in which a change rate of the virtual obstacle evaluation term in an arrangement direction of the two actual obstacles is smaller than that in another direction.

8. The control device according to claim 5, wherein when there are two or three actual obstacles to be avoided, the obstacle avoidance control is configured to decide the position of the virtual obstacle to a position which is equidistant from the two or three actual obstacles and has the shortest distance to each of the actual obstacles.

9. The control device according to claim 5, wherein the model prediction control is further configured to determine whether or not the moving body comes into a speed reducing state, and the obstacle avoidance control is configured to start operations when the moving body is determined to come into the speed reducing state under a situation that there are the plurality of actual obstacles to be avoided by the model prediction control.

10. The control device according to claim 1, wherein when the positions of the plurality of actual obstacles are acquired by the processor, the model prediction control is configured to calculate, for each of the plurality of actual obstacles, an evaluation value indicating a probability in which a collision with the moving body may occur, and decides which of the plurality of actual obstacles is the actual obstacle to be avoided based on the evaluation value calculated for each of the actual obstacles.

11. The control device according to claim 1, wherein when there are two or three actual obstacles to be avoided, the obstacle avoidance control is configured to decide the position of the virtual obstacle to a position which is equidistant from the two or three actual obstacles and has the shortest distance to each of the actual obstacles.

12. The control device according to claim 1, wherein the model prediction control is further is configured to determine whether or not the moving body comes into a speed reducing state, and the obstacle avoidance control is configured to start operations when the moving body is determined to come into the speed reducing state under a situation that there are the plurality of actual obstacles to be avoided by the model prediction control.

13. The control device according to claim 12, wherein when an absolute value of a difference between a current value and a previous value of a minimum value of the evaluation function, or an absolute value of a difference between a current value and a previous value of the position of the moving body is less than a prescribed threshold value, the model prediction control is further configured to determine that the moving body comes into the speed reducing state.

14. The control device according to claim 13, wherein when the absolute value of the difference between the current value and the previous value of the minimum value of the evaluation function, or the absolute value of the difference between the current value and the previous value of the position of the moving body is less than the prescribed threshold value for a prescribed number of times continuously, the model prediction control is further configured to determine that the moving body comes into the speed reducing state.

Description

BRIEF DESCRIPTION OF THE DRAWINGS

(1) FIG. 1 is an illustration diagram of a use form of a control device according to an embodiment of the present invention.

(2) FIG. 2 is an illustration diagram of a schematic configuration of the control device according to the embodiment.

(3) FIG. 3 is an illustration diagram of a control result example of the control device according to the embodiment.

(4) FIG. 4 is an illustration diagram of a capability of the control device according to the embodiment.

(5) FIG. 5 is an illustration diagram of a control result example of the control device according to the embodiment.

DESCRIPTION OF THE EMBODIMENTS

(6) Hereinafter, an embodiment of the present invention is described with reference to the drawings.

(7) FIG. 1 shows a use form of a control device 10 according to an embodiment of the present invention, and FIG. 2 shows a schematic configuration of the control device 10 according to the embodiment.

(8) The control device 10 (FIG. 1) according to the embodiment is a device which is used for respectively controlling, by a model prediction control, a plurality of (one in the diagram) motors 21 driving a specific load device 22 and is generally referred to as a servo amplifier, a servo driver, or the like. Moreover, the load device 22 is an arm of an industrial robot, a carrying device, or the like. Hereinafter, a part including the plurality of motors 21 controlled by the control device 10 and the load device 22 driven by each of the motors 21 is referred to as a control object 20.

(9) As shown in FIG. 2, the control device 10 is configured to function as an acquisition part 11 and a model prediction control part 12.

(10) The acquisition part 11 is a unit which acquires a position (center position) and a size of each actual obstacle existing in the surrounding of a moving body in the control object 20 (a hand of a robot or the like, hereinafter simply referred to as a moving body) by analyzing an imaging result obtained by a camera 30. Moreover, the camera 30 may capture images only in a prescribed range in an advancing direction of the moving body. In addition, the acquisition part 11 may acquire the position and the size only of each actual obstacle existing in the prescribed range in the advancing direction of the moving body (in other words, each actual obstacle to be avoided).

(11) The model prediction control part 12 is a unit which controls, based on the position and the size of each obstacle acquired by the acquisition part 11 and information from the control object 20 (a position of the moving body and the like), the control object 20 by the model prediction control in order that the moving body avoids each obstacle and moves along a target trajectory instructed from a programmable logic controller (PLC) 31.

(12) The control performed by the model prediction control part 12 is the control in which (an input u minimizing a value of an evaluation function J described below is calculated using a prediction model of the control object 20 and based on a state x of the control object to supply to the control object 20).

(13) [Mathematical Formula 1]

(14) $\begin{array}{cc}J=\phi \left(x\left(t+T\right)\right)+{\int }_0^{t+T}L\left(x\left(\tau \right),u\left(\tau \right)\right)d\tau & \left(\mathrm{Formula}1\right)\end{array}$
Moreover, a first term on the right side of Formula 1 denotes a terminal cost, and L in Formula 1 denotes a stage cost. In addition, T denotes a time width which is previously set as a length of a predicted section in the model prediction control.

(15) That is, the control performed by the model prediction control part 12 is basically the same as the model prediction control which is generally performed.

(16) Herein, the model prediction control part 12 is configured to change the stage cost L used for the calculation of the J value depending on the situation.

(17) Taking the above as the premise, hereinafter, a capability of the model prediction control part 12 is described specifically. Moreover, the control device 10 is a device which can also move the moving body in a three-dimensional space. However, hereinafter, the moving body is set to be moved on a plane surface for convenience of description.

(18) The model prediction control part 12 periodically determines the presence/absence of the actual obstacle to be avoided based on the position and the size of each actual obstacle acquired by the acquisition part 11. More specifically, when no position and size of the actual obstacle is acquired by the acquisition part 11 (when there is no actual obstacle), the model prediction control part 12 determines that there is no actual obstacle to be avoided. In addition, when the positions and the sizes of one or more actual obstacles are acquired by the acquisition part 11, based on the position and the size of each actual obstacle, the model prediction control part 12 specifies actual obstacles, which satisfy a condition in which a value of an actual obstacle evaluation term L.sub.k defined by the following Formula 2 is equal to or greater than a predetermined value, from the actual obstacles of which the positions and the sizes are acquired by the acquisition part 11. Then, the model prediction control part 12 determines that each actual obstacle which has been specified is the actual obstacle to be avoided.
[Mathematical formula 2]
L.sub.k=g.sub.k exp(−√{square root over ((x.sub.1−x.sub.ok).sup.2+(y.sub.1−y.sub.ok).sup.2)})  (Formula 2)

(19) In the above Formula 2, a suffix k denotes a number which begins from 1 and is assigned for each actual obstacle that satisfies the above condition. x.sub.1 and y.sub.1 denote the position of the moving body (x and y coordinates), and x.sub.ok and y.sub.ok denote the position of an actual obstacle k (x and y coordinates). In addition, g.sub.k denotes a value which is decided according to the size of the actual obstacle k.

(20) Moreover, the model prediction control part 12 is configured to select three actual obstacles in descending order of values of the actual obstacle evaluation terms and determines that each of the selected actual obstacles is the actual obstacle to be avoided when there are four or more actual obstacles which satisfy the above condition. Thus, the maximum value of the reference character k is 3.

(21) In addition, the model prediction control part 12 is configured to periodically determine whether or not the moving body comes into a speed reducing state. The determination processing may determine that the moving body comes into the speed reducing state when an absolute value of a difference between a current value and a previous value of the position of the moving body is less than a prescribed position threshold value; or may determine that the moving body comes into the speed reducing state when the absolute value of the difference between a current value and a previous value of the minimum value of the calculated J value is less than a prescribed threshold value of the J value. In addition, in order to prevent erroneous detection, the above determination processing may be set to be a processing which determines that the moving body comes into the speed reducing state when the absolute value of the difference between the current value and the previous value of the position of the moving body, or the absolute value of the difference between the current value and the previous value of the minimum value of the J value, is less than a threshold value for a prescribed number of times continuously.

(22) As described above, the model prediction control part 12 periodically determines whether or not the moving body comes into the speed reducing state and periodically determines the presence/absence of the actual obstacle to be avoided, and the model prediction control part 12 also performs a processing of determining which of the first to the third conditions described below is satisfied based on both of the determination results. First condition: there is no actual obstacle to be avoided. Second condition: there are one or more actual obstacles to be avoided, and the third condition is not satisfied. Third condition: under a situation that there are a plurality of actual obstacles to be avoided, the moving body comes into a speed reducing state.

(23) Besides, when the first condition is satisfied (when there is no actual obstacle to be avoided), the model prediction control part 12 performs a first model prediction control that is a model prediction control using a standard cost L.sub.0 described below as a stage cost L.

(24) [Mathematical formula 3]

(25) $L_0=\frac{1}{2}\left({\left(x\mathrm{ref}-x\right)}^{T}Q\left(x\mathrm{ref}-x\right)+u^{T}Qu\right)$

(26) Moreover, xref denotes a target state amount at a certain time in a predicted section, x denotes a state amount at the same time, and u denotes a control input at the same time. In addition, Q denotes a coefficient (weight coefficient) indicating a weight of the state amount in the stage cost, and R denotes a coefficient (weight coefficient) indicating a weight of the control input.

(27) In addition, when the second condition is satisfied, the model prediction control part 12 performs a second model prediction control which is model prediction control using, as the stage cost L, the following value, that is, a sum of the standard cost L.sub.0 and the actual obstacle evaluation term L.sub.k of each actual obstacle.

(28) [Mathematical formula 4]

(29) $L=L_0+\underset{k=1}{\overset{\mathrm{kmax}}{.Math.}}{L}_k$

(30) Moreover, kmax in the above formula denotes the total number of the actual obstacles to be avoided (≤3 in the embodiment).

(31) In addition, when the third condition is satisfied, firstly, the model prediction control part 12 decides a position which is equidistant from the two or three actual obstacles to be avoided and has the shortest distance to each actual obstacle. Next, the model prediction control part 12 calculates a weight g.sub.p in which a value of the virtual obstacle evaluation term described below is the sum of the value of the actual obstacle evaluation term of each actual obstacle.
L.sub.p=g.sub.p exp(−√{square root over ((x.sub.1−x.sub.pk).sup.2+(y.sub.1−y.sub.pk).sup.2)})  [Mathematical formula 5]
Moreover, x.sub.pk and y.sub.pk denote the position of the virtual obstacle which is decided (x and y coordinates).

(32) Specifically, for example, when there are two actual obstacles to be avoided, the model prediction control part 12 calculates the g.sub.p according to the following formula by using values V.sub.1 and V.sub.2 of actual obstacle evaluation terms L.sub.1 and L.sub.2 at that time point (in a case in which the moving body is in the current position).
g.sub.p=(V.sub.1+V.sub.2)exp(√{square root over ((x.sub.1−x.sub.pk).sup.2+(y.sub.1−y.sub.pk).sup.2)})  [Mathematical Formula 6]

(33) Besides, the model prediction control part 12 starts a third model prediction control which is a model prediction control using a sum of the baseline cost L.sub.0 and a virtual obstacle evaluation term L.sub.p as the stage cost L.

(34) As described above, with respect to the control device 10, a capability is applied of performing the model prediction control using an evaluation function in which the obstacle evaluation term of each actual obstacle is replaced by the virtual obstacle evaluation term (the third model prediction control) when the moving body comes into the speed reducing state under the situation that there are the plurality of actual obstacles to be avoided. In this case, if the evaluation function including the obstacle evaluation term is simply used, a problem may be generated easily, for example, a problem that the moving body comes into a stationary state, or a problem that an obstacle avoidance route of the moving body makes a large detour. Besides, according to the third model prediction control, as shown by a solid line in FIG. 3, the moving body can be moved in a form that the above troubles do not occur. Moreover, in FIG. 3, obs 1 and obs 2 represent actual obstacles to be avoided, and pobs represents a virtual obstacle. In addition, x denotes a target position group which predetermines the target trajectory (a command from a PCS 32).

(35) In addition, the control device 10 is configured to treat only an actual obstacle having a large value of the obstacle evaluation term as the actual obstacle to be avoided. Thus, for example, as shown in FIG. 4, even when there are four actual obstacles obs 1 to obs 4, the actual obstacles obs 3 and obs 4 having small values of the obstacle evaluation terms are not treated as the actual obstacles to be avoided. Therefore, as shown in FIG. 5, even when there are a great number of actual obstacles (obs 1 to obs 4 in the diagram), the avoidance route of the actual obstacle can also be prevented from excessively making a large detour.

(36) <Deformation Form>

(37) With regard to the control device 10 according to the embodiment described above, various deformations can be made. For example, during the third model prediction control, the following stage cost L may be used.

(38) [Mathematical formula 7]

(39) $\begin{array}{cc}L=L_0+\underset{k=1}{\overset{\mathrm{kmax}}{.Math.}}{L}_k+L_p& \left(\mathrm{Formula}3\right)\end{array}$

(40) Moreover, when the above stage cost L is used, the g.sub.p value can be a value (for example, a value which is previously set) smaller than the above value.

(41) In addition, during the third model prediction control, a calculation may be performed in which each L.sub.k value in the above Formula 3 is set to “0”.

(42) In addition, the actual obstacle evaluation term and the virtual obstacle evaluation term may be functions in which the values become greater as the distances to the moving body become shorter (in other words, a probability of the collision with the moving body becomes higher). Thus, a function different from the above may be used as each evaluation term. In addition, in order to prevent the avoidance trajectory from excessively making a large detour (in order to shorten a period in which the moving body is away from the target trajectory), when there are two actual obstacles to be avoided, as the virtual obstacle evaluation term, a function may be used in which a change rate in an arrangement direction of the two actual obstacles is smaller than that in another direction.

(43) The control device 10 can also be deformed to a device which uses different actual obstacle evaluation terms when selecting an actual obstacle to be an avoided object and when performing actual model prediction control. In addition, it is natural that, for example, the camera 30 may be a light detection and ranging (LIDAR) or the like, and the control device 10 may be deformed to a device which controls a movement direction of a vehicle.

(44) <Additions>

(45) A control device (10), which controls a driving device (21) that moves a moving body, including:

(46) an acquisition part (11) that acquires a position of each actual obstacle which may obstruct movement of the moving body along a target trajectory; and

(47) a model prediction control part (12) that moves the moving body along the target trajectory by calculating a control input and supplying the control input to the driving device based on a prediction model, determines the presence/absence of an actual obstacle to be avoided according to information from the acquisition part, and uses a standard cost as the stage cost when there is no actual obstacle to be avoided, wherein the control input minimizes a value of an evaluation function including a sum of stage costs in a predicted section of a prescribed time width, and the prediction model is used for predicting the position of the moving body from the control input to the driving device, and wherein
the model prediction control part (12) includes
an obstacle avoidance control means that operates when there are a plurality of the actual obstacles to be avoided, decides the position of a virtual obstacle from the positions of the plurality of actual obstacles acquired by the acquisition part so as to be positioned between the plurality of actual obstacles, and calculates the control input that minimizes the evaluation function by using, as the stage cost, an addition result of the standard cost and a virtual obstacle evaluation term for which a prescribed function, which uses, as parameters, at least the position of the virtual obstacle and the position of the moving body, is multiplied by a virtual obstacle weight.