Patent Description:
In order to move a moving body without colliding with an obstacle, in an evaluation function for deciding a route of the moving body, an evaluation term for not allowing the moving body to approach the obstacle (a penalty term, hereinafter referred to as an obstacle evaluation term) is included for each obstacle (for example, see <CIT> and <CIT>).

<NPL> proposes a Nonlinear Model Predictive Control (NMPC) optimization technique which incorporates specified uncertainties into the planned trajectories. At the core of the procedure lies the propagation of model parameter uncertainty and initial state uncertainty as high-confidence ellipsoids in pose space. The quadrotor trajectories are then computed to avoid obstacles by a required safety margin, expressed as ellipsoid penetration while minimizing control effort and achieving a user-specified goal location. Combining this technique with online model identification results in robust obstacle avoidance behavior.

If the evaluation function including the obstacle evaluation term for each obstacle is used, the moving body can be moved without colliding with the obstacle. However, if the evaluation function including the obstacle evaluation term is simply used, a case in which the collision with the obstacle cannot be avoided suitably may occur, for example, a case in which the moving body comes into a stationary state, or a case in which an obstacle avoidance route of the moving body makes a large detour.

The present invention is provided by the appended claims. The following disclosure serves a better understanding of the present invention. In the following description, any embodiments referred to and not falling within the scope of the appended claims are merely examples useful for the understanding of the invention. The present invention is completed in view of the above problems, and aims to provide a control device which can suitably avoid a collision with an obstacle when the moving body is caused to follow with respect to a target trajectory by model prediction control.

In order to achieve the above aim, a control device which controls a driving device that moves a moving body according to an aspect of the present invention includes: an acquisition part that acquires a position of each actual obstacle which may obstruct movement of the moving body along a target trajectory; and a model prediction control part 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. Besides, the model prediction control part of the control device includes an obstacle avoidance control means that operates when there are a plurality of 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, the 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.

That is, the control device has a configuration in which a virtual obstacle is assumed to exist and the control input is calculated in a case in which the collision between the moving body and the obstacle cannot be avoided suitably if the evaluation function including the obstacle evaluation term is simply used (a case in which two obstacles exist in a way of clamping the target trajectory, or the like). Thus, according to the control device, even in the case in which the collision between the moving body and the obstacle cannot be avoided suitably if the evaluation function including the obstacle evaluation term is simply used, the collision between the moving body and the obstacle can be avoided suitably.

The obstacle avoidance control means may be a means that uses, as the stage cost, the 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 actual obstacle to be avoided; or may be a means that uses, as the stage cost, the addition result of the standard cost and the virtual obstacle evaluation term.

When the latter means is used as the obstacle avoidance control means, a capability of (deciding the value of the virtual obstacle weight in a way that when the operation is started, 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 actual obstacle to be avoided ) is preferably applied to the obstacle avoidance control means.

In addition, when there are two or three actual obstacles to be avoided, the obstacle avoidance control means may decide the position of the virtual obstacle to a position which is equidistant from the two or three actual obstacles are equal and has the distance shortest to each actual obstacle.

In addition, in the control device, a configuration in which (the model prediction control part further includes a determination means which determines whether or not the moving body comes into a speed reducing state, and the obstacle avoidance control means of the model prediction control part starts the operation when the moving body is determined to come into the speed reducing state under a situation that there are a plurality of actual obstacles to be avoided by the determination means) may be used. Moreover, as the determination means, for example, the following means can be used: a means which determines 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 minimum value of the evaluation function, or the absolute value of the difference between a current value and a previous value of the position of the moving body is less than a prescribed threshold value; or a means 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 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 a prescribed threshold value for a prescribed number of times continuously.

In addition, as the obstacle avoidance control means, a means may be used which uses 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 when there are two actual obstacles to be avoided.

The model prediction control part may take all of the actual obstacles of which the positions are acquired by the acquisition part as the obstacles to be avoided. In addition, when the positions of the plurality of actual obstacles are acquired by the acquisition part, the model prediction control part may calculate, for each of the plurality of actual obstacles, an evaluation value indicating a probability in which a collision with the moving body can occur, and may decide which of the plurality of actual obstacles is the actual obstacle to be avoided based on the evaluation value calculated for each actual obstacle.

According to the present invention, when the moving body is caused to follow with respect to the target trajectory by the model prediction control, a collision between the moving body and the obstacle can be avoided suitably.

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

<FIG> shows a use form of a control device <NUM> according to an embodiment of the present invention, and <FIG> shows a schematic configuration of the control device <NUM> according to the embodiment.

The control device <NUM> (<FIG>) 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 <NUM> driving a specific load device <NUM> and is generally referred to as a servo amplifier, a servo driver, or the like. Moreover, the load device <NUM> is an arm of an industrial robot, a carrying device, or the like. Hereinafter, a part including the plurality of motors <NUM> controlled by the control device <NUM> and the load device <NUM> driven by each of the motors <NUM> is referred to as a control object <NUM>.

As shown in <FIG>, the control device <NUM> is configured to function as an acquisition part <NUM> and a model prediction control part <NUM>.

The acquisition part <NUM> 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 <NUM> (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 <NUM>. Moreover, the camera <NUM> may capture images only in a prescribed range in an advancing direction of the moving body. In addition, the acquisition part <NUM> 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).

The model prediction control part <NUM> is a unit which controls, based on the position and the size of each obstacle acquired by the acquisition part <NUM> and information from the control object <NUM> (a position of the moving body and the like), the control object <NUM> 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) <NUM>.

The control performed by the model prediction control part <NUM> 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 <NUM> and based on a state x of the control object to supply to the control object <NUM>). [Mathematical formula <NUM>] <MAT> Moreover, a first term on the right side of Formula <NUM> denotes a terminal cost, and L in Formula <NUM> 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.

Herein, the model prediction control part <NUM> is configured to change the stage cost L used for the calculation of the J value depending on the situation.

Taking the above as the premise, hereinafter, a capability of the model prediction control part <NUM> is described specifically. Moreover, the control device <NUM> 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.

The model prediction control part <NUM> 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 <NUM>. More specifically, when no position and size of the actual obstacle is acquired by the acquisition part <NUM> (when there is no actual obstacle), the model prediction control part <NUM> 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 <NUM>, based on the position and the size of each actual obstacle, the model prediction control part <NUM> specifies actual obstacles, which satisfy a condition in which a value of an actual obstacle evaluation term Lk defined by the following Formula <NUM> 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 <NUM>. Then, the model prediction control part <NUM> determines that each actual obstacle which has been specified is the actual obstacle to be avoided. [Mathematical formula <NUM>] <MAT> In the above Formula <NUM>, a suffix k denotes a number which begins from <NUM> and is assigned for each actual obstacle that satisfies the above condition. x<NUM> and y<NUM> denote the position of the moving body (x and y coordinates), and xok and yok denote the position of an actual obstacle k (x and y coordinates). In addition, gk denotes a value which is decided according to the size of the actual obstacle k.

Moreover, the model prediction control part <NUM> 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 <NUM>.

In addition, the model prediction control part <NUM> 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.

As described above, the model prediction control part <NUM> 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 <NUM> 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.

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

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.

In addition, when the second condition is satisfied, the model prediction control part <NUM> 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<NUM> and the actual obstacle evaluation term Lk of each actual obstacle.

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

In addition, when the third condition is satisfied, firstly, the model prediction control part <NUM> 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 <NUM> calculates a weight gp 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. <MAT> Moreover, xpk and ypk denote the position of the virtual obstacle which is decided (x and y coordinates).

Specifically, for example, when there are two actual obstacles to be avoided, the model prediction control part <NUM> calculates the gp according to the following formula by using values V<NUM> and V<NUM> of actual obstacle evaluation terms L<NUM> and L<NUM> at that time point (in a case in which the moving body is in the current position).

Besides, the model prediction control part <NUM> starts a third model prediction control which is a model prediction control using a sum of the baseline cost L<NUM> and a virtual obstacle evaluation term Lp as the stage cost L.

As described above, with respect to the control device <NUM>, 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>, the moving body can be moved in a form that the above troubles do not occur. Moreover, in <FIG>, obs <NUM> and obs <NUM> represent actual obstacles to be avoided, and pobs represents a virtual obstacle. In addition, × denotes a target position group which predetermines the target trajectory (a command from a PCS <NUM>).

In addition, the control device <NUM> 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>, even when there are four actual obstacles obs <NUM> to obs <NUM>, the actual obstacles obs <NUM> and obs <NUM> having small values of the obstacle evaluation terms are not treated as the actual obstacles to be avoided. Therefore, as shown in <FIG>, even when there are a great number of actual obstacles (obs <NUM> to obs <NUM> in the diagram), the avoidance route of the actual obstacle can also be prevented from excessively making a large detour.

With regard to the control device <NUM> 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. [Mathematical formula <NUM>] <MAT>.

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

In addition, during the third model prediction control, a calculation may be performed in which each Lk value in the above Formula <NUM> is set to "<NUM>".

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.

Claim 1:
A control device (<NUM>) which controls a driving device that moves a moving body, comprising:
an acquisition part (<NUM>) that acquires a position of each of actual obstacles which may obstruct movement of the moving body along a target trajectory; and
a model prediction control part (<NUM>) 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 presence/absence of the actual obstacle to be avoided according to information from the acquisition part, and uses a baseline cost function, containing a weight coefficient Q indicating a weight of the state amount in a stage cost function and a weight coefficient R indicating a weight of the control input, as the stage cost function when there is no actual obstacle to be avoided, wherein the control input minimizes a value of an evaluation function comprising a sum of stage cost function in a predicted trajectory of a prescribed time width, and the prediction model is used for predicting a position of the moving body from the control input to the driving device, and
the control device (<NUM>) being characterized in that the model prediction control part (<NUM>) comprises
an obstacle avoidance control unit that operates when there are a plurality of the actual obstacles to be avoided, decides a position of an additional 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 function, an addition result of the baseline cost function and an additional obstacle evaluation term for which a prescribed function, which uses, as parameters, at least the position of the additional obstacle and the position of the moving body, is multiplied by an additional obstacle weight,
when there are two or three actual obstacles to be avoided, the obstacle avoidance control unit decides the position of the additional 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.