JP7221833B2 - Nonlinear model predictive controller - Google Patents

Description

Application of Article 30, Paragraph 2 of the Patent Law Presented at the 5th Study Group of the Research Group on Control Theory Based on Characterization of Nonlinear Phenomena held on August 31, 2018

The present invention relates to a nonlinear model predictive controller.

When controlling a system with low stability, such as a bipedal robot, model predictive control (receding horizon control) is used to perform control while predicting future system behavior (until a finite amount of time). It is effective to use Model predictive control is feedback control that solves an optimum control problem from each time to a finite time future for each control cycle (sampling cycle) to determine a control input value.

In feedback control, since the bipedal robot is a highly nonlinear system involving walking motions, etc., the control of the bipedal robot is preferably performed by nonlinear model predictive control.

Non-Patent Document 1 discloses a non-linear control model controller for a controlled object (for example, motion of a leg of a robot) assuming a collision with a surrounding object.

M. Yamakita, A. Taura, and Y. Onodera, "An application of nonlinear receding horizon control to posture control with collisions," Proceedings of International Conference on Advanced lntelligent Mechatronics, July 2005, p.1505-1510

However, it is difficult to control while predicting at what timing in the future (the next moment) a collision with a surrounding object (for example, a leg of the robot touches the ground) will occur. Therefore, in the technique described in Non-Patent Document 1, control is executed with the timing of the collision with the surrounding object set in advance as a provisional time. Therefore, there is a demand for a highly accurate nonlinear model predictive control system whose control results better match actual future collision timings.

The present invention has been made in view of the above-mentioned actual situation. The object is to provide a model predictive controller.

The nonlinear model predictive control device according to the present invention performs feedback control while calculating the optimization problem of the nonlinear control model of the controlled object, thereby controlling the controlled object while predicting the future response of the controlled object at each time. A non-linear model predictive control device configured to be able to perform the following at each time on a controlled object configured to be executed and controlled on the premise that it repeatedly collides with a surrounding object: predicting means for predicting the number of collisions for a predetermined period after the current time, and computing the optimization problem of the non-linear control model on the premise that collisions with surrounding objects will occur, based on the predicted number of collisions. and computing means for executing in response.

As described above, the present invention predicts the number of collisions of the controlled object within a predetermined period of time at each time, and calculates the optimization problem of the nonlinear control model according to the predicted number of collisions. Therefore, according to the nonlinear model predictive control device according to the present invention, highly accurate predictive control can be executed in which the control result better matches the actual collision timing in the future.

According to the present invention, it is possible to provide a highly accurate non-linear model predictive control device in which the control result better matches the future actual collision timing for a controlled object that is assumed to collide with a surrounding object.

1 is a schematic diagram showing a robot system according to a first embodiment; FIG. 1 is a functional block diagram showing the configuration of a robot system according to Embodiment 1; FIG. FIG. 4 is a diagram for explaining a method of applying the robot according to the first embodiment to a compass model; 1 is a schematic diagram showing a compass-type robot according to a first embodiment; FIG. FIG. 5 is a schematic diagram showing two states of the compass robot of FIG. 4 , a single-leg support period and a double-leg support period; 1 is a diagram showing a model according to Embodiment 1; FIG. FIG. 5 is a diagram showing a result of simulating walking control of a compass model as an example of nonlinear model predictive control according to the first embodiment; FIG. 5 is a diagram showing a result of simulating walking control of a compass model as an example of nonlinear model predictive control according to the first embodiment; FIG. 5 is a diagram showing a result of simulating walking control of a compass model as an example of nonlinear model predictive control according to the first embodiment; FIG. 5 is a diagram showing a result of simulating walking control of a compass model as an example of nonlinear model predictive control according to the first embodiment; FIG. 5 is a diagram showing a result of simulating walking control of a compass model as an example of nonlinear model predictive control according to the first embodiment; FIG. 5 is a diagram showing a result of simulating walking control of a compass model as an example of nonlinear model predictive control according to the first embodiment; FIG. 10 is a graph showing predicted collision times on the horizon as a result of simulating walking control of a compass model as an example of nonlinear model predictive control according to the first embodiment; It is a figure which shows the graph which expanded a part of FIG.

BEST MODE FOR CARRYING OUT THE INVENTION Hereinafter, embodiments of the present invention will be described with reference to the drawings. In each drawing, the same elements are denoted by the same reference numerals, and redundant description is omitted as necessary.

<Embodiment 1>
FIG. 1 is a schematic diagram showing a robot system 1 according to a first embodiment. Also, FIG. 2 is a functional block diagram showing the configuration of the robot system 1 according to the first embodiment. The robot system 1 has a robot 100 and a controller 2 that controls the motion of the robot.

The robot 100 has a torso 102 and two legs, a right leg 110R and a left leg 110L. The robot 100 is a bipedal robot capable of walking using two legs (a right leg 110R and a left leg 110L). The right leg 110R and the left leg 110L are provided below the body 102 of the robot 100. As shown in FIG. Here, as shown in FIG. 1, the forward direction of the robot 100 is the X-axis direction, and the upward direction is the Y-axis direction. Further, hereinafter, "R" is attached to the reference numerals of the components relating to the right leg 110R, and "L" is attached to the reference numerals of the components relating to the left leg 110L. R" and "L" may be omitted as appropriate.

The right leg 110R has a hip joint 120R, an upper leg 112R, a knee joint 122R, a lower leg 114R, an ankle joint 124R, and a foot 116R in this order from the body 102 side. Similarly, the left leg 110L includes a hip joint 120L, an upper leg 112L, a knee joint 122L, a lower leg 114L, an ankle joint 124L, and a foot 116L in this order from the body 102. have. A sole sensor 118 is provided at each of the bottoms of the foot portion 116R and the foot portion 116L. The sole sensor 118 detects the load applied to the bottom of the foot 116 .

The hip joint portion 120R and the hip joint portion 120L are attached to the lower portion of the body 102. As shown in FIG. Upper thighs 112R and 112L are connected to body 102 via hip joints 120R and 120L, respectively. In other words, the right leg 110R and the left leg 110L are connected to the trunk 102 via the hip joint 120R and the hip joint 120L, respectively.

Also, the upper leg 112R and the lower leg 114R are connected via the knee joint 122R. Similarly, the upper leg 112L and the lower leg 114L are connected via the knee joint 122L. Also, the lower leg portion 114R and the foot portion 116R are connected via the ankle joint portion 124R. Similarly, the lower leg portion 114L and the foot portion 116L are connected via the ankle joint portion 124L.

The hip joint 120 rotates around an axis perpendicular to the XY plane (that is, an axis horizontal to the lateral direction of the robot 100). This allows the right leg 110R and the left leg 110L to move forward and backward. Therefore, the robot 100 can perform a walking motion by alternately extending the right leg 110R and the left leg 110L forward.

The knee joint part 122 rotates around an axis perpendicular to the XY plane. As a result, the right leg 110R and the left leg 110L can bend at the knee joint portion 122. FIG. Also, the ankle joint part 124 rotates around an axis perpendicular to the XY plane. This allows the foot portion 116 to move up and down with respect to the lower leg portion 114 .

As shown in FIG. 2 , each joint (hip joint 120 , knee joint 122 and ankle joint 124 ) of the robot 100 has an angle sensor 130 and a motor 140 . The angle sensor 130 is an encoder, for example, and detects the joint angle of each joint. The motor 140 functions as an actuator that operates each joint. Each joint may also have a torque sensor 136 that detects the torque of the motor 140 of each joint. Also, a camera for detecting the surrounding conditions of the robot 100 may be built into the body 102 .

The control device 2 has a function as a computer, for example. The control device 2 may be mounted inside the robot 100 (for example, the body 102). Also, the control device 2 may be physically separated from the robot 100, and in that case, may be connected to the robot 100 via a wire or wirelessly so as to be communicable. The control device 2 controls the movements of the robot 100, particularly the movements of the right leg 110R and the left leg 110L. More specifically, the control device 2 controls the postures of the right leg 110R and the left leg 110L by controlling the torque of the motors of each joint. That is, in the robot system 1, the control device 2 functions as a master device, and the robot 100 functions as a slave device.

The control device 2 has a CPU (Central Processing Unit) 4, a ROM (Read Only Memory) 6, and a RAM (Random Access Memory) 8 as main hardware components. The CPU 4 functions as an arithmetic device that performs control processing, arithmetic processing, and the like. The ROM 6 has a function of storing control programs, arithmetic programs and the like executed by the CPU 4 . The RAM 8 has a function of temporarily storing processing data and the like.

The control device 2 also has a state acquisition unit 12, a nonlinear model predictive control unit 14, and a servo control unit 16 (hereinafter referred to as "components"). Each component can be implemented by executing a program stored in the ROM 6 by the CPU 4, for example. Moreover, each component may be realized by recording necessary programs in any non-volatile recording medium and installing them as necessary. Note that each component is not limited to being implemented by software as described above, and may be implemented by hardware such as some circuit element.

The state acquisition unit 12 functions as state acquisition means for acquiring data (state parameters) indicating the current walking state of the robot 100 . The state acquisition unit 12 acquires detection values of each sensor (the angle sensor 130, the sole sensor 118, and the torque sensor 136). The state acquisition unit 12 then outputs the acquired detection value (and the value obtained from the detection value) to the nonlinear model predictive control unit 14 . It should be noted that the "value obtained from the detected value" may be, for example, the speed of the joint angle (amount of change, time differentiation) when the "detected value" is the joint angle detected by the angle sensor 130. . In this case, the state parameter may indicate the joint angle and the velocity of the joint angle.

The nonlinear model predictive control unit 14 is a part corresponding to the nonlinear model predictive control device according to this embodiment. The nonlinear model predictive control unit 14 has a function as calculation means for calculating a control input value (input value) for controlling the motion of the robot 100 . The nonlinear model predictive control unit 14 can input at least part of the detection values (and values obtained from the detection values) from the state acquisition unit 12 as state parameters. The non-linear model predictive control unit 14 calculates a control input value for controlling the motion of the robot 100 using a model predictive control algorithm based on the state parameters. The nonlinear model predictive control unit 14 also outputs the calculated control input value to the servo control unit 16 .

Based on the state parameters, the nonlinear model predictive control unit 14 uses a nonlinear model predictive control (model predictive control) algorithm to calculate a control input value for controlling the operation of the controlled object, and the servo control unit 16 output to Details of the nonlinear model predictive control will be described later. Further, the nonlinear model predictive control unit 14 may receive necessary instruction values (step length, walking cycle, etc.) from an external host controller (not shown) of the robot system 1 . It should be noted that even if the controlled object is something other than a robot, the output destination is basically the same as that of the controlled object part.

The servo control unit 16 functions as control means for controlling the motion of the robot 100 using the control input values calculated by the nonlinear model predictive control unit 14 . The servo control unit 16 controls each joint of the robot 100 so as to obtain the calculated control input value. Also, the servo control unit 16 may have a function of a servo amplifier. Further, when performing torque control, the servo control unit 16 controls the motor 140 of each joint so that the torque of each joint (joint torque) becomes the calculated control input value. At this time, the servo control section 16 may perform feedback control using the torque values detected by the torque sensors 136 of the joints.

Next, the nonlinear model predictive control according to this embodiment will be explained.
Non-linear model predictive control is control that determines the optimal input (optimal solution of control input value) for a non-linear system up to a finite time in the future at each sampling time, and uses the initial value of the obtained input as the actual input. be. In other words, the nonlinear model predictive control unit 14 controls the controlled object while predicting the future response of the controlled object at each time by performing feedback control while calculating the optimization problem of the nonlinear control model of the controlled object. It is configured to be able to perform Nonlinear model predictive control has three advantages: it is nonlinear optimal control, it is feedback control, and it is easy to incorporate constraint conditions.

In this way, since the nonlinear model predictive control is feedback control, it is resistant to disturbances and can be combined with various constraint conditions. Because of these characteristics, nonlinear model predictive control is expected to be introduced to many systems. However, conventional iterative methods such as Newton's method have difficulty converging to the optimum solution within the sampling period.

In recent years, a new C/GMRES (continuation/generalized minimum residual method) method has been devised as an effective numerical calculation method for this problem. The C/GMRES method is an algorithm that combines the continuation method and the GMRES method. The C/GMRES method is a calculation method for tracking the optimum solution while obtaining the change rate of the optimum solution by utilizing the continuity of the optimum solution for a system in which state changes are continuous. By using this C/GMRES method, it is possible to control the system in real time even in nonlinear model predictive control. That is, by using the C/GMRES method, it has become possible to solve the optimal control problem up to a finite future time within the sampling period. Also in this embodiment, the calculation by the C/GMRES method can be applied. Note that the C/GMRES method will be described later.

The nonlinear model predictive control unit 14 according to the present embodiment has the following prediction means and calculation means as its main features. This prediction means is configured to execute and control an action (for example, walking) on the premise that collisions with surrounding objects (contact with the surrounding environment, etc.) occur repeatedly. , predicts the number of collisions from the time to a predetermined period later. This prediction means can also be called collision frequency prediction means.

The calculation means executes calculations of optimization problems of the nonlinear control model on the premise that collisions with surrounding objects may occur, according to the predicted number of collisions (separately for each predicted number of collisions). An example of control in such a nonlinear model predictive control unit 14 will be described below.

[Control object and control purpose]
Next, an example in which the nonlinear model predictive control described above is applied to control the motion of the robot 100 according to the present embodiment will be described. In Embodiment 1, an example in which robot 100 as a control object is a compass model will be described, but non-linear model predictive control can be applied even if robot 100 is not a compass model.

Note that the walking motion of the robot 100 includes a motion in which the free leg, which is a leg that is not on the ground, collides with (lands on) the ground. Before and after this collision, the generalized speed of the robot 100 changes discontinuously. That is, at this time, a state jump occurs. In general, walking motion is periodic motion. Therefore, it is possible to control the robot 100 to cause a state jump (that is, to land the free leg) at each predetermined cycle. Note that "landing" is not limited to collision (contact) of the free leg with the ground. In other words, "landing" means that the free leg comes into contact with the surface (walking surface) on which the robot 100 is walking.

FIG. 3 is a diagram for explaining a method of applying the robot 100 according to the first embodiment to a compass model. In the example shown in FIG. 3, the right leg 110R is the leg supporting leg that is in contact with the ground 90 while being supported by one leg, and the left leg 110L is the free leg (swing leg). In order to simulate that the supporting leg is in point contact with the ground, the control device 2 uses the torque sensor 136 provided at the ankle joint portion 124 of the supporting leg (the right leg 110R in the example of FIG. 3) to The torque of the leg ankle joint 124 is controlled to zero. The control device 2 also controls the knee joints 122 of the right leg 110R and the left leg 110L to be locked in the extended state. That is, the control device 2 controls the motors 140 of the knee joints 122 so that the joint angles of the knee joints 122 of the right leg 110R and the left leg 110L are the angles corresponding to the extended state (for example, 0). Further, the control device 2 uses the sole sensor 118 of the free leg (the left leg 110L in the example of FIG. 3) to detect the landing of the free leg. Thus, the robot system 1 can simulate a compass model.

FIG. 4 is a diagram for explaining an example in which the robot 100 according to the first embodiment is applied to a compass-type model, and is a schematic diagram showing the compass-type robot. A robot (biped walking robot) 100 exemplified in FIG. 4 is modeled as a compass model including joints 150 , supporting leg links 151 and free leg links 152 . Here, joint 150 corresponds to body 102 and hip joint 120 . Also, the supporting leg link 151 corresponds to the supporting leg of the right leg 110R and the left leg 110L. Also, the free leg link 152 corresponds to the free leg of the right leg 110R and the left leg 110L.

Assume that the mass of the joint 150 is m0. Also, as indicated by the arrow in FIG. 4, an input torque u is input around the joint 150 as a control input value. Here, it is assumed that the supporting leg link 151 and the swing leg link 152 have the same physical properties. Let l be the length of the support leg link 151 and the swing leg link 152 . Also, the mass of the supporting leg link 151 and the free leg link 152 is m. Let d be the length from the joint 150 to the center of gravity of each link (center of gravity 151 m and center of gravity 152 m). Let I be the moment of inertia around the center of gravity of the support leg link 151 and the free leg link 152 (the center of gravity 151m and the center of gravity 152m).

The angle of the supporting leg link 151 with respect to the vertical direction is θ1, and the angle of the free leg link 152 with respect to the vertical direction is θ2. However, in FIG. 4, the clockwise direction (the direction in which the joint 150 rotates forward about the lower end of each link) is positive. Therefore, in the state of FIG. 4, θ2<0.

The non-linear model predictive control unit 14 controls the most simplified model (compass model) of the bipedal robot 100, which can be exemplified by the robot 100 in FIG. 1 and is represented in FIG. . This control is real-time dynamic walking control using nonlinear model predictive control.

FIG. 5 is a schematic diagram showing two states of the compass robot, one-leg support period and two-leg support period. In the motion of the robot 100 shown in FIG. 4, the robot takes two states like the robots 100-1 and 100-2 shown in FIG. One state illustrated by robot 100-1 is when one leg is on the ground 90 and the other leg is off the ground 90 (single leg support phase). Another state shown by robot 100-2 is a state in which both legs are on the ground 90 (both legs support phase). Hereinafter, the leg that is in contact with the ground 90 during the one-leg support period will be referred to as the supporting leg, and the leg that is not in contact with the ground 90 during the one-leg support period will be referred to as the free leg.

Moreover, the following four assumptions are made about walking motion.
- The collision between the free leg and the ground 90 is assumed to be a completely inelastic collision. That is, the leg that collides with the ground 90 does not rebound.
• Immediately after impact, the previously supporting leg leaves the ground 90 without interaction.
・The collision between the free leg and the ground 90 is instantaneous. That is, the two-leg support period is assumed to be instantaneous.
・It is assumed that the speed of the robot changes instantaneously due to the impact force of the collision, but the coordinates do not change instantaneously.

Based on these assumptions, the dynamics of the walking motion of the robot can be divided into two phenomena: a continuous change of movement during the one-leg support period, and a discontinuous change of collision between the free leg and the ground 90 . We then derive two equations based on these assumptions.

ととると、ラグランジュの運動方程式が、

と得られる。 First, the equation of motion in the single-leg support phase is the generalized coordinate q as

Then, Lagrange's equation of motion is

is obtained.

Here, τ(u) is the control input torque to the joint, M(q) is the inertia matrix, H(q, q(dot)) is the term representing the force of gravity and the Coriolis force. As shown in the equation.

とすると、制御に用いるための状態方程式が

と得られる。 From this, the equation of motion of the robot and the state space vector x are

Then the state equation for control is

is obtained.

In the compass model described here, ZMP (zero moment point) is not taken into consideration as a constraint condition for walking.

Next, we derive the collision formula. For the sake of simplicity, in this embodiment, the coordinates are recalculated as θ ₁ =θ ₂ after the collision (however, the physical meaning does not change at all). Based on the assumption that the physical coordinates do not change upon collision, the coordinates immediately before the collision are q ⁻ = [θ ₁ ⁻ θ ₂ ⁻ ] ^T , and the coordinates immediately after the collision are q ⁺ = [θ ₁ ⁺ θ ₂ ⁺ ] ^T. and put the square matrix by I(bar) in the following equation.

Then, the following formula holds.

については、遊脚についての角運動量保存則から次式が成り立つ。 The speed immediately before collision and the speed immediately after collision are expressed by the following equations, respectively.

, the following equation holds from the law of conservation of angular momentum for the swing leg.

However, Q ⁻ and Q ⁺ are as shown in the following formulas.

と表すことができる。また、衝突が起きる条件としては遊脚の先端の地面９０からの高さが０になること、すなわち
ψ（ｘ）＝ｌ（ｃｏｓθ_１－ｃｏｓθ_２）＝０ ・・・（Ａ１４）
である。 From the above, the change in the state of the compass robot before and after the collision can be calculated using the coordinates x ⁻ and x ⁺ immediately before and after the collision

It can be expressed as. Also, as a condition for collision to occur, the height of the tip of the free leg from the ground 90 should be 0, that is, ψ(x)=l(cos θ ₁ -cos θ ₂ )=0 (A14).
is.

As can be seen from the above description, the compass-type walking model exemplified in the present embodiment can be represented by a general model as shown in FIG. where f _k (x(t), u(t)) is the state equation of the compass model at the k-th step, ψ _k (x) is the condition for the k-th step collision, and γ _k (x) is k Represents a discontinuous change in state due to a step collision. In addition, this model can represent general models in which a robot or the like makes contact with the outside world in addition to walking.

[Non-linear model predictive control and real-time control algorithm by C/GMRES method]
Next, we derive the formulation of the optimal control problem when applying nonlinear model predictive control to such a generalized model for each number of collisions on the prediction horizon. A real-time control algorithm executed in .

Here, a method of applying non-linear model predictive control to the generalized model of the walking motion shown in FIG. 6 will be described. Nonlinear model predictive control (hereinafter referred to as NMPC) is controlled based on a model of the system to be controlled and the state of the system at the current time t. In this embodiment, the model is the state equation x(dot)=fk(x, u) and the collision equation x+=γk(x−). Specifically, based on the above model and the state at current time t, NMPC selects the control input uOPT(τ) (t≤τ≤t+T) that optimizes the behavior of the system from current time t to future t+T. This is the desired control law. NMPC is a control law that gives the control input u(t) to the actual system as the initial value of the obtained optimal control input, that is, u(t)=uopt(τ).

*Derivation of optimality condition*
Here, when the system includes discontinuous phenomena such as collisions, it is necessary to derive optimality conditions accordingly. Therefore, here, an optimality condition is derived according to the number of collisions on the horizon.

In a system with state equation switching as in FIG. 6, when state xεR ⁿ is governed by state equation f _k (x, u), state x is governed by subsystem k, and subsystem k is active.

に従い、ある条件ψ_ｋ（ｘ）∈Ｒ^ｎについて
ψ_ｋ（ｘ（ｔ_ｋ－））＝０ ・・・（Ａ１６）
を満たすことで、状態ジャンプ（状態の不連続変化）
ｘ（ｔ_ｋ＋）＝γ_ｋ（ｘ（ｔ_ｋ－）） ・・・（Ａ１７）
が起きる。 That is, the model in FIG. 6 states that when subsystem k is active, state xεR ⁿ is

ψ _k (x(t _k −))=0 for some condition ψ _k (x)∈R ⁿ (A16)
By satisfying the state jump (discontinuous change of state)
x(t _k +)=γ _k (x(t _k −)) (A17)
happens.

に支配される。 Also, the active subsystem switches from subsystem k to subsystem k+1 and the state is

dominated by

When controlling this model by NMPC, it is necessary to derive optimality conditions and necessary conditions for optimal control and solve them numerically. On the other hand, when the system contains discontinuous events such as collisions, the optimal control requirements change depending on the number of collisions on the horizon. So here we have three scenarios: (Scene 1) no collision on the horizon, (Scene 2) one collision on the horizon, (Scene 3) multiple collisions on the horizon. , respectively.

That is, as described above, the nonlinear model predictive control unit 14 performs calculations for the optimization problem of the nonlinear control model on the assumption that a collision with surrounding objects may occur. will be performed. Specifically, as described in the following (Scene 1) to (Scene 3), the optimization problem is calculated for each of the cases from the case where there is no collision to the case where collision occurs multiple times (generally, the case where collision may occur).

が最小になるようなホライゾン上の最適制御入力ｕ_ＯＰＴ（τ）（ｔ≦τ≦ｔ＋Τ）を求める。そして、ＮＭＰＣでは、実際のシステムヘの制御入力をｕ（ｔ）＝ｕ_ＯＰＴ（ｔ）として与える。 (Scene 1) When no collision occurs on the horizon When no collision occurs on the horizon, NMPC uses the system model (A15) and the current system state x(t) as the evaluation function

Optimal control input u _OPT (τ) (t≦τ≦t+T) on the horizon that minimizes . Then, in NMPC, the control input to the actual system is given as u(t)=u _OPT (t).

However, L _k is the stage cost function assigned to subsystem k, φ _k is the terminal cost assigned to subsystem k and thereafter the variable τ (0 ≤ τ ≤ T) is treated as a time parameter on the evaluation interval, and t is treated as a constant parameter. Also, x ^* (τ; t) and u ^* (τ; t) are treated as corresponding to x(t+τ) and u(t+τ), respectively.

のもとで、評価関数

を最小にするような最適制御入力ｕ^＊（τ；ｔ）（０≦τ≦Τ）を求める問題となる。 At this time, the optimal control problem of NMPC is

Under the evaluation function

The problem is to find the optimum control input u ^* (τ; t) (0≦τ≦T) that minimizes .

Also, the control input to the actual system is
u(t)=u ^* (0;t) (A23)
given as

を最小にするような制御入力ｕ_i ^＊（ｔ）、ｉ＝０，・・・，Ｎ－１を求める問題になる。但し、Δτ＝Τ／Ｎであり、ｘ_i ^＊（ｔ）、ｕ_i ^＊（ｔ）はそれぞれｘ_i ^＊（ｉΔτ；ｔ）、ｕ_i ^＊（ｉΔτ；ｔ）に相当する値である。 Next, in order to numerically solve this nonlinear optimal control problem, the optimal control problem is discretized. When the horizon T is divided into N, the optimal control problem to be solved is
_x0 ^* (t)=x(t) (A24)
x _{i +1} ^* (t) = x _i ^* (t) + _fk (x _i ^* (t), u _i ^* (t)) Δτ,
i=0,...,N-1
... (A25)
Under the evaluation function

The problem is to find control inputs u _i ^* (t), i=0, . . . , N−1 that minimize . However, Δτ=T/N, and x _i ^* (t) and u _i ^* (t) are values corresponding to x _i ^* (iΔτ; t) and u _i ^* (iΔτ; t), respectively.

が得られる。但し、Ｈ_ｋ（ｘ，λ，ｕ）はサブシステムｋに対するハミルトニアン、
Ｈ_ｋ（ｘ，ｕ，λ）＝Ｌ_ｋ（ｘ，ｕ）＋λ^Ｔｆ_ｋ（ｘ，ｕ） ・・・（Ａ３０）
である。 Deriving the stationary and optimality conditions for this problem yields

is obtained. where H _k (x, λ, u) is the Hamiltonian for subsystem k,
_Hk (x,u,λ ₎ = _Lk (x,u)+ ^λTfk (x,u) (A30)
is.

_{At this} time, the optimal ^{control problem} is the unknown quantity x ₀ ^* (t), . It becomes a problem to find u _N−1 ^* (t), λ ₀ ^* (t), . . . , λ _N ^* (t). If u ₀ ^* (t),..., u _N-1 ^* (t) are known, x ₀ ^* (t),..., x _N ^* (t), λ ₀ ^* (t), . . , λ _N ^* (t) can be calculated from (A24)−(A28).

として定義する。 Therefore, U(t) as an essential unknown quantity is

defined as

となる。 Then the vector constructed of optimality conditions for U(t) is

becomes.

と予測することができる。 (Scene 2) When one collision occurs on the horizon Next, the optimality condition is derived for the case where one collision occurs on the horizon. Suppose a collision occurs at time τ _k on the horizon, then for the state

can be predicted.

を最小にするような最適制御入力ｕ^＊（τ；ｔ）（０≦τ≦Τ）を求める問題となる。 Then the optimal control problem is expressed under the equations for these states as

The problem is to find the optimum control input u ^* (τ; t) (0≦τ≦T) that minimizes .

として行うことができる。ここで、Δτ_ｋ１＝τ_ｋ－ｉ_ｋΔτ、Δτ_ｋ２＝Δτ－Δτ_ｋ１である。 Next, the optimality condition is obtained by discretizing on the horizon in the same way as when collisions do not occur on the horizon. First, the step i _k at which collision occurs is i _k Δτ≦τ _k ≦(i _k +1)Δτ (A38)
, the state prediction is

can be done as where Δτ _k1 =τ _k −i _k Δτ and Δτ _k2 =Δτ−Δτ _k1 .

と離散化される。 Also, the initial value of the state on the horizon is x ₀ ^* (t)=x(t) (A45)
given as The evaluation function J is

and discretized.

At this time, the optimal control problem is a sequence of control inputs u _i ^* (t) (i=0, . . . , N− 1), which reduces to the problem of finding a control input that yields u ^* (τ _k +; t). To derive the optimality condition for this problem, we introduce the Lagrangian multiplier νεR ^l for the collision condition (A41). Then, the optimality condition is obtained as follows.

At this time, the optimal control problem is x ₀ ^* (t), . . . , x _ik ^* (t), x ^* (τ _k − _; t), x ^* (τ _k +; t), x _ik+1 ^* (t), . . . , x _N ^* (t), λ ₀ ^{* (} t), . , λ ^* (τ _k −; t), λ ^* (τ _k +; t), λ _ik ^* (t), ..., λ _N ^* (t), u ₀ ^* (t), ..., In _the problem of finding u _ik ^* (t), u ^* (τ _k ^{+ ;} t), u _{ik +1} ^* ( _t ) _, . be returned.

Among these unknown quantities, essential unknown quantities are defined as follows.

However, U _k (t) is defined as follows.

By such definition, if this U(t) is known, x ₀ ^* (t ⁾ , . . . , x _ik ^* (t), x ^* (τ _{k −} ; ; t), x _ik+1 ^* (t), ..., x _N ^* (t), λ ₀ ^* (t), ..., λ _ik ^* (t), ..., λ ^* (τ _k - ; t), λ ^* (τ _k +; t), λ _ik ^* (t), ..., λ _N ^* (t) from (A39) - (A45), (A47) - (A52) can be done.

と得られる。 The condition that U(t) should satisfy is

is obtained.

However, F _k (U _k (t), t) is represented by the following equation.

(Scene 3) When multiple collisions occur on the horizon The optimality condition when multiple collisions occur on the horizon can be obtained by extending the optimality condition when the number of collisions is one. Let the k, . . . , k ₊ l-th collision occur at times τ _k , . ^At _this time, for the _steps ⁱ _{k ,} . ), x _ik+1 ^* (t) can be determined for k=k, . . . , k+l. Similarly, for λ, λ ^* (τ _k −; t), λ ^* (τ _k +; t), λ _ik ^* (t) from (A49)-(A51) to each i _k , . . . , i _k+l can be determined. For _all i _{except i k , .}

として定義することができる。 In this case, the essential unknown quantity is

can be defined as

At this time, the conditions that U(t) should satisfy are as follows.

の代わりに

を代用する。 where F _k is (A61) and for each i _k step of the elements of F,

Instead of

substitute.

*Execution algorithm for non-linear model predictive control*
In order to perform nonlinear model predictive control, it is necessary to numerically solve the optimality condition obtained as described above. In this embodiment, the C/GMRES method, which is a high-speed numerical calculation algorithm of NMPC, is used in order to control a complicated system such as a robot even within a short sampling period.

**C/GMRES method**
The C/GMRES method is a method combining the continuous deformation method and the GMRES method. The C/GMRES method does not directly solve the nonlinear equation F(U(t), x(t), t)=0 to obtain U(t) directly. In the C/GMRES method, on the assumption that U(t) is continuous with respect to time, the optimum U(t) is obtained at each sampling time,
U(t+Δt)=U(t)+U(t)Δt (A64)
Update as However, Δt is the sampling period.

として変形する。ここで、ζ＞０は安定化パラメータである。この方程式は

についての線形方程式としてみなすことができる。そこで、この方程式を線形方程式の高速数値解法であるＧＭＲＥＳ法を用いて解く。以上がＣ／ＧＭＲＥＳ法の簡単な説明である。 In the C/GMRES method, the continuous deformation method is used to obtain U(t), and F=0

transforms as where ζ>0 is a stabilization parameter. This equation is

can be viewed as a linear equation for Therefore, this equation is solved using the GMRES method, which is a high-speed numerical solution method for linear equations. The above is a brief description of the C/GMRES method.

**Extension of the C/GMRES method to collision phenomena**
The C/GMRES method assumes continuity of U(t). On the other hand, in the C/GMRES method, a problem for a system including a collision phenomenon is accompanied by a discontinuity phenomenon, and the update by (A64) may not hold the premise of continuity of the solution. In order for this continuity to hold, the control input assigned to a subsystem k on the horizon needs to be updated by the control input assigned to a similar subsystem.

として更新する。 That is, for any collision k over the horizon, if τ _k (t+Δt)<i _k Δτ, then u _i ^* (t+Δt) for each i with τ _k (t+Δt)/Δτ<i≦i _k is

Update as

として更新する。 Similarly, when (i _k +1)Δτ<τ _k (t+Δt), for each i that satisfies i _k <i<τ _k (t+Δt)/Δτ, u _i ^* (t+Δt) is

Update as

として前進差分近似される。 Another modification is forward difference approximation of Jacobian matrices and vectors. Using some vector WεR ^mN , wεR ⁿ , ωεR, and a sufficiently small real number h>0, the Jacobian matrix of equation (A65) is

is a forward difference approximation as

This difference approximation also assumes continuity for the subsystems on the prediction horizon at times t and t+h. On the other hand,
i _k Δτ≦τ _k (t)<i _k Δτ+h (A69)
When , this continuity ceases to hold.

として後退差分近似を行う。但し、ｈを十分小さくしておけば、（Ａ６９）はほとんど起こらない。 So in such a case

to perform backward difference approximation. However, if h is sufficiently small, (A69) hardly occurs.

** Initialization Algorithm for Variables Added by Switches **
Next, we describe the initialization algorithm for variables added by collisions.
If switch k is not on the horizon at time t−Δt, but it is at time t, ie switch k appears on the horizon at time t, then: That is, in such a case, new variables u ^* (τ _k +; t), ν _k ^* , τ _k that were not included in the optimal control problem at time t−Δt are added at time t. It will be. At this time, the new variables u ^* (τ _k +; t), ν _k ^* , and τ _k have not been obtained at the previous time, so they cannot be obtained by the C/GMRES method. Also _, when switch k occurs at i _k before N−1 steps, u _ik ^{* (} t), . We need to re-determine optimally for system k+1. However, as long as the sampling period is sufficiently small, i _k =N−1 in most cases, and the only variables to be initialized at this time are u ^* (τ _k +; t), ν _k ^* , and τ _k . OK. At this time, originally, it is necessary to solve the optimum control problem once again using Newton's method or the like for the entire horizon, but there is a problem that the calculation time becomes enormous. Therefore, it is preferable to adopt a method of initializing these values by obtaining partially optimum values in a short calculation time. Such techniques are described below.

を観測したときを考える。このとき、部分的に初期化を行うためにまずｉ_ｋステップ目までの制御入力、状態の系列は最適であると仮定する。すなわち、ｉ_ｋステップ目以降での最適な変数を求める問題を定義し解くことでこの追加された変数の初期化を行う。以降では、ｉ_ｋ＝Ｎ－１のとき、ｉ_ｋ＜Ｎ－１の２通りに分けて記述を行う。 First, let us assume that we observe that the collision condition ψ=0 is satisfied on the horizon. i.e. for the first time at some step

Consider when you observe At this time, it is assumed that the sequence of control inputs and states up to the ik- _th step is optimal for partial initialization. That is, the added variables are initialized by defining and solving the problem of finding the optimum variables after the i _kth step. In the following, when i _k =N−1, two cases of i _k <N−1 are described.

が計算できる。 When i _k =N−1:
Then, if τ _k , u ^* (τ _k +; t), ν _k ^* are known, using the already calculated x _N−1 ^* (t), u _N−1 ^* (t), ,

can be calculated.

を最小にするようなｕ^＊（τ_ｋ＋；ｔ）、ν_ｋ ^＊、τ_ｋを求める問題である。 The optimal control problem to be solved here is the evaluation function after the N-1 step under (A72)-(A75)

The problem is to find u ^* (τ _k +; t), ν _k ^* , τ _k that minimizes .

が得られる。 Deriving the optimality condition for this problem,

is obtained.

として与える。次に、ｕ^＊（τ_ｋ＋；ｔ）とν_ｋ ^＊（ｔ）とについて、ニュートン法を行うための適当な初期推定解を与える。 This initialization is performed as follows. Here, since ψ _k (x _i ^* (t)) < 0 for the first time on the horizon is considered when i=N, first τ _k is changed to ψ _k (x _N−1 ^* (t)) and ψ _k (x _N ^* (t)) from

give as Next, for u ^* (τ _k +; t) and ν _k ^* (t), we give a suitable initial guess solution for performing Newton's method.

とする。 And the unknown quantity we want to find is

and

とする。 Also, the condition that U _k,init (t) should satisfy is

and

The forward difference Newtonian GMRES method iterates over the unknown quantity to be determined and the conditions that U _k,init (t) should satisfy. Algorithm 1 below summarizes this initialization for this U _k,init (t).

[Algorithm 1: Initialization of U _k,init (t) (when i _k =N−1)]
1: Initialize τ _k by (A98).
2: Substitute suitable initial guesses for u ^* (τ _k +; t) and ν _k ^* (t).
3: x ^* (τ _k −; t), x ^* (τ _k +; t), x _N ^* (t), λ _N ^* (t), λ ^* (τ _k +; t), F _{k , init} (U _{k, init} (t), t).
4: while |F _{k, init} (U _{k, init} (t), t) |<τ _init or i<i _max do
5: Obtain ΔU _k,init by using the forward difference Newton-GMRES method on F _k,init (U _k,init (t),t).
6: Update U _k,init (t) as U _k,init (t)←U _k,init (t)+ΔU _k,init .
7: x ^* (τ _k −; t), x ^* (τ _k +; t), x _N ^* (t), λ _N ^* (t), λ ^* (τ _k +; t), F _{k , init} (U _{k, init} (t), t).
8: end while
9: End of initialization of U _k,init (t).

のもとで、

を最小にするようなｕ^＊（τ_ｋ＋；ｔ）、ｕ_ｉｋ＋１ ^＊（ｔ），・・・，ｕ_Ｎ－１ ^＊（ｔ）、ν_ｋ ^＊、τ_ｋを求める問題である。 When i _k <N−1:
The optimal control problem to be solved at this time is

under the

The problem is to find ^{u * (} τ _{k +} ; t), u _{ik+ 1} ^* ( _t ), .

The optimality condition for this problem is derived as follows.

として与える。 As with i _k =N−1, x _ik ^* (t), u _ik ^* (t) are given as boundary conditions for this initialization problem. Performing this initialization is the same as when i _k =N−1. First, τ _k is derived from ψ _k (x _ik ^* (t)) and ψ _k (x _ik+1 ^* (t)) as

give as

とする。 Next, we give u(τ _k +; t) and ν _k ^* (t) suitable initial guess solutions. For u _{ik +1} ^* (t), ^. the desired unknown quantity

and

となる。 The condition that U _k,init (t) should satisfy is

becomes.

The forward difference Newtonian GMRES method iterates over the unknown quantity to be determined and the conditions that U _k,init (t) should satisfy. Algorithm 2 below summarizes this initialization for this U _k,init (t).

[Algorithm 2: Initialization of U _k,init (t) (when i _k < N−1)]
1: Initialize τ _k by (A98).
2: Substitute suitable initial guesses for u ^* (τ _k +; t) and ν _k ^* (t).
3: x ^* ( _τk- ;t), x ^* ( _τk +;t), _xik+1 ^* (t), ..., _xN ^* (t), _λN ^* (t), ... , λ _ik+2 ^* (t), λ ^* (τ _k +; t) to calculate F _k,init .
4: while |F _{k, init} (U(t), x(t), t) |<τ _init or i<i _max do
5: Obtain ΔU _k, init by using the forward difference Newton-GMRES method on F _k,init (U(t), x(t), t).
6: Update U _k,init (t) as U _k,init (t)←U _k,init (t)+ΔU _k,init .
7: x ^* (τ _k −; t), x ^* (τ _k +; t), x _ik+1 ^* (t), ..., x _N ^* (t), λ _N ^* (t), ... , λ _ik+2 ^* (t), λ ^* (τ _k +; t) to calculate F _k,init .
8: end while
9: End of initialization of U _k,init (t).

[Numerical simulation]
Next, the result of numerical simulation of the walking control of the compass type model using the above-described method will be shown. The simulation described below applies the nonlinear model predictive control algorithm according to the present embodiment to the robot 100 according to the compass model illustrated in FIG.

を、適当な目標値ν_ｒｅｆに近づけるような項

を評価関数に加える。また、その際に遊脚が地面から高く上がりすぎて非効率な穂動きをしないようｑ_２（θ_１＋θ_２）^２も加える。最後に、使用エネルギーの少ない自然な歩行を実現するため、ｒｕ^２を加える。 *Evaluation function*
First, an evaluation function for use in compass-type walking control is set. In order to realize continuous walking control, we consider adding the action of swinging the leg forward as an evaluation function. In other words, the speed at which the free leg moves forward

to a suitable target value ν _ref

is added to the evaluation function. At that time, q ₂ (θ ₁ +θ ₂ ) ² is also added so that the free leg does not rise too high from the ground and move inefficiently. Finally, ru ² is added to achieve natural walking with less energy consumption.

となる。また、本実施の形態では、終端コストについてφ（ｘ）＝０としている。 From the above, the evaluation function is

becomes. Also, in the present embodiment, φ(x)=0 for the termination cost.

*Simulation conditions*
The physical parameters of the compass model used in the simulation are m ₀ =m=1.0 [kg], l=1.0 [m], d=0.5 [m], I=0.08333 [kg· m ² ].

As simulation conditions, the initial value of the state based on (A6) is x(0)=[−0.14 0.14 0.50 0.58] ^T , and the model state update and sampling period during simulation is Δt = 0.001 [s]. The parameters in the evaluation function are q ₁ =q ₂ =1.0, r=0.5, v _ref =0.5 [rad/s]. The length of the horizon is T(t)=T _f (1−e ^−αt ), α=1.0, T _f =0.8 [s], and the division number of the evaluation interval is N=80. Also, the difference between the difference approximations (A68) and (A70) is given as h=1.0×10 ⁻⁸ .

*simulation result*
7 to 12 are diagrams showing results of simulating walking control of a compass model as an example of nonlinear model predictive control according to the present embodiment. FIG. 13 is a diagram showing a graph of predicted collision times on the horizon in the walking control, and FIG. 14 is a graph showing a part of FIG. 13 enlarged.

Change in θ ₁ in FIG. 7, change in θ ₂ in FIG. 8, change in θ ₁ (dot) in FIG. 9, change in θ ₂ (dot) in FIG. 10, change in u in FIG. FIG. 12 shows changes in ||F||.

||F|| (error norm) in FIG. Represents the error of the current solution from the solution. There is a point where ||F|| is larger than other points. This coincides with the time when θ ₁ and θ ₂ are vertical, that is, the time when the actual controlled object collides with the ground. Therefore, it is considered that this occurred because the collision changed the optimality condition on the horizon (the optimality condition described for Scenes 1 to 3). Considering this point, it can be seen that the simulation results shown in FIGS. 7 to 12 show smooth behavior of the control input.

FIG. 13 shows that no collision is detected in the evaluation section at the time when t _k =0. Also, in the evaluation section length set in this simulation, only one collision occurred on the horizon. In FIG. 14, it can be seen that the collision time on the horizon is optimized at each time. Also, in this simulation performed with a sampling period of 1 [ms], the update time per sampling of NMPC is around 0.8 [ms], and it can be seen that walking control in real time is successful.

[Features of this embodiment]
As described above, in this embodiment, as one of its main features, the prediction means predicts the number of collisions for the controlled object at each time until a predetermined period from that time.

A brief supplementary explanation of such prediction of the number of future collisions will be given. This prediction is executed for each control cycle, and in the optimum control as described above, the state at each time in the future (within the prediction interval) is also predicted in the process of finding the solution. Therefore, by applying (A33) to (A36) to the future state predicted by the previous optimum control before executing the current optimum control, it is possible to predict the number of future collisions. . In addition, as described above, this prediction is executed for each control cycle, so if the length of the prediction interval (horizon) is constant, the more the walking speed increases, the more collisions are predicted. This will reduce the number of collisions that occur.

Then, as described above, in the present embodiment, the computing means computes the optimization problem of the nonlinear control model on the premise that collisions with surrounding objects may occur, depending on the predicted number of collisions ( (separately for each predicted number of collisions). A brief supplementary explanation of this calculation will be given.

In this embodiment, the collision time is incorporated into the optimal control problem. That is, in the present embodiment, as exemplified for the control for each scene 1 to 3 in "Derivation of the optimality condition", which of the scenes 1 to 3 is used based on the prediction of the number of collisions (which optimal control The optimal control problem is switched by using the necessary condition of If the collision time is not included in the optimization problem, i.e., if the collision time is not changed, it is possible to formulate so that the collision occurs at a predetermined collision time and control using the C/GMRES method as usual. can.

However, if the collision time is simply incorporated into the optimum control problem, the optimum control problem to be solved will change depending on the number of collisions. can be difficult. This is because if the number of collisions differs between the previous and current control cycles, the optimal control problem solved last time is different from the optimal control problem to be solved this time. Specifically, since the optimal control problem itself changes when the number of collisions changes, the solution vector will also have a different shape. For example, the current optimization problem includes variables that were not included in the solution vector in the previous optimization problem, such as the Lagrangian constant v and the collision time τ for the collision condition.

Therefore, as described in the present embodiment, it is desirable not only to switch the optimality condition, but also to initialize the newly added control variables associated with this switching. This initialization is to initialize the solution vector based on the previous solution finding result. As exemplified in the "initialization algorithm for variables added by the switch", the control variables newly added along with this switching refer to u ^* (τ _k +; t), ν _k ^* , τ _k , for example. .

Thus, in the present embodiment, the optimum control problem is calculated based on the prediction of the number of collisions, and the solution of the previously solved optimum control problem is used to appropriately set the initial value of the current optimum control problem. is preferred.

As described above, the present embodiment predicts the number of collisions of the controlled object within a predetermined period of time at each time, and calculates the optimization problem of the nonlinear control model according to the predicted number of collisions. there is That is, in the present embodiment, on the premise that repeated collisions occur, the nonlinear control model is optimized by changing it for each number of collisions that occur within the same period, for example.

Therefore, according to the present embodiment, the probability that the difference between the predicted timing and the actual timing in each collision becomes small increases, and statistically speaking, the difference between the predicted timing and the actual timing decreases. As a result, the non-linear model predictive control device according to the present embodiment can execute highly accurate predictive control in which the control result better matches the actual collision timing in the future.

In other words, in the present embodiment, formulation is performed according to the number of collisions on the prediction interval of the optimum control, and the control input related to the control of the motion and the collision time are optimized at the same time. Adaptive and optimal behavior can be generated accordingly. In such an optimal control method, the optimal control problem is formulated in a way that corresponds to switching at an arbitrary time. It can be said that it is a method to simultaneously optimize the control inputs of

Therefore, according to the present embodiment, appropriate switching timing and control input can be generated according to the state, so that more efficient and stable operation of the controlled object can be realized. For example, if the switching timing is determined in advance, it becomes difficult to satisfy the optimality of the control. Otherwise, an input that increases power is required. On the other hand, in the present embodiment, it is possible to generate an appropriate switching timing, thereby satisfying the optimality of control. For example, taking walking as an example, when a disturbance occurs and the state deviates from the normal walking motion, not only the motion trajectory but also the timing at which the next foot touches the ground can be changed in a natural way (with less torque). ) can be returned to a steady state.

(Modification)
It should be noted that the present invention is not limited to the above embodiments, and can be modified as appropriate without departing from the scope of the invention. For example, one leg of the robot 100 has the hip joint 120, the knee joint 122, and the ankle joint 124, but the configuration is not limited to this. The legs of the robot 100 may have less than three joints or more than three joints. In this case, the state vector and the joint torque vector (control input value) can be appropriately changed according to the number of joints. Functions such as state equations can also be changed as appropriate according to the number of joints.

Also, in the above-described embodiments, an example in which the nonlinear system is a bipedal robot has been described. However, the non-linear model predictive control algorithm according to the present embodiment can also be applied to non-linear systems other than bipedal robots, and can also be applied to non-compass models.

That is, the nonlinear system control method according to the present embodiment can be applied to any nonlinear system assuming repeated collisions with surrounding objects, such as those exemplified below. It should be noted that even for a controlled object that at first glance seems unlikely to repeatedly collide with surrounding objects, it is possible to intentionally perform control in such a way that collisions with surrounding objects can occur repeatedly. The form of is also useful for such control.

For example, a robot hand or a robot arm provided on the robot can be controlled. Collisions in this example can occur when a robotic hand or arm presses an object such as the environment or an object to be manipulated, grasps or releases an object, hits or hits an object such as a sphere, and so on. An example of a non-linear system that hits back an object such as a sphere is a table tennis robot.

Further, for example, the object to be controlled in the present embodiment may be a humanoid robot, an animal robot, or the like that can move by landing its arms and legs on the floor at the same time. Collisions in this example are when the humanoid or animal robot moves with its arms and legs simultaneously contacting a wall, floor, table, etc., or when the humanoid or animal robot moves against a ladder or wall. It can occur, for example, when climbing etc.

Further, for example, the controlled object in the present embodiment may be an unmanned aerial vehicle such as a drone. A collision in this example may occur when the unmanned aerial vehicle contacts or leaves an object to be manipulated or inspected, grasps or releases an object to be transported or captured, and the like.

Further, for example, the object to be controlled in the present embodiment may be a tool of a processing machine or the like. Collisions in this example can occur, such as when a tool of a machine tool touches or leaves an object, such as a workpiece.

Further, for example, the controlled object in the present embodiment may be a transmission of an automobile or the like. A collision in this example can occur when the clutch of the transmission is in a contact state (power transmission state) or separated state (power cutoff state).

Further, for example, the object to be controlled in this embodiment may be an airplane or the like. A collision in this example can occur during take-off and landing of an airplane, such as when controlling to optimize motion, including before and after touchdown. Specifically, it is a case where the engine and the aircraft are controlled so as to decelerate immediately after landing while landing on a desired route.

Further, for example, the object to be controlled in this embodiment may be a train or the like. Collisions in this example can occur, for example, when train coupling is controlled to optimize motion, including before and after coupling. Specifically, it is a case where the motor is controlled so as to reduce the impact at the time of connection and the load on the drive motor.

In the above examples, the programs can be stored and delivered to computers using various types of non-transitory computer readable media. Non-transitory computer-readable media include various types of tangible storage media. Examples of non-transitory computer-readable media include magnetic recording media (eg, flexible discs, magnetic tapes, hard disk drives), magneto-optical recording media (eg, magneto-optical discs), CD-ROMs (Read Only Memory), CD-Rs, CD-R/W, semiconductor memory (eg, mask ROM, PROM (Programmable ROM), EPROM (Erasable PROM), flash ROM, RAM (Random Access Memory)). The program may also be delivered to the computer on various types of transitory computer readable medium. Examples of transitory computer-readable media include electrical signals, optical signals, and electromagnetic waves. Transitory computer-readable media can deliver the program to the computer via wired channels, such as wires and optical fibers, or wireless channels.

1... Robot system, 2... Control device, 12... State acquisition unit, 14... Non-linear model prediction control unit, 16... Servo control unit, 100, 100-1, 100-2. Robot 102 Torso 110L Left leg 110R Right leg 112 Upper leg 114 Lower leg 116 Foot 118 Sole sensor 120 Hip joint 122 Knee joint 124 Ankle joint 130 Angle sensor 136 Torque sensor 140 Motor 150. .. joint 151 .. support leg link 152 .. free leg link

Claims (1)

A nonlinear model prediction that is configured to be able to control the controlled plant while predicting the future response of the controlled plant at each time by performing feedback control while computing the optimization problem of the nonlinear control model of the controlled plant. a controller,
Prediction that predicts the number of collisions at each time until a predetermined period of time from that time for a controlled object that is configured to execute and control operations on the premise that collisions with surrounding objects will occur repeatedly. means and
a calculation means for executing calculations of the optimization problem of the nonlinear control model on the premise that collisions with surrounding objects will occur, according to the predicted number of collisions;
A nonlinear model predictive controller, comprising:

Images