JP6781101B2 - Non-linear system control method, biped robot control device, biped robot control method and its program - Google Patents

Description

The present invention relates to a control method for a nonlinear system, a control device for a biped robot, a control method for a biped robot, and a program thereof.

When controlling a system with low stability such as a bipedal walking robot, model predictive control (receding horizon control) that controls while predicting the system behavior in the future (until after a finite time) is used. It is effective to use. Model prediction control is feedback control that solves the optimum control problem from each time to the finite time future for each control cycle (sampling cycle) and determines the control input value.

In feedback control, since the bipedal walking robot is a highly non-linear system accompanied by walking motion and the like, it is preferable that the bipedal walking robot is controlled by the non-linear model predictive control. Here, the nonlinear model predictive control generally requires a large amount of calculation time. Therefore, it has been difficult to determine the optimum solution of the control input value in real time (real time) using the nonlinear model predictive control.

In relation to this technique, Non-Patent Document 1 discloses a technique called C / GMRES method (continuation / generalized minimum residual method) capable of performing nonlinear model predictive control in real time (real time). The C / GMRES method is an algorithm that combines the continuation method and the GMRES method. The C / GMRES method is a calculation method in which the optimum solution is tracked while obtaining the rate of change of the optimum solution by utilizing the continuity of the optimum solution for a system in which the 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.

Toshiyuki OHTSUKA and Hironori A. FUJII, "Real-Time Receding-Horizon Control Algorithm for Nonlinear Systems", Proceedings of the Society of Instrument and Control Engineers, December 1997, Vol.33, No.12, p. 1131-1139

In a non-linear system, which is a system having non-linearity, when moving with physical contact with the surrounding environment, the state change may be discontinuous due to the physical contact. On the other hand, the technique according to Non-Patent Document 1 is based on the premise that the state change of the system is continuous. Therefore, it is difficult to control a nonlinear system that may make physical contact by using the technique according to Non-Patent Document 1. Therefore, it has been difficult to control a nonlinear system with discontinuous state changes in real time by using nonlinear model predictive control.

The present invention provides a control method for a nonlinear system capable of controlling a nonlinear system with discontinuous state changes in real time, a control device for a bipedal walking robot, a control method for a bipedal walking robot, and a program thereof. ..

The method for controlling a non-linear system according to the present invention uses an acquisition step for acquiring a state parameter indicating the state of the non-linear system and an algorithm for model prediction control based on the acquired state parameter. It has a calculation step for calculating a control input value for controlling the system and a control step for controlling the non-linear system using the calculated control input value, and the state at a designated timing in the calculation step. The control input value in the predetermined evaluation interval in the model prediction control algorithm is used for each control cycle of the non-linear system by using a constraint parameter that constrains the state of the non-linear system so that is discontinuously changed. The rate of change of the optimum solution is calculated, the optimum solution of the control input value in the control cycle next to the control cycle is calculated using the rate of change, and the current control input value is calculated from the optimum solution.

As described above, the present invention uses a constraint parameter that constrains the state of the nonlinear system so that the state changes discontinuously at a specified timing, thereby causing a discontinuous state change at the assumed timing. Can be woken up. Therefore, the theory of nonlinear model predictive control can be easily applied to the control of a nonlinear system, and the C / GMRES method can also be applied. Therefore, the present invention makes it possible to control a nonlinear system with a discontinuous state change in real time.

Further, the control device for a bipedal walking robot according to the present invention is a control device for a bipedal walking robot that controls the operation of a bipedal walking robot capable of performing bipedal walking using two legs. The operation of the bipedal walking robot is controlled by using the state acquisition means for acquiring the state parameter indicating the state related to walking of the bipedal walking robot and the model prediction control algorithm based on the acquired state parameter. It has a calculation means for calculating a control input value for performing the operation, and a control means for controlling the operation of the bipedal walking robot by using the calculated control input value, and the calculation means has a designated timing. In advance in the model prediction control algorithm for each control cycle of the bipedal walking robot, using a restraint parameter that constrains the state of the bipedal walking robot so that the free leg of the two legs lands. The rate of change of the optimum solution of the control input value in the defined evaluation interval is calculated, the optimum solution of the control input value in the control cycle next to the control cycle is calculated using the rate of change, and the optimum solution is used. , The current control input value is calculated.

Further, the control method of the bipedal walking robot according to the present invention is a control method of the bipedal walking robot that controls the operation of the bipedal walking robot capable of performing bipedal walking using two legs. Based on the acquisition step of acquiring the state parameter indicating the state related to the walking of the bipedal walking robot and the acquired state parameter, the operation of the bipedal walking robot is controlled by using the model prediction control algorithm. It has a calculation step for calculating a control input value for the purpose and a control step for controlling the operation of the bipedal walking robot using the calculated control input value, and at a timing specified in the calculation step. Predetermined in the model prediction control algorithm for each control cycle of the biped robot using a restraint parameter that constrains the state of the biped robot so that the free leg of the two legs lands. The rate of change of the optimum solution of the control input value in the evaluated evaluation section is calculated, the optimum solution of the control input value in the control cycle next to the control cycle is calculated using the rate of change, and the optimum solution is used. The current control input value is calculated.

Further, the program according to the present invention is a program that realizes a control method of a bipedal walking robot that controls the operation of a bipedal walking robot capable of performing bipedal walking using two legs. To control the operation of the bipedal walking robot by using an acquisition step for acquiring a state parameter indicating a state related to walking of the legged robot and an algorithm of model prediction control based on the acquired state parameter. This is a calculation step for calculating the control input value, and the bipedal robot uses a constraint parameter that constrains the state of the biped robot so that the free leg of the two legs lands at a specified timing. For each control cycle of the walking robot, the rate of change of the optimum solution of the control input value in the predetermined evaluation section in the model prediction control algorithm is calculated, and the rate of change is used to calculate the next control cycle of the control cycle. The operation of the bipedal walking robot is performed using the calculation step of calculating the optimum solution of the control input value in the above and calculating the current control input value from the optimum solution, and the calculated control input value. Have the computer perform the control steps to control.

As described above, the present invention uses a restraint parameter that constrains the state of the biped robot so that the swing leg lands at a specified timing, so that the swing leg does not land at the assumed timing. It is possible to cause a continuous state change. Therefore, the theory of nonlinear model predictive control can be easily applied to the control of a bipedal walking robot, and the C / GMRES method can also be applied. Therefore, the present invention makes it possible to control the movement of a bipedal walking robot accompanied by a discontinuous state change in real time.

Also, preferably, the constraint parameter is included in the evaluation function used in the model prediction control algorithm. This makes it possible to handle optimization problems in the same way as nonlinear model predictive control with no discontinuous state changes.

Further, preferably, the restraint parameter specifies the posture of the biped robot when the swing leg lands at the timing. This makes it possible to control the bipedal walking robot so that the swing leg lands in the assumed posture.

Also, preferably, the restraint parameter specifies a target angle of the joints of the two legs when the swing leg lands at the timing. This makes it possible to control the joint portion so that the swing leg lands at the assumed joint angle.

Also, preferably, the constraint parameter includes an adjustable gain. This makes it possible to stabilize the control regardless of the performance of the control device.

According to the present invention, a method for controlling a nonlinear system capable of controlling a nonlinear system with a discontinuous state change in real time, a control device for a bipedal walking robot, a control method for a bipedal walking robot, and a program thereof are provided. Can be provided.

It is the schematic which shows the robot system which concerns on Embodiment 1. FIG. It is a functional block diagram which shows the structure of the robot system which concerns on Embodiment 1. FIG. It is a figure for demonstrating the nonlinear model predictive control. It is a figure for demonstrating the update of an input series. It is a figure for demonstrating the method of applying the robot which concerns on Embodiment 1 to a compass type model. It is a figure which shows the example which applied the robot which concerns on Embodiment 1 to a compass type model. It is a figure for demonstrating the state jump. It is a figure which shows the state of the robot just before the collision of a swing leg link. It is a figure which shows the state of the robot immediately after the collision of a swing leg link. It is a flowchart which shows the control method of the robot performed by the control device which concerns on Embodiment 1. FIG. It is a figure which shows the robot which concerns on Embodiment 2. It is a figure which shows the state which applied the robot which concerns on Embodiment 2 to a knee flexion model. It is a figure which shows the example which applied the robot which concerns on Embodiment 2 to a knee flexion model. It is a figure which shows the simulation result which applied the algorithm of the nonlinear model prediction control to the nonlinear system which concerns on this embodiment. It is a figure which shows the simulation result which applied the algorithm of the nonlinear model prediction control to the nonlinear system which concerns on this embodiment. It is a figure which shows the simulation result which applied the algorithm of the nonlinear model prediction control to the nonlinear system which concerns on this embodiment. It is a figure which shows the simulation result which applied the algorithm of the nonlinear model prediction control to the nonlinear system which concerns on this embodiment. It is a figure which shows the simulation result which applied the algorithm of the nonlinear model prediction control to the nonlinear system which concerns on this embodiment. It is a figure which shows the simulation result which applied the algorithm of the nonlinear model prediction control to the nonlinear system which concerns on this embodiment. It is a figure which shows the graph of the control input of a steady state in the simulation result.

(Embodiment 1)
Hereinafter, embodiments of the present invention will be described with reference to the drawings. In each drawing, the same elements are designated by the same reference numerals, and duplicate explanations are omitted as necessary.

FIG. 1 is a schematic view showing a robot system 1 according to the first embodiment. Further, FIG. 2 is a functional block diagram showing the configuration of the robot system 1 according to the first embodiment. The robot system 1 includes a robot 100 and a control device 2 that controls the operation of the robot.

The robot 100 has a body 102 and two legs, a right leg 110R and a left leg 110L. The robot 100 is a bipedal walking robot capable of performing a walking motion using two legs (right leg 110R and left leg 110L). The right leg 110R and the left leg 110L are provided in the lower part of the body 102 of the robot 100. Here, as shown in FIG. 1, the front direction of the robot 100 is the X-axis direction, and the upward direction is the Y-axis direction. Hereinafter, "R" is added to the code of the component related to the right leg 110R, and "L" is added to the code of the component related to the left leg 110L. However, when the left and right are not distinguished for each component, "R" is added. "R" and "L" may be omitted as appropriate.

The right leg 110R has a hip joint portion 120R, an upper leg portion 112R, a knee joint portion 122R, a lower leg portion 114R, an ankle joint portion 124R, and a foot portion 116R in order from the side closest to the torso 102. 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 order from the side closer to the torso 102. Have. Sole sensors 118 are provided on the bottoms of the foot 116R and the foot 116L, respectively. 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 part of the body 102. Then, the upper thigh portion 112R and the upper thigh portion 112L are connected to the torso 102, respectively, via the hip joint portion 120R and the hip joint portion 120L, respectively. In other words, the right leg 110R and the left leg 110L are connected to the torso 102 via the hip joint portion 120R and the hip joint portion 120L, respectively.

Further, the upper leg portion 112R and the lower leg portion 114R are connected via the knee joint portion 122R. Similarly, the upper leg portion 112L and the lower leg portion 114L are connected via the knee joint portion 122L. Further, 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 about an axis perpendicular to the XY plane (that is, a laterally horizontal axis of the robot 100). As a result, the right leg 110R and the left leg 110L can move back and forth. Therefore, the robot 100 can perform a walking motion by alternately pushing the right leg 110R and the left leg 110L forward.

The knee joint 122 rotates about an axis perpendicular to the XY plane. As a result, the right leg 110R and the left leg 110L can perform a flexion motion at the knee joint portion 122. Also, the ankle joint 124 rotates about an axis perpendicular to the XY plane. As a result, the foot portion 116 can move up and down with respect to the lower leg portion 114.

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

The control device 2 has a function as, for example, a computer. The control device 2 may be mounted inside the robot 100 (for example, the body 102). Further, the control device 2 may be physically separated from the robot 100, and in that case, the control device 2 may be communicably connected to the robot 100 via a wire or a radio. The control device 2 controls the operation of the robot 100, particularly the operation 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 motor of each joint portion. That is, in the robot system 1, the control device 2 has a function as a master device, and the robot 100 has a function 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 a main hardware configuration. The CPU 4 has a function as an arithmetic unit that performs control processing, arithmetic processing, and the like. The ROM 6 has a function for storing a control program, an arithmetic program, and the like executed by the CPU 4. The RAM 8 has a function for temporarily storing processing data and the like.

Further, the control device 2 includes a state acquisition unit 12, a nonlinear model prediction control unit 14, and a servo control unit 16 (hereinafter, referred to as “each component”). Each component can be realized, for example, by the CPU 4 executing a program stored in the ROM 6. Further, each component may be realized by recording a necessary program on an arbitrary non-volatile recording medium and installing it as needed. It should be noted that each component is not limited to being realized by software as described above, and may be realized by some hardware such as a circuit element.

The state acquisition unit 12 has a function as a state acquisition means for acquiring data (state parameters) indicating a state related to the current walking of the robot 100. The state acquisition unit 12 acquires the detected value of each sensor from each sensor (angle sensor 130, sole sensor 118, and torque sensor 136). Then, the state acquisition unit 12 outputs the acquired detected value (and the value obtained from the detected value) to the nonlinear model prediction control unit 14. The "value obtained from the detected value" may be, for example, the speed (change amount, time derivative) of the joint angle when the "detected value" is the joint angle detected by the angle sensor 130. .. In this case, the state parameters may indicate the joint angle and the speed of the joint angle.

The nonlinear model prediction control unit 14 has a function as a calculation means for calculating a control input value (input value) for controlling the operation of the robot 100. The nonlinear model prediction control unit 14 uses at least a part of the detected value (and the value obtained from the detected value) from the state acquisition unit 12 as a state parameter, and uses an algorithm for model prediction control based on the state parameter. The control input value for controlling the operation of the robot 100 is calculated. Further, the nonlinear model prediction control unit 14 outputs the calculated control input value to the servo control unit 16. Details will be described later. Further, the nonlinear model prediction control unit 14 may input necessary instruction values (step length, walking cycle, etc.) by an external controller (not shown) of the robot system 1.

The servo control unit 16 has a function as a control means for controlling the operation of the robot 100 by using the control input value calculated by the nonlinear model prediction control unit 14. The servo control unit 16 controls each joint portion of the robot 100 so as to obtain the calculated control input value. Further, the servo control unit 16 may have a servo amplifier function. Further, when performing torque control, the servo control unit 16 controls the motor 140 of each joint so that the torque (joint torque) of each joint becomes the calculated control input value. At this time, the servo control unit 16 may perform feedback control using the torque value detected by the torque sensor 136 of each joint portion.

(Model prediction control)
Here, an outline of a method of model prediction control (non-linear model prediction control) performed by the nonlinear model prediction control unit 14 according to the present embodiment will be described. Non-linear model predictive control is a control in which the optimum input (optimal solution of the control input value) up to the finite time future is obtained for the nonlinear system at each sampling time, and the initial value of the obtained inputs is used as the actual input. is there. The nonlinear model predictive control has three advantages: the nonlinear optimal control, the feedback control, and the constraint condition can be easily incorporated.

As described above, since the nonlinear model predictive control is feedback control, it is strong against disturbance and various constraint conditions can be combined. Due to these characteristics, nonlinear model predictive control is expected to be introduced into many systems. However, with conventional iterative methods such as Newton's method, it is difficult to converge to the optimum solution within the sampling period.

In recent years, the C / GMRES method has been newly devised as an effective numerical calculation method for this problem. By using the C / GMRES method, it has become possible to solve the optimal control problem up to the finite time future within the sampling period. However, as shown below, there are cases where the nonlinear model prediction control by the C / GMRES method cannot be applied at present.

That is, many non-linear systems such as the robot 100 may be accompanied by an event (referred to as "state jump") in which the state changes discontinuously. It has been difficult to apply the current nonlinear model predictive control by the C / GMRES method to the problem including the state jump. When trying to optimize a system with state jumps directly, it is necessary to optimize the time and input of state jumps at the same time. In addition, the Lagrange multiplier for the state jump will be added.

Further, in order to solve these problems numerically by using the C / GMRES method, it is necessary to significantly change the algorithm of the C / GMRES method. Therefore, the inventors of the present application have found a method of adding a penalty function (penalty term) to the evaluation function of nonlinear model predictive control. By adding the penalty function to the evaluation function, the time when the state jump occurs (in the present embodiment, the time when the swing leg lands) can be specified. In addition, the C / GMRES method can be applied without increasing the Lagrange multiplier.

First, the outline of the nonlinear model predictive control will be described. As a control target, consider a nonlinear system represented by the equation of state as shown by the following equation (1).

However, x (t) is a state vector, and u (t) is a control input vector (a vector indicating a control input value). Further, R ⁿ and R ^m indicate a set of all n-dimensional real number vectors and a set of all m-dimensional real number vectors, respectively. Non-linear model predictive control _obtains an input input (t + τ) that minimizes the evaluation function represented by the following equation (2) at each time t for the system represented by the equation (1). This is a control in which the initial value u _opt (t) is set as the actual control input value u (t) at time t.

Here, T is the length of the evaluation interval at time t. The function φ (x) is a scalar value function called the termination cost. The function L (x, u) is a scalar value function called the stage cost. τ is a time parameter in the evaluation interval, and 0 ≦ τ ≦ T. In addition, T is usually given by the following equation (3) using positive scalar values T _f and α (α> 0).

As described above, the nonlinear model prediction control is feedback control because the optimum input based on the state is obtained at each time t.
Further, a vector function represented by the following equation (4) can be given as a constraint condition.
As for the constraint condition, not only the equality constraint condition but also the inequality constraint condition can be incorporated.

FIG. 3 is a diagram for explaining nonlinear model predictive control. As shown in FIG. 3, the nonlinear model predictive control, the model behavior time to T seconds later at the current time t ₀ through optimization calculations to predict prompts _{u opt (t 0 + τ)} . Then, as its initial value _u opt _{(t 0)} the actual control input value u at the current time _{t 0 (t 0).}

Similarly, after the sampling period Δt seconds which is the control period, the at current time t ₁ the model behavior period until after T seconds performs optimization calculation predicts input u _{opt (t} 1 ₊ τ) at the time Ask. Then, the initial value u _opt (t ₁ ) is set as the actual control input value u (t ₁ ) at the current time t ₁ . Similarly, the control input value u (t) is calculated for each sampling cycle.

The problem to be solved at each time t in the nonlinear model predictive control is the optimal control problem as shown in the following equations (5) to (8) for the time τ on the evaluation interval.
However, the state variable vector and the control input vector at time τ (0 ≦ τ ≦ T) on the evaluation interval at time t are x ^* (τ; t) = x ^* (t + τ) and u ^* (τ;, respectively. t) = u ^* (t + τ). The subscript * indicates that the value is on the evaluation interval.

When J in Eq. (7) is regarded as a functional and the retention condition is obtained using the variational method, the necessary conditions for optimal control (Euler-Lagrange equation) are as shown in Eqs. (9) to (14) below. can get.

here,
Is a scalar value function called the Hamilton function, where λ ^* (τ; t) is the contingent variable for equation (9) and μ ^* (τ; t) is the Lagrange multiplier for equation (14).

On the other hand, all actual numerical calculations are performed discretely. Therefore, equations (9) to (14) must all be treated as discrete approximations. Therefore, consider discretely approximating the evaluation interval (0 ≦ τ ≦ T) to N steps. The time step of the evaluation section at that time is shown by the following equation (16).

Then, the i-th (1 ≦ i ≦ N) step on the evaluation interval, that is, the state at the time t + iΔτ, is set.
It is expressed as. The same applies to u ^* _i (t), λ ^* _i (t) and μ ^* _i (t). In addition, T and N are predetermined values. Therefore, Δτ is also a predetermined value.

Discrete approximation of equations (9) to (14) under these conditions gives Euler-Lagrange equations discretely approximated as shown in equations (17) to (22) below.

However, the discretely approximated Hamiltonian function is defined by the following equation (23).

From the above, the problem of finding the optimum input in nonlinear model predictive control is to solve the above equations (17) to (22), and for i = 0 to i = N, x ^* _i (t) and u ^* _i. It comes down to the problem of finding (t), λ ^* _i (t) and μ ^* _i (t).
Here, x ^* _i (t) is explicitly obtained from the above equations (17) and (18). Further, λ ^* _i (t) is explicitly obtained from the obtained x ^* _i (t) and the above equations (19) and (20). Therefore, the essential unknowns of x ^* _i (t), u ^* _i (t), λ ^* _i (t) and μ ^* _i (t) are the vectors represented by the following equation (24). It is defined by U (t).
In addition, ": =" is an equal sign meaning a definition. That is, in the above equation (24), the left side U (t) is defined by the vector on the right side.

Then, this U (t) is obtained by solving the equation represented by the following equation (25).

Next, the C / GMRES method will be described. In iterative methods such as Newton's method, it is difficult to solve equation (25) within each sampling time. On the other hand, if U (t) is continuous with respect to time t, U (t) can be updated for each sampling period Δt as shown by the following equation (26).

Therefore, in order to obtain U (t), when t = 0, U (0) is obtained, and when t> 0, the time derivative of U (t), that is, the amount of change in U (t).
You just have to ask. The calculation of equation (26) corresponds to numerical integration of the amount of change in U (t) described above.

here,
In order to obtain, consider that the equation (25) holds for all t, and consider dealing with the equation represented by the following equation (27), which is equivalent to the above equation (25).

Further, the equation (27) can be rewritten as shown by the following equation (28).
However, ζ is a positive real number and is called a stabilization parameter.

When the total derivative of Eq. (28) is executed and rearranged, the amount of change in U
Is obtained by solving the simultaneous equations represented by the following equation (29).

Therefore, when t> 0, the only problem to be solved at each time is the simultaneous equations represented by the above equation (29). Further, as a numerical solution method of the simultaneous equations represented by the equation (29), the GMRES method capable of obtaining a highly accurate solution from the simultaneous equations with a small number of iterations can be used.

The method described above is a real-time optimization algorithm for nonlinear model predictive control. An algorithm that combines the transformation method (Continuation method) utilizing the continuity of U (t) and the GMRES method is called a C / GMRES method.

Next, the nonlinear model predictive control considering the state jump will be described. When the state x (t) ∈ R ⁿ satisfies the condition expressed by the following equation (30), a system in which a state jump occurs immediately after that is assumed.
However, ξ (x (t)) ∈ R ^l is a vector value.

Here, state jump, to take place before and after the time t _j. That, t _j respectively just before and just after the time of "t _j -" is represented as and "t _{j +"} system which is assumed here, meet the following equation (31).

Further, in the system assumed here, the state x (t _j +) immediately after the state jump is positively changed from the state x (t _j −) immediately before the state jump, as expressed by the following equation (32). Suppose you are asked.

Furthermore, it is assumed that the equation of state of the system can be switched with the state jump. At this time, the equations of state before and after the state jump are described as shown by the following equations (33) and (34).
The equation of state does not necessarily have to be switched, and f ₁ (x, u) = f ₂ (x, u) may be used. Further, for the sake of clarification of the explanation, it is assumed that the constraint condition shown in the equation (4) does not exist in the system dealt with here.

To consider the problem of the non-linear model predictive control, in the following description, the jump time on the evaluation section and tau _j. That is, τ _j satisfies the following equations (35) and (36).

In general, in order to optimize the above system, the retention condition derived by the variational method may be numerically solved. When the equations of the system are given by the above equations (33), (34) and (32) and t _j is not fixed, the retention conditions are given by the following equations (37) to (48).
Where ν ∈ R ^l is a Lagrange multiplier for equation (31).

The Hamiltonian functions H ₁ and H ₂ are represented by the following equations (49) and (50), respectively.

Among the above stationary condition, formula indicating the condition regarding the time _{t j} in Formula (41), a (48). Here, it is difficult to obtain τ _j from these equations, considering that the numerical calculation is actually performed by discreteization as described above. If τ _j cannot be obtained, the state x ^* (τ; t) on the evaluation interval and the contingent variable λ ^* (τ; t) on the evaluation interval cannot be obtained. In addition, ν has also been added as a new unknown quantity. Therefore, even if τ _j is obtained, the above equation (24) cannot be used as it is as an equation for calculating the essential unknown quantity, and the unknown quantity and the unknown equation must be newly defined. It doesn't become.

Therefore, in the algorithm according to the present embodiment, a penalty function represented by the following equation (51) using a sufficiently large positive scalar value p is added to the evaluation function.

By optimizing an evaluation function obtained by adding a penalty function represented by the formula (51), for the specified time tau _j, may it holds the following equation (52).

In other words, by using the above penalty function, it is possible to produce a state jump in the specified time t _j. In other words, the above penalty function is a constraint parameter that constrains the state of the system so that the state becomes discontinuous at a specified timing. Further, since the constraint condition represented by the equation (52) is added for the penalty function, the Lagrange multiplier ν need not be added. Therefore, in the algorithm according to the present embodiment, the optimization problem of the nonlinear model predictive control can be treated as the same number of unknown problems as the nonlinear model predictive control without the state jump.

Therefore, the problem of the nonlinear model predictive control including the state jump can be treated in the same manner as the nonlinear model predictive control without the state jump by adding the equation (51) to the evaluation function. However, since the use of penalty functions, it is necessary to specify in advance the proper state jump time t _j.

As described above, in the algorithm according to the present embodiment, by using the penalty function, it is possible to cause a discontinuous state change at the assumed timing. Therefore, the original theory of nonlinear model predictive control can be easily applied, and further, the C / GMRES method can be applied. Therefore, as will be described later, it is possible to control a non-linear system such as a bipedal walking robot in real time.

Next, the stagnation condition when the penalty function is used will be described. Here, if the retention condition is derived for continuous time, the terms represented by the following equations (53) and (54) occur.

As described above, in order to perform numerical calculations on a computer, it is necessary to consider each equation of the stagnation condition by discrete approximation. However, equation (53) and (54) can not perform a discrete approximation before and after tau _j. Therefore, for the problem of the present embodiment, the equation of state and the evaluation function are discretely approximated, and then the retention condition is derived from the variational method.

First, step i that satisfies the following equation (55) is defined as jump step _ij .

Using _ij , the above equations (33), (34), and (32) are discretely approximated as shown in the following equations (56), (57), and (58), respectively.

Further, the penalty term (penalty function) represented by the equation (51) is discretely approximated as shown by the following equation (59).

Next, the retention condition is obtained by the variational method. The optimal control problem for this discretely approximated system is a sequence of inputs u ₀ (with the addition of equation (59) that minimizes the discretely approximated evaluation function as represented by equation (60) below. t), ..., u _N-1 (t) is a problem to be obtained.

Here, the equations (56), (57), and (58) can be regarded as equality constraints as shown by the following equations (61), (62), and (63), respectively.

The Lagrange multiplier vector of equations (61) and (62) is λ _{i + 1} (t), and the Lagrange multiplier vector of equation (63) is
And. At this time, the Lagrange function is defined by the following equation (64).

Substituting J represented by the formula (60) into J of the formula (64), the following formula (65) is obtained.

However, the Hamiltonian functions H ₁ and H ₂ are defined by the following equations (66) and (67), respectively.

Further, the variation of the equation (65) is expressed as the following equation (68).

If the control input value is optimal, in the formula (68), any .delta.x _i (t), for .delta.u _i (t),
Is established. Here, if it is noted that δ x ₀ (t) = 0 from x ₀ (t) = x (t), the retention conditions represented by the following equations (69) to (79) are derived. However, the subscript * indicates that the value is on the evaluation section.

Here, the state x ^* _i (t) on the evaluation interval can be calculated from the above equations (69), (70), (71), and (72). Further, the adjoint variable λ ^* _i (t) can be calculated from the above equations (73), (74), (75) and (76). Therefore, in the model according to the present embodiment, since the constraint condition is not considered, the vector U (t) indicating the unknown quantity is defined by the following equation (80).

This U (t) can be obtained by solving the equation represented by the following equation (81).
This equation may be solved using the C / GMRES method described above.

Here, consider updating the input series (optimal solution) for each sampling cycle. FIG. 4 is a diagram for explaining the update of the input sequence. As shown by the arrow A in FIG. 4, in the C / GMRES method, the forward difference approximation is performed as shown by the above equation (26). Here, the time of the _ijth step in the evaluation section at time t, that is, (t + i _j Δτ) is the time before the state jump (that is, the time before time t _j ), and i _j in the evaluation section at time t + Δt. It is assumed that the time of the step, that is, (t + Δt + i _j Δτ) is the time after the state jump (that is, the time after the time t _j ). At this time, the jump step at time t is _ij , but the jump step at time t + Δt is _ij -1. At this time,
Is the input before the state jump (hereinafter referred to as the formula Uj _b ),
The input status after jump (hereinafter, wherein Uj _a hereinafter) becomes.

Therefore, in the system according to the present embodiment, since the states are discontinuous before and after the state jump, when i = _ij , simply,
Cannot be. That is, when calculated as Equation (26) ', the input Uj _a post above states jump, and thus not updated as the optimum input.

Therefore, in this embodiment, consider the input Uj _a status after jumping, and be approximated from the ^u _{* i} of i ≠ _{i j.} Input after the state jump Uj _a is an input on the evaluation section of the time t + Δt + _{i j} Δτ. For proper approximation, as shown by arrow B in FIG. 4, the input at the time closest to this time, that is, the input at time t + (i _j + 1) Δτ
It can be approximated from.

Thus, when a jump steps in jumping step and time t + Delta] t at time t is different, the input Uj _a status after jumping is updated as shown in the following equation (82).

However, when _ij = N-1, the approximation shown in the above equation (82) cannot be performed, so the input is updated as shown in the following equation (83) as usual.

The above equation assumes that Δt and Δτ satisfy the following equation (84).
Δt <2Δτ ・ ・ ・ (84)
However,
2Δτ≤Δt ・ ・ ・ (85)
Even when, the series of u ^* _i (t + Δt) can be appropriately approximated from the series of u ^* _i (t) in real time.

In the C / GMRES method, when determining the amount of change in U (t), F (U, x, t) is used to perform forward difference approximation as shown in the following equation (86). There is.

However, assuming that the difference time of the difference approximation is h, when a state jump occurs between the time t and the time t + h, the state quantity changes greatly discontinuously, and the difference approximation cannot be performed accurately. Therefore, the time t is represented by the following equation (87), t <t _j ≤ t + h ... (87).
When the condition is satisfied, the backward difference approximation is performed as shown by the following equation (88).

(Application to biped robots)
Next, an example in which the above-mentioned nonlinear model predictive control is applied to the control of the operation of the robot 100 according to the present embodiment will be described. In the first embodiment, an example in which the robot 100 is a compass type model will be described, but as will be described later, the nonlinear model prediction control can be applied even if the robot 100 is not a compass type model.

The walking motion of the robot 100 includes an motion in which the swing leg collides with the ground (landing). Before and after this collision, the generalized speed of the robot 100 changes discontinuously. That is, at this time, a state jump occurs. Also, in general, the walking motion is a periodic motion. Therefore, it is possible to control the robot 100 so as to generate a state jump (that is, land the swing leg) at predetermined intervals. The "landing" is not limited to the collision (contact) of the free leg with the ground. That is, "landing" means that the swing leg comes into contact with the surface (walking surface) on which the robot 100 is walking.

FIG. 5 is a diagram for explaining a method of applying the robot 100 according to the first embodiment to the compass model. In the example shown in FIG. 5, the right leg 110R is a support leg and the left leg 110L is a swing leg (swing leg). The control device 2 supports by using a torque sensor 136 provided at the ankle joint portion 124 of the support leg (right leg 110R in the example of FIG. 5) in order to simulate that the support leg is in point contact with the ground. The torque of the ankle joint portion 124 of the leg is controlled to 0. Further, the control device 2 controls the knee joint portions 122 of the right leg 110R and the left leg 110L so as to be locked in the extended state. That is, the control device 2 controls the motor 140 of the knee joint portion 122 so that the joint angles of the knee joint portions 122 of the right leg 110R and the left leg 110L are angles (for example, 0) corresponding to the extended state. Further, the control device 2 detects the landing of the swing leg by using the sole sensor 118 of the swing leg (left leg 110L in the example of FIG. 5). In this way, the robot system 1 can simulate a compass model.

FIG. 6 is a diagram showing an example in which the robot 100 according to the first embodiment is applied to a compass model. In the example shown in FIG. 6, the robot 100 is modeled on a compass model including a joint 150, a support leg link 151, and a swing leg link 152. Here, the joint 150 corresponds to the torso 102 and the hip joint 120. Further, the support leg link 151 corresponds to the support leg of the right leg 110R and the left leg 110L. Further, the swing leg link 152 corresponds to the swing leg of the right leg 110R and the left leg 110L.

Let the mass of the joint 150 be m ₀ . Further, as shown by the arrow in FIG. 6, the input torque u is input as a control input value around the joint 150. Here, it is assumed that the physical properties of the support leg link 151 and the swing leg link 152 are the same as each other. Let l be the length of the support leg link 151 and the swing leg link 152. Further, the mass of the support leg link 151 and the swing leg link 152 is m.

Further, the angle of the support leg link 151 with respect to the vertical direction is set to θ _1, and the angle of the swing leg link 152 with respect to the vertical direction is set to θ ₂ . However, in FIG. 6, clockwise (direction in which the joint 150 rotates forward around the lower end of each link) is positive. Therefore, in the state of FIG. 6, θ ₂ <0.

Next, it is assumed that the following assumptions are made regarding the walking motion of the compass model illustrated in FIG.
-The collision (landing) between the swing leg link 152 and the ground 90 is instantaneous.
-The collision between the swing leg link 152 and the ground 90 is a completely inelastic collision.
-The coefficient of friction between the link and the ground is ∞.
-Both legs (both links) do not receive force from the ground 90 at the same time.

From the above assumption, both legs (support leg link 151 and swing leg link 152) do not reach the ground 90 at the same time. Further, the speed of the swing leg link 152 becomes 0 at the time of a collision. Therefore, there are two equations required for walking control of this model: the equation of motion (state equation) during the one-leg support period and the equation at the time of collision of the swing leg link 152 (collision equation).

When only one leg (that is, the support leg link 151) is in contact with the ground 90, the equation shown by the following equation (89) is derived from Lagrange's equation of motion.

However, q is a generalized coordinate vector represented by the following equation (S1).
Further, M (q) is an inertial matrix, H (q, q (dots)) is a term of gravity and Coriolis force, and Nu is a generalized force with respect to q. Note that R ^{n × m} indicates a set of the entire real number matrix of n × m.

Here, the details of the equation of motion shown in the equation (89) are shown in the following equations (E1), (E2), and (E3). In the following formula, ^{I m} is the center of gravity of the support leg link 151 and the swing link 152 (centroid 151m and centroid 152m) is a moment of inertia about. Further, l _G is 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).

Further, the state vector x ∈ R ⁴ is shown in the following equation (S2).

In this case, the equation of state is expressed by the following equation (90) from the above equation (89).
In the compass type model according to the present embodiment, ZMP (zero moment point) is not considered as a walking constraint condition.

Next, the collision equation will be described. Before explaining the collision equation, the state jump will be explained.
FIG. 7 is a diagram for explaining a state jump. FIG. 7 is a diagram showing a state of the right leg 110R or the left leg 110L. Here, it is assumed that the state of the right leg 110R is shown. FIG. 7 is a graph (phase diagram) in which the horizontal axis indicates the angle of the right leg 110R and the vertical axis indicates the angular velocity of the right leg 110R.

In state I, the right leg 110R separates from the ground and becomes a free leg. Therefore, from state I, the angle and angular velocity of the right leg 110R (free leg link 152) are (θ ₂ , θ ₂ (dots)). At this time, in the period from the state I to the state II described later, the angle and the angular velocity change continuously. Then, in the state II, the right leg 110R (free leg link 152), which was a free leg, lands on the ground 90. Immediately, the state shifts to state III, and the right leg 110R becomes a support leg (support leg link 151). At this time, when transitioning from the state II to the state III, the angle hardly changes, but the angular velocity changes abruptly. Therefore, a state jump occurs when transitioning from state II to state III.

From state III, the angle and angular velocity of the right leg 110R (support leg link 151) is (θ ₁ , θ ₁ (dot)). At this time, in the period from the state III to the state IV described later, the angle and the angular velocity change continuously. Then, in the state IV, the left leg 110L (free leg link 152), which was a free leg, lands on the ground 90. Immediately, the state shifts to the state I, and the right leg 110R becomes a free leg (free leg link 152). At this time, when transitioning from the state IV to the state I, the angle hardly changes, but the angular velocity changes abruptly. Therefore, a state jump occurs when transitioning from the state IV to the state I.
In this way, when the swing leg lands on the ground, the parameters indicating the state (angle and angular velocity of the leg in the example of FIG. 7) change discontinuously. This phenomenon is a state jump.

Next, the definitions of generalized coordinates and generalized velocity before and after the collision will be described.
FIG. 8 is a diagram showing a state of the robot 100 immediately before the collision of the swing leg link 152. As shown in FIG. 8, the generalized coordinates and the generalized velocity immediately before the collision are represented by the following equations (G1) and (G2), respectively.

FIG. 9 is a diagram showing a state of the robot 100 immediately after the collision of the swing leg link 152. As shown in FIG. 9, the generalized coordinates and the generalized velocity immediately after the collision are represented by the following equations (G3) and (G4), respectively.

Here, the following two conservation laws for angular momentum are applied to the collision between the swing leg link 152 and the ground 90.
-Angular momentum of the entire system around the tip of the leg that was a free leg before the collision (free leg link 152).
-Angular momentum of the leg (support leg link 151) that was the support leg before the collision around the joint 150.
The above law of conservation of angular momentum is expressed by the following equation (91) from the above equations (G1), (G2), (G3) and (G4).

Note that Q ⁺ and Q ⁻ are represented by the following equations (E4) and (E5), respectively.

Here, the generalized coordinates do not change before and after the collision. Therefore, the following equation (92) holds for the generalized coordinate q ⁻ immediately before the collision and the generalized coordinate q ⁺ immediately after the collision.

Further, the above equations (91) and (92) are expressed by the following equation (93) using the state space vector x ⁻ immediately before the collision and the state space vector x ⁺ immediately after the collision.
However, I ₂ is a 2 × 2 identity matrix.

Further, Z (q ⁻ ) is a 2 × 2 matrix satisfying the following equation (94).

Next, consider applying the above-mentioned nonlinear model predictive control to the compass model according to the first embodiment. As described above, the walking motion is an motion of "continuously pushing the legs forward". In this model, an evaluation function is set to bring the opening angles of the legs (supporting leg link 151 and swing leg link 152) closer to the target value, and the coordinates of each link are exchanged each time the swing leg link 152 lands. Perform walking control.

At this time, the terminal cost φ and the stage cost L of the evaluation function J described above are expressed by the following equations (95) and (96), respectively.
However, s _f , q _0, and r are positive scalar values representing weights, respectively. Further, θ _ref is a target value (target angle) of the leg opening angle.

In addition, considering that θ ₁ and θ ₂ are exchanged each time the swing leg lands and θ ₁ (dot) and θ ₂ (dot) are exchanged, the equation (93) of the state jump is as follows. Is rewritten as in equation (97).

However, I ^_'2 ∈R ₂ ^× ₂ are the following matrix.

Next, assuming that the walking cycle is T _step , the jump time t _j is expressed by the following equation (98) using an integer k.
t _j = kT _step ... (98)
In addition, T (t) represented by the equation (3) is set so that the state jump does not occur more than once in the evaluation section. At this time, a collision occurs between the leg (swing link 152) represented by the equation (91) and the ground 90 for each period T _step , and in other cases, the model is modeled by the equation of motion expressed by the equation (97). Can describe the state of. That is, the state of the model is represented by the following equations (99) and (100).

Here, the state jump at time t _j, ie must occur collision of the leg (the idle leg link 152) with the ground 90. Further, since the lengths of the support leg link 151 and the swing leg link 152 are the same as each other, when the swing leg link 152 lands on the ground 90, θ ₁ = −θ ₂ . Therefore, the condition under which the state jump occurs is expressed by the following equation (101).

Therefore, the penalty function (penalty term) added to the evaluation function is represented by the following equation (51)'. Here, p ₁ is a gain (weight), which is a sufficiently large positive scalar value. The gain p _1, depending on the performance of the control unit 2 of the computer, and can be appropriately adjusted.

Further, in addition to the above penalty term, it is considered to add a term so that the leg opening angle at the time of landing of the swing leg link 152 approaches the target value. Therefore, the term represented by the following equation (51) ”is also added to the evaluation function as a penalty term. Here, p ₂ is a gain (weight) and is a sufficiently large positive scalar value. This gain p ₂ Can be adjusted as appropriate according to the performance of the computer of the control device 2.

When the equations (51)'and'(51)' are added together, the penalty term according to the first embodiment is represented by the following equation (102), whereby the penalty term (constraint parameter) is specified in advance. The state of the robot 100 is specified so that the swing leg lands at the determined timing (time t _j ). In other words, the penalty term (constraint parameter) is set by the swing leg at the predetermined timing (time t _j ). The posture (joint angle of each joint) of the robot 100 when landing is specified.
Therefore, in the control of the robot 100 using the nonlinear model prediction control in the compass model according to the first embodiment, the penalty function represented by the above equation (102) is applied as the penalty function shown by the equation (51).

FIG. 10 is a flowchart showing a control method of the robot 100 performed by the control device 2 according to the first embodiment. The control method shown in FIG. 10 uses the above-mentioned nonlinear model predictive control. Therefore, in the control method of the robot 100 according to the first embodiment, the vector U (t) represented by the equation (80) is obtained from the retention conditions represented by the equations (69) to (79), and the vector U (t) is obtained. The value of each component of (t) is calculated by solving the equation represented by the equation (81). As a result, the control input value for controlling the operation of the robot 100 is calculated. Here, in the flowchart shown in FIG. 10, S102 corresponds to the acquisition step of acquiring the state parameter, S104 to S114 correspond to the calculation step of calculating the control input value, and S116 corresponds to the operation of the robot 100. Corresponds to the control step to be controlled.

First, the control device 2 acquires a state vector (state parameter) indicating the state of the robot 100 and performs state observation (step S102). Specifically, the state acquisition unit 12 of the control device 2 acquires the joint angles of the hip joint portion 120R and the hip joint portion 120L from the angle sensor 130 of the hip joint portion 120R and the hip joint portion 120L. Then, the state acquisition unit 12 calculates θ ₁ and θ ₂ (FIG. 6) at the current time t from these joint angles. The state acquisition unit 12 can acquire the angle θ ₁ of the support leg link 151 with respect to the vertical direction from, for example, the joint angle of the hip joint portion 120 on the leg that is the support leg and the inclination of the body 102. Similarly, the state acquisition unit 12 can acquire the angle θ ₂ of the swing leg link 152 with respect to the vertical direction from, for example, the joint angle of the hip joint portion 120 on the swing leg and the inclination of the body 102. The inclination of the body 102 can be acquired by using an inclination sensor such as a gyro sensor.

The state acquisition section 12 calculates the theta ₁ and theta ₂ variation theta ₁ (dots) and theta ₂ (dots). For example, the state acquisition unit 12 changes the amount of change θ from the difference between θ ₁ and θ _{2 at} time t−Δt and θ ₁ and θ ₂ at time t in the sampling period (control cycle) immediately before time t, respectively. ₁ (dot) and θ ₂ (dot) may be calculated.

As a result, the state acquisition unit 12 acquires the state vector x (t) at time t. Further, as shown by the equation (72), the state acquisition unit 12 sets this x (t) as the initial value of the state vector in the evaluation interval for the time t. That is, x ^* ₀ (t) = x (t). The non-linear model prediction control unit 14 may perform the processing for this equation (72).

Next, the nonlinear model prediction control unit 14 of the control device 2 updates the state variables in the evaluation interval at time t for each i = 1 to N (step S104). Specifically, the nonlinear model prediction control unit 14 updates the state variables x ^* _i (t) as shown by the equations (69) to (71). In the example of landing of the swing leg of the compass type model according to the first embodiment, it is assumed that f related to the equation of state is the same before and after the jump step. That is, f ₁ (x, u) = f ₂ (x, u).

Then, the nonlinear model prediction control unit 14 updates the state variable in i ≠ _ij using the equation of state shown in equation (90) as shown in equations (69) to (70). Further, in i = _ij , the nonlinear model prediction control unit 14 updates the state variables as shown in the equation (71) by using the equation of the state jump shown in the equation (97). Further, the RAM 8 of the control device 2 stores the obtained state variable x ^* _i (t).

Next, the control device 2 updates the adjoint variable in the evaluation interval at time t for each i = 1 to N (step S106). Specifically, as shown by the equations (73) to (76), the nonlinear model prediction control unit 14 updates the adjoint variable λ ^* _i (t) by using each x ^* _i updated in S104. To do. Further, the RAM 8 of the control device 2 stores the obtained contingent variable λ ^* _i (t).

As described above, since f ₁ (x, u) = f ₂ (x, u), H ₁ = H ₂ . Then, the nonlinear model prediction control unit 14 calculates the Hamiltonian function H using the equation of state shown in equation (90) and the equation of stage cost L shown in equation (96), and equations (73) to (74). As shown by, the contingent variable at i ≠ i _j is updated. Further, in i = _ij , the nonlinear model prediction control unit 14 has the state jump equation shown in the equation (97), the penalty function equation shown in the equation (102), and the stage cost shown in the equation (96). Using equation L, the contingent variable is updated as shown in equation (75). Further, in i = N, the nonlinear model prediction control unit 14 updates the adjoint variable as shown in the equation (76) by using the equation of the termination cost shown in the equation (95).

Next, the control device 2 derives the vector function F (step S108). Specifically, the nonlinear model prediction control unit 14 calculates the vector U (t) represented by the equation (80), so that the state variables x ^* _i (t) obtained by the processing of S104 and S106 and the accompanying Using the variable λ ^* _i (t), the equation of the vector function F (U, x) represented by the equation (81) is derived. As described above, in the first embodiment, H ₁ = H ₂ .

Next, the control device 2 calculates the total derivative of the vector U (t) (step S110). Specifically, the nonlinear model prediction control unit 14 uses the C / GMRES method from the following equation (103), which is a modification of the equation (29), to obtain the total derivative of U (U (dot)), that is, U ( The rate of change of t) is calculated. In other words, the nonlinear model prediction control unit 14 calculates the rate of change of the optimum solution of the control input value for each control cycle.
As a result, as is clear from the equation (80), the time derivative (u ^* _i (dot)) of u ^* _i (t) can be obtained for i = 0 to N-1. The RAM 8 stores the obtained time derivative value of u ^* _i (t).

Next, the control device 2 updates the input sequence u ^* _i (t) (step S112). Specifically, the nonlinear model prediction control unit 14 updates the input series u ^* _i (t) from the equations (26) and (82) by the following equations (104) and (105).

Here, from equations (104) and (105), the nonlinear model prediction control unit 14 uses the rate of change of the input series u ^* _i (t) and u ^* _i (t) at a certain time t to perform the next sampling. The input sequence at the time t + Δt having the period Δt is calculated. Therefore, in order to calculate the input series u ^* _i (t) at time t, the input series u ^* _i (t−Δt) and u at time t−Δt of the sampling period Δt immediately before the current time t ^* The time derivative of _i (t−Δt) will be used. As a result, the value of each component of U (t) represented by the formula (80) can be obtained. In other words, the values of i = 0 to N-1 of the input series u ^* _i (t) are obtained. The RAM 8 stores the values of i = 0 to N-1 of the input series u ^* _i (t).

Next, the control device 2 determines an input value (control input value) (step S114). Specifically, the nonlinear model prediction control unit 14 determines the input value u ₀ by the following equation (106).
That is, the nonlinear model prediction control unit 14 determines the first component of the U (t) components as the input value. The nonlinear model prediction control unit 14 outputs the determined input value to the servo control unit 16.

Next, the control device 2 controls the robot 100 (step S116). Specifically, the servo control unit 16 determines the joint torque instructed to each joint unit from the input value determined in S114. More specifically, the servo control unit 16 applies the joint torque τ ₁ of the hip joint portion 120 of the leg corresponding to the support leg link 151 and the joint torque τ ₂ of the hip joint portion 120 of the leg corresponding to the swing leg link 152. , Determined by the following equations (107) and (108).
τ ₁ = u ₀ ... (107)
τ ₂ = −u ₀ ... (108)

In order to prevent the posture of the body 102 (joint 150) from collapsing, the servo control unit 16 determines the joint torque τ ₁ and the joint torque τ ₂ as in the following equations (107) and (108). May be good.
Here, θ _torso is a forward tilt angle of the fuselage 102 with respect to the vertical direction. Further, k _p and k _d are gains (weights) and are predetermined constants. This gain can be appropriately adjusted according to the performance of the computer of the control device 2.
The servo control unit 16 controls the motor 140 of each joint portion (hip joint portion 120) so as to obtain the determined joint torque.

As described above, the control device 2 according to the first embodiment sets the state of the robot 100 so that the swing leg lands at a designated timing when controlling the operation of the robot 100 by using the algorithm of the nonlinear model prediction control. A constraint function (constraint parameter) is used. As a result, it is possible to cause a discontinuous state change of landing of the swing leg at the assumed timing. Therefore, the original theory of nonlinear model predictive control can be easily applied, and further, the C / GMRES method can be applied. Therefore, it is possible to control a non-linear system such as a bipedal walking robot in real time.

Further, as shown in the above equation (102), the penalty function according to the first embodiment specifies the posture of the robot 100 when the swing leg lands at a predetermined timing. This makes it possible to control the robot 100 so that the swing leg lands in the assumed posture. Further, the penalty function according to the first embodiment specifies the joint angle of each joint portion of the robot 100 when the swing leg lands at a predetermined timing. This makes it possible to control the joint portion of the robot 100 so that the swing leg lands at the assumed joint angle.

Also, as shown in equation (102), the penalty function includes adjustable gains (p ₁ and p ₂ ). The gain must be a sufficiently large scalar value to ensure that the state changes at the specified timing. However, depending on the performance of the computer of the control device 2, if the gain is too large, the sensitivity becomes excessive, which may result in unstable control. Therefore, it is possible to stabilize the control by adjusting the gain.

(Embodiment 2)
Next, the second embodiment will be described. The second embodiment differs from the first embodiment in that the robot 100 is a humanoid robot more similar to a human. Then, in the second embodiment, the above-mentioned nonlinear model prediction control is applied to a model (knee flexion model) in which the knee is bent to perform bipedal walking. Since other points are substantially the same as those in the first embodiment, the description thereof will be omitted.

FIG. 11 is a diagram showing a robot 100 according to the second embodiment. Similar to the robot 100 according to the first embodiment, the robot 100 according to the second embodiment has a body 102, a right leg 110R, and a left leg 110L. The configurations of the right leg 110R and the left leg 110L are the same as those of the first embodiment. The torso 102 also has a waist 102a, a chest 102b, and a lumbar joint 102c. A head 104 is provided on the upper side of the body 102. A sensor such as a camera may be provided on the head 104.

Further, the robot 100 has a right arm 160R and a left arm 160L on the right side and the left side of the body 102 (chest 102b), respectively. The robot 100 according to the second embodiment can perform a predetermined operation by using the right arm 160R and the left arm 160L. Although not shown in FIG. 11, end effectors capable of gripping an object may be provided at the tips of the right arm 160R and the left arm 160L.

The right arm 160R has a shoulder joint portion 170R, an upper arm portion 162R, an elbow joint portion 172R, and a forearm portion 164R in order from the side closest to the body 102. Similarly, the left arm 160L has a shoulder joint 170L, an upper arm 162L, an elbow joint 172L, and a forearm 164L in order from the side closer to the torso 102. The shoulder joint 170R and the shoulder 170L are attached to the right side and the left side of the body 102, respectively. Then, the upper arm portion 162R and the upper arm portion 162L are connected to the body 102, respectively, via the shoulder joint portion 170R and the shoulder joint portion 170L, respectively. In other words, the right arm 160R and the left arm 160L are connected to the torso 102 via the shoulder joint 170R and the shoulder joint 170L, respectively.

Further, the upper arm portion 162R and the forearm portion 164R are connected via the elbow joint portion 172R. Similarly, the upper arm portion 162L and the forearm portion 164L are connected via the elbow joint portion 172L. Further, the shoulder joint portion 170 may be configured to rotate around three axes orthogonal to each other. Further, the elbow joint portion 172 rotates around one axis. Further, as shown in FIG. 2, the shoulder joint portion 170 and the elbow joint portion 172 have an angle sensor 130 and a motor 140.

FIG. 12 is a diagram showing a state in which the robot 100 according to the second embodiment is applied to the knee flexion model. In the example shown in FIG. 12, the right leg 110R is a support leg and the left leg 110L is a free leg. The control device 2 uses a torque sensor 136 provided at the ankle joint portion 124 of the support leg (right leg 110R in the example of FIG. 12) to simulate that the support leg is in point contact with the ground. The torque of the joint portion 124 is controlled to 0. Further, the control device 2 detects the landing of the swing leg by using the sole sensor 118 of the swing leg (left leg 110L in the example of FIG. 12). Here, unlike the first embodiment, in the second embodiment, the knee joint portion 122 is not locked. In this way, the robot system 1 can simulate a knee flexion model.

FIG. 13 is a diagram showing an example in which the robot 100 according to the second embodiment is applied to a knee flexion model. In the example shown in FIG. 13, the robot 100 includes a support leg lower leg link 201, a support leg upper thigh link 202, a body link 203, a swing leg upper leg link 204, a swing leg lower leg link 205, and joints 211, 212. , 213, 214, 215, and is modeled on a knee flexion model. Here, the body link 203 corresponds to the components above the body 102 of the robot 100.

Further, the support leg lower leg link 201 corresponds to the lower leg portion 114 on the support leg of the right leg 110R and the left leg 110L. The support leg upper thigh link 202 corresponds to the upper thigh portion 112 on the support leg of the right leg 110R and the left leg 110L. The free leg upper thigh link 204 corresponds to the upper thigh portion 112 on the free leg of the right leg 110R and the left leg 110L. The swing leg lower leg link 205 corresponds to the lower leg portion 114 on the swing leg of the right leg 110R and the left leg 110L.

Further, the joint 211 corresponds to the ankle joint portion 124 applied to the support leg. The joint 212 corresponds to the knee joint 122 over the support leg. The joint 213 corresponds to the hip joint 120. The joint 214 corresponds to the knee joint 122 on the swing leg. The joint 215 corresponds to the ankle joint portion 124 on the swing leg. Here, the joint 211 is in contact with the ground 90. The position of the joint 211, that is, the position of the tip of the support leg is defined as (X _b , Y _b ). Further, the position of the joint 215, that is, the position of the tip of the swing leg is defined as (X _c , Y _c ).

Here, a parameter k (k = 1 to 5) for distinguishing each link and each joint 211 to 215 is provided. k = 1 corresponds to the support leg lower leg link 201 and the joint 211. Similarly, k = 2 corresponds to the support leg upper thigh link 202 and the joint 212. k = 3 corresponds to the fuselage link 203 and the joint 213. k = 4 corresponds to the swing leg upper thigh link 204 and the joint 214. And k = 5 corresponds to the swing leg lower leg link 205 and the joint 215.

Therefore, the support leg lower leg link 201, the support leg upper thigh link 202, the torso link 203, the free leg upper thigh link 204, and the free leg lower leg link 205 are linked to links # 1, # 2, # 3, # 4, #, respectively. It may be indicated as 5. Then, when generalizing each link, it may be referred to as link # k. Similarly, the joints 211 to 215 may be referred to as joints # 1, # 2, # 3, # 4, and # 5, respectively. Then, when generalizing each joint, it may be referred to as joint #k.

The lengths (link lengths) of the support leg lower leg link 201, the support leg upper thigh link 202, the torso link 203, the free leg upper thigh link 204, and the free leg lower leg link 205 are set to l ₁ , l ₂ , l ₃ , respectively. Let l ₄ and l ₅ . Further, the masses (link masses) of the support leg lower leg link 201, the support leg upper thigh link 202, the torso link 203, the free leg upper thigh link 204, and the free leg lower leg link 205 are m ₁ , m ₂ , and m ₃ , respectively. , M ₄ , and m ₅ . The support leg lower link 201, the support leg thigh link 202, the body link 203, the free leg thigh link 204, and the moment of inertia of the center of gravity around each link of the free leg shank link 205, ^{respectively,} _{I m} 1, I ^{Let it be m} ₂ , ^Im ₃ , ^Im ₄ , and ^Im ₅ . In addition, l _k , m _k and ^Im _k are predetermined values.

Further, the angles (link angles) of the support leg lower leg link 201, the support leg upper thigh link 202, the torso link 203, the free leg upper thigh link 204, and the free leg lower leg link 205 with respect to the vertical direction are set to θ ₁ and θ ₂ , respectively. , Θ ₃ , θ ₄ , and θ ₅ . The link angles θ ₁ , θ ₂ , θ ₃ , θ ₄ , and θ ₅ are joints detected by the angle sensor 130 of each joint (hip joint 120, knee joint 122, and ankle joint 124) of the robot 100. It can be calculated geometrically and uniquely from the angle. Therefore, the link angles θ ₁ , θ ₂ , θ ₃ , θ ₄ , and θ ₅ correspond to the joint angles of the joints of the robot 100.

Further, the position of the center of gravity 201 m of the support leg lower leg link 201 is defined as (X _c1 , Y _c1 ). The position of the center of gravity 202 m of the support leg upper thigh link 202 is defined as (X _c2 , Y _c2 ). The position of the center of gravity 203 m of the fuselage link 203 is defined as (X _c3 , Y _c3 ). The position of the center of gravity 204 m of the free leg upper thigh link 204 is defined as (X _c4 , Y _c4 ). The position of the center of gravity 205 m of the swing leg lower leg link 205 is defined as (X _c5 , Y _c5 ). Further, k = 1 to 5 for each of the distance from the joint #k, to link the center of gravity of the #k #k (mass point), and _{r k.} The r _k is a predetermined value.

Here, the position of the center of gravity #k (X _ck , Y _ck ) is expressed by the following equations (109) and (110). The second term of equations (109) and (110) is set to 0 when k <4.

By performing the time derivative with respect to the equations (109) and (110), the velocity of the center of gravity #k can be obtained as shown by the following equations (111) and (112).

In this case, the Lagrangian Γ can be expressed as the following equation (113). In addition, g is gravitational acceleration.

At this time, Lagrange's equation of motion is expressed as the following equation (114).

Note that q and η are the vectors shown below.
Note that η is a joint torque vector. Here, η ₁ is the torque of the ankle joint portion 124. Further, η ₂ is the torque of the knee joint portion 122 of the support leg. Further, η ₃ is the torque of the hip joint portion 120 of the support leg, in other words, the torque of the body link 203 with respect to the support leg. η ₄ is the torque of the hip joint portion 120 of the swing leg. Further, η ₅ is the torque of the knee joint portion 122 of the swing leg.

Then, the following equation (115) can be derived from the equation (114). That is, in the knee flexion model according to the second embodiment, the equation of the equation (89) in the compass type model is modified as shown by the following equation (115). Note that M (q) and H (q, q (dots)) can be modified to correspond to the knee flexion model.

Here, N is a matrix represented by the following equation (116).

Further, since the equation (91) holds also in the knee flexion model according to the second embodiment, Q ⁺ and Q ⁻ can be modified to correspond to the knee flexion model. Further, the termination cost φ and the stage cost L of the evaluation function J shown in the equations (95) and (96) can also be modified to correspond to the knee flexion model.
Further, regarding the penalty term in the knee flexion model according to the second embodiment, the one shown by the formula (102) is modified to the one shown by the following formula (117). Note that q _ref indicates the target posture when the swing leg lands. In other _{words, q ref} indicates a vector of a link angle theta ₁ which corresponds to the joint angle of each _{joint, θ 2, θ 3, θ} 4, θ 5 of the target value (target angle) when the free leg lands. Specifically, q _ref is the stride length that X _c − X _b wants to achieve when the swing leg lands, and Y _c = Y _b (the height of the tip of the swing leg is the height of the tip of the support leg). Is the same as q).

However, R is a penalty weight matrix represented by the following equation (118). In addition, p _{1 to} p ₅ are predetermined values.

Then, in the flowchart shown in FIG. 10, in the process of S102, the state acquisition unit 12 acquires the joint angle of each joint portion from the angle sensor 130 of the hip joint portion 120, the knee joint portion 122, and the ankle joint portion 124. Then, the state acquisition unit 12 calculates θ _{1 to} θ ₅ (FIG. 13) at the current time t and the amount of change thereof from these joint angles, and acquires the state vector x (t). Then, the nonlinear model prediction control unit 14 can calculate the joint torques η _{1 to} η ₅ by performing the calculations S104 to S114 using each of the modified functions. Then, the servo control unit 16 can control the motor of each joint unit from the calculated joint torques η _{1 to} η ₅ .

As described above, the equation of state, the penalty term, and the like can be replaced with those in the compass model according to the first embodiment to those in the knee flexion model according to the second embodiment. This makes it possible to use the nonlinear model prediction control algorithm used in the control of the compass type model in the control of the robot 100 related to the knee flexion model according to the second embodiment. Therefore, with respect to the knee flexion model according to the second embodiment, it is possible to perform nonlinear model prediction control in real time as in the first embodiment.

(simulation result)
Next, the simulation results of the nonlinear system using the algorithm of the nonlinear model prediction control according to the present embodiment will be described. In the simulation described below, the nonlinear model prediction control algorithm according to the present embodiment is applied to the robot 100 according to the compass model according to the first embodiment.

Table 1 shows the physical parameters of the compass model used in the simulation. Table 2 shows the weights of the evaluation functions of the nonlinear model predictive control used in the simulation. Further, the target value of the leg opening angle is set to θ _ref = 0.32 [rad], and the walking cycle is set to T _step = 0.8 [s]. Further, the initial value of the state vector shown in the equation (S2) is x (t) = [-0.166, 0.165, 0.6, 0.75] ^T.

Table 3 shows each parameter used for the numerical calculation of the C / GMRES method. Here, h _dir is the difference time h of the forward difference approximation in the GMRES method represented by the equation (86). Further, r _trol is an allowable value of the optimum condition residual at the start of the simulation. The simulation time was set to 10 [s]. Further, in the evaluation interval T (t) shown in the equation (3), T _f = 0.8 [s] and α = 1.0.

14 to 19 are diagrams showing simulation results in which the nonlinear model prediction control algorithm is applied to the nonlinear system according to the present embodiment. Further, FIG. 20 is a diagram showing a graph of control input in a steady state in the simulation result. FIG. 14 shows the simulation results in which the nonlinear model prediction control algorithm according to the present embodiment is applied to the robot 100 according to the compass model according to the first embodiment under the conditions shown in Tables 1 to 3. .. 14 to 17 show the simulation results of θ ₁ , θ ₂ , θ ₁ (dot) and θ ₂ (dot), respectively. Further, FIG. 18 shows a simulation result of the control input value u. Further, FIG. 19 shows a simulation result of || F || (error norm), which is the magnitude of the vector F represented by the equation (81).

It can be seen that the state jump occurs in the vicinity of t = 8.0 [s] and the vicinity of t = 8.8 [s] in FIG. 20 where the graph stands vertically. As described above, in this simulation, we have succeeded in generating a periodic state jump in the steady state. Further, from the graph of u (t) in FIG. 18, it can be seen that steady walking is simulated after t = 5 [s].

(Modification example)
The present invention is not limited to the above embodiment, and can be appropriately modified without departing from the spirit. For example, one leg of the robot 100 has a hip joint 120, a knee joint 122, and an ankle joint 124, but the configuration is not limited to this. The legs of the robot 100 may have less than three joints or may have 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. Then, the functions such as the equation of state and the penalty function can be changed as appropriate according to the number of joints.

Further, in the above-described embodiment, an example in which the nonlinear system is a bipedal walking robot has been described, but the nonlinear model prediction control algorithm according to the present embodiment is also applied to a nonlinear system other than the bipedal walking robot. It is possible. That is, the non-linear system control method according to the present embodiment is applicable to any non-linear system with a state jump, as illustrated below. Then, as described above, the timing at which the state jump occurs in the nonlinear model prediction control may be specified.

For example, the nonlinear system according to the present embodiment may be a robot hand or a robot arm such as the arm (right arm 160R or left arm 160L) of the robot 100 shown in FIG. The state jump in this example can occur when the robot hand or robot arm presses an object such as the surrounding environment or an operation target, grips or releases the object, hits or hits an object such as a sphere, and the like. .. An example of a non-linear system that hits an object such as a sphere is a table tennis robot.

Further, for example, the nonlinear system according to the present embodiment may be a humanoid robot or an animal robot that can move by landing the arms and legs on the floor or the like at the same time. The state jump in this example is when the humanoid robot or the animal robot moves with its arms and legs in contact with a wall, floor, table, etc. at the same time, or when the humanoid robot or the animal robot moves on a ladder or It can occur when climbing a wall or the like.

Further, for example, the nonlinear system according to the present embodiment may be an unmanned aerial vehicle such as a drone. The state jump in this example can occur when the unmanned aerial vehicle touches or leaves an object to be operated or inspected, grips or releases an object to be transported or captured, and the like.

Further, for example, the nonlinear system according to the present embodiment may be a tool of a processing machine or the like. The state jump in this example can occur when the tool of the machining machine comes into contact with or separates from an object such as a machining target.

Further, for example, the nonlinear system according to the present embodiment may be a transmission of an automobile or the like. The state jump in this example may occur when the clutch of the transmission is in a contact state (power transmission state) or in a separated state (power cutoff state).

Further, for example, the nonlinear system according to the present embodiment may be a power source of a hybrid vehicle or the like. The state jump in this example can occur, for example, when the power source of the hybrid vehicle switches between the motor and the engine. Further, for example, the nonlinear system according to the present embodiment may be a battery of an electric vehicle, a hybrid vehicle, or the like. The state jump in this example can occur, for example, when the battery switches between charging and discharging.

Further, for example, the nonlinear system according to the present embodiment may be an automatic driving system such as an automobile. The state jump in this example may occur when the presence or absence of a preceding vehicle or a following vehicle changes due to a lane change or merging of the own vehicle. Further, the state jump in this example may occur when a collision with an object is unavoidable and the control is performed so as to perform the best possible operation including the situation after the collision.

Further, for example, the nonlinear system according to the present embodiment may be an airplane or the like. The state jump in this example can occur during takeoff 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 airframe are controlled so as to decelerate promptly after landing while landing on a desired route.

Further, for example, the nonlinear system according to the present embodiment may be a train or the like. The state jump in this example can occur in the connection of trains, such as when controlling to optimize the motion including before and after the connection. Specifically, it is a case where the motor is controlled so as to reduce the impact at the time of connection and the load of the drive motor.

For any nonlinear system as described above, the control method shown in FIG. 10 can be implemented. In this case, the control method of the nonlinear system obtains the vector U (t) represented by the equation (80) from the retention conditions represented by the equations (69) to (79), and each of the vectors U (t). The value of the component is calculated by solving the equation represented by the equation (81). As a result, the control input value for controlling the nonlinear system is calculated. Then, in the flowchart shown in FIG. 10, S102 corresponds to the acquisition step of acquiring the state parameter of the nonlinear system, and S104 to S114 corresponds to the calculation step of calculating the control input value for controlling the nonlinear system. , S116 correspond to the control steps that control the nonlinear system. Then, in the calculation step, the control input value is calculated using the constraint parameter that constrains the state of the nonlinear system so that the state changes discontinuously at the specified timing. At this time, the rate of change of the optimum solution of the control input value in the predetermined evaluation interval in the model prediction control algorithm is calculated for each control cycle of the nonlinear system, and the rate of change is used to calculate the next control cycle of the control cycle. The optimum solution of the control input value in the above is calculated, and the current control input value is calculated from the optimum solution.

In the above example, the program can be stored and supplied to a computer using various types of non-transitory computer readable media. Non-transitory computer-readable media include various types of tangible storage media. Examples of non-temporary computer-readable media include magnetic recording media (eg, flexible disks, magnetic tapes, hard disk drives), magneto-optical recording media (eg, magneto-optical disks), CD-ROMs (Read Only Memory), CD-Rs, It includes a CD-R / W and a semiconductor memory (for example, a mask ROM, a PROM (Programmable ROM), an EPROM (Erasable PROM), a flash ROM, and a RAM (Random Access Memory)). The program may also be supplied to the computer by various types of transient computer readable media. Examples of temporary computer-readable media include electrical, optical, and electromagnetic waves. The temporary computer-readable medium can supply the program to the computer via a wired communication path such as an electric wire and an optical fiber, or a wireless communication path.

1 ... Robot system, 2 ... Control device, 12 ... State acquisition unit, 14 ... Non-linear model prediction control unit, 16 ... Servo control unit, 100 ... Robot, 102 ... Body, 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, 201 ... Support leg lower leg link, 202 ... Support leg upper leg link, 203 ... Body link, 204 ... Free leg upper leg link, 205.・ ・ Free leg lower leg link, 211,212,213,214,215 ... Joint

Claims (8)

It is a control method for nonlinear systems.
An acquisition step for acquiring a state parameter indicating the state of the nonlinear system, and
A calculation step of calculating a control input value for controlling the nonlinear system using the model predictive control algorithm based on the acquired state parameters.
It has a control step for controlling the nonlinear system using the calculated control input value.
In the calculation step, a constraint parameter that constrains the state of the nonlinear system so that the state changes discontinuously at a specified timing is used in advance in the model prediction control algorithm for each control cycle of the nonlinear system. The rate of change of the optimum solution of the control input value in the defined evaluation interval is calculated, the optimum solution of the control input value in the control cycle next to the control cycle is calculated using the rate of change, and the optimum solution is used. , A control method for a nonlinear system that calculates the current control input value. A control device for a bipedal walking robot that controls the operation of a bipedal walking robot capable of bipedal walking using two legs.
A state acquisition means for acquiring a state parameter indicating a state related to walking of the bipedal walking robot, and
A calculation means for calculating a control input value for controlling the operation of the bipedal walking robot by using the model prediction control algorithm based on the acquired state parameters.
It has a control means for controlling the operation of the bipedal walking robot by using the calculated control input value.
The calculation means uses a restraint parameter that constrains the state of the bipedal walking robot so that the free leg of the two legs lands at a designated timing, and is used for each control cycle of the bipedal walking robot. , The rate of change of the optimum solution of the control input value in the predetermined evaluation interval in the model prediction control algorithm is calculated, and the rate of change is used to optimize the control input value in the next control cycle of the control cycle. A control device for a bipedal walking robot that calculates a solution and calculates the current control input value from the optimum solution. The control device for a bipedal walking robot according to claim 2, wherein the constraint parameter is included in an evaluation function used in the model prediction control algorithm. The control device for a bipedal walking robot according to claim 2 or 3, wherein the restraint parameter specifies the posture of the bipedal walking robot when the swing leg lands at the timing. The control device for a bipedal walking robot according to claim 4, wherein the restraint parameter specifies a target angle of the joints of the two legs when the swing leg lands at the timing. The control device for a bipedal walking robot according to any one of claims 2 to 5, wherein the constraint parameter includes an adjustable gain. It is a control method of a bipedal walking robot that controls the operation of a bipedal walking robot capable of performing bipedal walking using two legs.
An acquisition step for acquiring a state parameter indicating a state related to walking of the biped robot, and
Based on the acquired state parameters, a calculation step of calculating a control input value for controlling the operation of the bipedal walking robot using a model prediction control algorithm, and a calculation step.
It has a control step for controlling the operation of the bipedal walking robot using the calculated control input value.
In the calculation step, for each control cycle of the bipedal walking robot, a restraint parameter that constrains the state of the bipedal walking robot so that the free leg of the two legs lands at a designated timing is used. , The rate of change of the optimum solution of the control input value in the predetermined evaluation interval in the model prediction control algorithm is calculated, and the rate of change is used to optimize the control input value in the next control cycle of the control cycle. A control method for a bipedal walking robot that calculates a solution and calculates the current control input value from the optimum solution. It is a program that realizes a control method for a bipedal walking robot that controls the movement of a bipedal walking robot that can perform bipedal walking using two legs.
An acquisition step for acquiring a state parameter indicating a state related to walking of the biped robot, and
It is a calculation step of calculating a control input value for controlling the operation of the bipedal walking robot by using the model prediction control algorithm based on the acquired state parameters, and is the calculation step of calculating the control input value at a designated timing. Predetermined in the model predictive control algorithm for each control cycle of the bipedal walking robot using a constraint parameter that constrains the state of the bipedal walking robot so that the free leg of the two legs lands. The rate of change of the optimum solution of the control input value in the evaluation section is calculated, the optimum solution of the control input value in the control cycle next to the control cycle is calculated using the rate of change, and the present is obtained from the optimum solution. The calculation step for calculating the control input value of
A program that causes a computer to execute a control step that controls the operation of the bipedal walking robot using the calculated control input value.