Posture control method, posture control device, and program

Abstract

To provide a posture control method, a posture control device, and a program capable of controlling a nonlinear inverted pendulum whose height is variable without performing iterative calculations.SOLUTION: A posture control method is a control method for a nonlinear inverted pendulum model in which the height of the center of gravity is variable and the trajectory of the center of gravity is an energy-conserving system. A control device calculates a conserved energy function on the basis of a measured value of the center of gravity, converts the conserved energy function into a curved function, calculates a set of characteristic vectors as a convergence component and a divergence component by approximation of a controllable region without iterative computation in a phase diagram of the converted curved function, and feeds back the calculated convergence component and divergence component.SELECTED DRAWING: Figure 9

Description

[Technical Field]

[0001] The present invention relates to a posture control method, a posture control device, and a program. [Background technology]

[0002] In the walking control of bipedal robots, various methods have been proposed using a simplified model of a constant-height inverted pendulum (LIP (Linear Inverted Pendulum)). The motion of a constant-height inverted pendulum can be considered as being divided into a divergent component (DCM (Divergent Component of Motion)) and a convergent component (CCM (Convergent Component of Motion)). The divergent component in particular is widely used because it guarantees the continuity of the gait and can theoretically relax boundary conditions. Furthermore, the stabilizable range can be analytically derived by performing divergent component feedback control, allowing the construction of the best regulator in terms of stabilization.

[0003] Since the condition of a constant height is an unrealistic constraint when running or hopping, research is being conducted into control methods for a nonlinear inverted pendulum model with variable height (see, for example, Non-Patent Document 1). [Prior art documents] [Non-patent literature]

[0004] [Non-Patent Document 1] Ko Yamamoto, Takumi Kamioka,et al., 'Survey on model-based biped motion control for humanoid robots', ADVANCED ROBOTICS(2020), Volume 34, 2020 - Issue 21-22: Special Issue on Humanoid Robotics, p1353-1369, 2020 Summary of the Invention [Problem to be solved by the invention]

[0005] However, in the prior art, since it is difficult to obtain an analytical solution due to the variable height, it is necessary to obtain a solution numerically through iterative calculations.

[0006] The present invention has been made in consideration of the above-mentioned problems, and an object of the present invention is to provide an attitude control method, an attitude control device, and a program that can control a nonlinear inverted pendulum with a variable height without performing iterative calculations. [Means for solving the problem]

[0007] (1) In order to achieve the above object, an attitude control method according to one aspect of the present invention is a control method for a nonlinear inverted pendulum model in which the height of the center of gravity is variable and the trajectory of the center of gravity is an energy conservation system, in which a control device calculates a conservation energy function based on a measurement value of the center of gravity, converts the conservation energy function into a curve function, calculates a set of eigenvectors as convergent components and divergent components by approximation without iterative calculation of a controllable region (e.g., a region where the input is not saturated) in a phase diagram of the converted curve function, and feeds back the calculated convergent components and divergent components.

[0008] (2) In the attitude control method according to one aspect of (1) above, the trajectory of the center of gravity may be an ellipse or a hyperbola, and the control device may linearize the phase diagram of the curve function of the ellipse around the origin, and feed back a set of linearized eigenvectors as a convergent component and a divergent component.

[0009] (3) In the attitude control method according to one aspect of (1) above, the curve function may be a hyperbolic function, and the control device may feed back an asymptote of the curve described by the hyperbolic function as a convergent component and a divergent component of the nonlinear inverted pendulum model.

[0010] (4) In the attitude control method according to any one of the above (1) to (3), the conservation energy function may be set using a parameter.

[0011] (5) In the attitude control method according to one aspect of (2) above, the set λ=±ω′ of the linearized eigenvectors, which are the convergent component and divergent component, may be found from the following equation:

number

[0012] (6) In the attitude control method according to one aspect of the above (3), θ is a state quantity of the inverted pendulum expressed by a parameter, and θ · is the time derivative of θ, g is the gravitational acceleration, C is the divergence component denominator variable, X(θ) is a function, Ω(θ) is a function, and x is the position in the movement direction, and the asymptote may be found by the following equation.

number

[0013] (7) In the attitude control method according to one aspect of the above (1), K P is the gain and K V is the gain, θ is the state of the parametric inverted pendulum, and θ · is the time derivative of θ, and τ θ is the input to the system, ω is the gradient of the divergence component, and K P / K V and the control device may perform the feedback based on the following equation:

number

[0014] (8) In the attitude control method according to one aspect of the above (1), K P is the gain and K V is the gain, θ is the state of the parametric inverted pendulum, and θ· is the time derivative of θ, and τ θ is the input to the system, ω is the gradient of the divergence component, and K P / K V where Ω(θ) is a function of θ, and the control device may perform the feedback based on the following equation:

number

[0015] (9) In order to achieve the above object, an attitude control device according to one embodiment of the present invention is a control device for controlling the attitude of a moving body, and includes: a nonlinear inverted pendulum model in which the height of the center of gravity is variable and the trajectory of the center of gravity is an energy conservation system; a function calculation unit that calculates a conservation energy function based on a measurement value of the center of gravity; a transformation that converts the conservation energy function into a curve function; and a feedback unit that calculates a set of eigenvectors as convergent components and divergent components by approximation without iterative calculation of a controllable region (e.g., a region in which the input is not saturated) in a phase diagram of the converted curve function, and feeds back the calculated convergent components and divergent components.

[0016] (10) In order to achieve the above object, a program according to one aspect of the present invention is a program that causes a computer of a control device for controlling the attitude of a moving body, the control device having a nonlinear inverted pendulum model in which the height of the center of gravity is variable and the trajectory of the center of gravity is an energy conservation system, to calculate a conservation energy function based on measurements of the center of gravity, convert the conservation energy function into a curve function, calculate a set of eigenvectors as convergent components and divergent components by approximation without iterative calculation of a controllable region (e.g., a region in which the input is not saturated) in a phase diagram of the converted curve function, and feed back the calculated convergent components and divergent components. [Effects of the Invention]

[0017] According to the above (1) to (10), it is possible to control a nonlinear inverted pendulum with a variable height without performing iterative calculations. Also, according to the above (1) to (10), it is possible to derive a divergent component from the energy function without performing iterative calculations. [Brief explanation of the drawings]

[0018] [Figure 1] 1A and 1B are diagrams for explaining a phase plane, a divergent component, and a convergent component. [Figure 2] FIG. 1 illustrates an example of the configuration of a robot control system according to an embodiment. [Figure 3] 1A and 1B are diagrams for explaining a fixed-height inverted pendulum model and a variable-height inverted pendulum model. [Figure 4] FIG. 1 is an image diagram of a phase diagram when the divergence component of a height-variable inverted pendulum model is considered using conventional technology. [Figure 5] FIG. 10 is a diagram for explaining linearization of a divergent component and a convergent component. [Figure 6] FIG. 2 is a schematic block diagram of a stabilization control system. [Figure 7] FIG. 10 is a diagram for explaining symbols used in an equation of an inverted pendulum state. [Figure 8] This is a phase diagram when stabilization control is performed with kP and KV appropriately determined. [Figure 9] 4 is a flowchart of a process performed by a control device according to the first embodiment. [Figure 10] FIG. 1 is a diagram showing an example of a phase diagram in a nonlinear system. [Figure 11] This is a phase diagram with the horizontal axis being θ1 (rad / s) and the vertical axis being θ·1 (rad / s). [Figure 12] FIG. 10 is a diagram showing an example of a phase diagram when linearized near (θ, θ·)=(0, 0) according to the embodiment. [Figure 13] FIG. 10 is a diagram illustrating an example of a phase diagram when a gain is set using a nonlinear divergent component according to the embodiment. [Figure 14] 4 is a flowchart of a process performed by a control device according to the embodiment. DETAILED DESCRIPTION OF THE INVENTION

[0019] Hereinafter, embodiments of the present invention will be described with reference to the drawings. In the drawings used in the following description, the scale of each component is appropriately changed so that each component can be recognized. In all the drawings for explaining the embodiments, the same reference numerals are used for components having the same functions, and repeated explanations will be omitted. Furthermore, in this application, "based on XX" means "based on at least XX," and includes cases where it is based on other elements in addition to XX. Furthermore, "based on XX" is not limited to cases where XX is used directly, but also includes cases where it is based on XX that has been calculated or processed. "XX" is any element (for example, any information).

[0020] [overview] In this embodiment, for a walking robot, the divergence component is derived from the energy function without performing iterative calculations for a nonlinear inverted pendulum with variable height. Also, in this embodiment, by using a model that can derive curved trajectories including straight lines, control is performed without creating a trajectory using a piecewise linear model to obtain an analytical solution for the divergence component as in conventional technology.

[0021] [Overview of control using the inverted pendulum model, explanation of terms] First, an outline of the terms, principles, etc. used in this embodiment will be explained. A well-known example of a robot walking model is the inverted pendulum model, in which the center of gravity is located above the fulcrum. In such a model, the center of gravity is set on the body, and the fulcrum is set on the sole of the foot. Many robots with feet use the zero moment point (ZMP) for trajectory planning and control. The ZMP is synonymous with the center of pressure of the floor pressure.

[0022] In conventional technology, the height of the center of gravity during walking is kept constant by leg force f. In inverted pendulum motion with a constant height, the ZMP can be fixed at the origin to establish an equation of motion. In this equation of motion, the time derivative of the center of gravity position is velocity, and the time derivative of velocity is acceleration. A vector like position and velocity that represents the instantaneous state of an object is called a state vector. This state vector can be expressed as, for example, in Figure 1, with the position of the center of gravity x (m) on the horizontal axis and the velocity of the center of gravity x on the vertical axis. · The plane expressed in m / s is called the phase plane. Figure 1 is a diagram for explaining the phase plane, divergent component, and convergent component. In Figure 1, the dashed line g1 represents the asymptote (separate line) of the divergent component (DCM), and the dashed line g2 represents the asymptote (separate line) of the convergent component (CCM). The divergent component is x + (x · / ω), where ω is the slope of the divergence component, ω = √(g / b), b is the height from the floor to the center of gravity, and g is the acceleration due to gravity (see, for example, Reference 1).

[0023] Reference 1: Hideji Kajita (ed.), "Humanoid Robots (Revised 2nd Edition)", Ohmsha, pp. 132-219, 2020

[0024] [Robot Control System] Next, a configuration example of the robot control system 1 of this embodiment will be described. Fig. 2 is a diagram showing a configuration example of the robot control system of this embodiment. The robot control system 1 includes, for example, a robot 2 and a control device 3. The control device 3 may be included in the robot 2. The robot 2 includes, for example, a first foot 21, a second foot 22, a sensor 23, a body 24, a control unit 25, a communication unit 26, and a memory unit 27. The robot 2 may also include a power source, an arm, a head, and the like. The control device 3 includes, for example, an acquisition unit 31, a model 32, a function calculation unit 33, a conversion unit 34, a feedback unit 35, a control unit 36, an output unit 37, and a storage unit 38.

[0025] (robot) The robot 2 is, for example, a bipedal robot. The robot 2 transmits and receives information to and from the control device 3 via a wired or wireless network NW.

[0026] The crotch of each of the first foot 21 and the second foot 22 is connected to the body via a joint. Each of the first foot 21 and the second foot 22 is, for example, a lower limb that includes a thigh, a lower leg, and a foot, and includes joints at the crotch, knee, and ankle. Each joint is also equipped with an encoder (sensor 23). Each of the first foot 21 and the second foot 22 is also equipped with an actuator at each joint.

[0027] The sensors 23 are, for example, encoders attached to each joint, acceleration sensors attached to the feet, force sensors, gyro sensors, etc. The sensors 23 detect, for example, the waist link posture, floor reaction force, joint angle, etc.

[0028] The body 24 is connected to a first leg 21 and a second leg 22. The body 24 may also be connected to an arm or a head.

[0029] The control unit 25 outputs the detection results obtained by the sensor 23 to the control device 3 via the communication unit 26. The control unit 25 controls the operation of the feet (first foot 21, second foot 22) according to the control value, control command, or drive signal output by the control device 3.

[0030] The communication unit 26 outputs the detection value output by the control unit 25 to the control device 3. The communication unit 26 acquires the control value, control command, or drive signal output by the control device 3.

[0031] The storage unit 27 stores programs, thresholds, mathematical expressions, etc. that the control unit 25 uses for control, etc.

[0032] (Control device) The control device 3 is, for example, a personal computer.

[0033] The acquisition unit 31 acquires the detection values ​​output by the robot 2. The acquisition unit 31 may also acquire operation instructions set or input by the operator of the robot 2.

[0034] The model 32 is, for example, a nonlinear inverted pendulum model whose orbit is an energy-conserving system.

[0035] The function calculation unit 33 calculates the ZMP position, the center of gravity position, etc., using the detection results detected by the sensor 23. The function calculation unit 33 calculates the energy function of the conservative system using the calculated ZMP position, the center of gravity position, etc., and the model 32.

[0036] The conversion unit 34 converts the conservative energy function into a curved (elliptic or hyperbolic) function format. The curved line may or may not include a perfect circle.

[0037] The feedback unit 35 linearizes an elliptic curve, for example, near the origin, or calculates a nonlinear asymptote of a hyperbola. The feedback unit 35 feedback-controls the set of linearized eigenvalue vectors or the asymptote as a convergent or divergent component of the model 32.

[0038] The control unit 36 ​​generates a control value, a control command, or a drive signal using the convergent and divergent components that are fed back.

[0039] The output unit 37 outputs a control value, a control command, or a drive signal to the robot 2.

[0040] The storage unit 38 stores programs, thresholds, formulas, etc. used by each unit of the control device 3.

[0041] [Height-variable inverted pendulum model] Next, the height-variable inverted pendulum model used in this embodiment will be described. FIG. 3 is a diagram for explaining a fixed-height inverted pendulum model and a variable-height inverted pendulum model.

[0042] The image g10 shows the trajectory of the center of gravity of an inverted pendulum model with a fixed height. The circle g11 indicates the center of gravity, and the dashed line g12 indicates the trajectory. In the case of an inverted pendulum model with a fixed height, as shown in g12, the trajectory moves along the circumference of a circle.

[0043] The image g20 shows the trajectory of the center of gravity of the height-variable inverted pendulum model of this embodiment. pend and the dashed line g22 is the mass point p pend The circle g23 is the center of gravity of the waist and its position p hip The chain line g24 is the center of gravity of the waist p hip The circle g25 is the locus of the mass point and its position p impend and the dashed line g26 is the mass point p impend This is the trajectory of In this embodiment, efficient control is performed by actively changing the height, as in the image g20 in FIG.

[0044] A control method that takes into account the divergence component of a height-variable inverted pendulum model has been proposed (see Non-Patent Document 1). However, with this conventional technology, an analyzable phase diagram cannot be drawn due to the presence of an integral term (3g∫·dx) as in the conserved energy function of the following equation (1). Since the vertical and horizontal axes change from moment to moment as shown in Figure 4, it is difficult to obtain a solution. Therefore, the control input is determined by iterative calculations such as MPC (Model Predictive Control) and SQP (Sequential Quadratic Programming). Figure 4 is an image of a phase diagram when the divergence component of a height-variable inverted pendulum model is taken into account using conventional technology. Note that each curved arrow indicates how the position and velocity of the center of gravity on the phase plane change after a minimum time. In equation (1), x G is the position of the COM (Center of Mass) in the x-axis direction, and h(x G ), f(x G ) is x G is a function of

[0045]

number

[0046] [Linearization of divergent and convergent components] Next, linearization of the divergent and convergent components will be described. Fig. 5 is a diagram for explaining linearization of the divergent and convergent components.

[0047] The diagram with reference symbol g100 is a conventional inverted pendulum model with a fixed height. Point O is the fulcrum, x is the position in the x-axis direction, b is the height in the y-axis direction, and f is the leg force. In this case, the height is kept constant by the leg force f, so the state vector is expressed by the following equation (2).

[0048]

number

[0049] The divergent and convergent components are x · =λx, where λ is given by the following equation (3):

[0050]

number

[0051] As shown in equations (2) and (3), the solution of the conventional inverted pendulum model with a fixed height can only be found as a linear system.

[0052] <First Example> In contrast, in this embodiment, as shown in image g110 in Figure 5, an ellipse g111 where x = x(θ) and y = y(θ) is drawn depending on the leg force f. In this embodiment, an example will be described in which the trajectory of the center of gravity is an ellipse or a hyperbola. Because the height of the center of gravity is variable, it is assumed here that the trajectory of the center of gravity follows an ellipse. Note that the chain line g112 is a curve in the case of a perfect circle. Note that θ is a state quantity of the inverted pendulum expressed as a parameter.

[0053] Here, the following equation (4) holds under the conditions of (a·sin(θ), b·cos(θ)) (ellipse), (a·sinh(θ), b·cosh(θ)), etc. Note that under the conditions, a is a constant value, and α is a constant value in symbol g110 in Figure 5. Also, θ is the angle between the y-axis (vertical direction) and leg force f.

[0054]

number

[0055] Also, C is the denominator variable of the divergence component, and is expressed as y(δx / δθ)·x(δx / δθ). In particular, the ellipse p=[x, y] T (T is transpose)=A[e c3θ ,e -c3θ ]T (c3 in the formula is c3) and the hyperbola p=A[sinhθ,coshθ] T When expressed as a parameter, C becomes a constant and its value is -2|A|c3. Note that A is a tensor matrix containing complex numbers as components, and c3 is a constant containing complex numbers. Also, the linear trajectory p=A|θ+h x , h y | or p = A | θ + h x , θ+h y In the case of |, C is a constant, and h y , h y -h x This becomes: Furthermore, (θ,θ · )=(0,0) and linearize it as follows: When equation (4) is approximated as in equation (5) below, equation (2) becomes equation (6) below.

[0056]

number

[0057]

number

[0058] From equation (6), the set λ of eigenvalue vectors is given by the following equation (7): This set of eigenvectors is the divergent component and the convergent component.

[0059]

number

[0060] In this case, (q, q · )=(0,0) can be approximated linearly, but cannot be approximated if it is far away from the vicinity.

[0061] [Stabilization control of walking robots] Here, an outline of the control system of the walking robot will be explained. Fig. 6 is a schematic block diagram of a stabilization control system for a walking robot. As shown in Fig. 6, the stabilization control system includes, for example, a robot 401, a center of gravity / ZMP measurement 402, a center of gravity / ZMP feedback 403, a ZMP distribution 404, and a ground reaction force control 405.

[0062] The robot 401 is a bipedal robot equipped with sensors. Center of gravity and ZMP measurement 402 calculates and obtains the ZMP and center of gravity following the detection results (waist link posture, floor reaction force, joint angle) detected by the sensors of the robot 401 . The center of gravity / ZMP feedback 403 calculates a target ZMP based on the center of gravity / ZMP planned trajectory included in the command value and the center of gravity and ZMP based on the actual measured values ​​of the robot 401 . The ZMP distribution 404 converts the desired ZMP into a desired ground reaction force. The floor reaction force control 405 calculates the target joint angle by inverse kinematics based on the joint angle pattern included in the command value and the target floor reaction force, and controls the movement of the robot 401 . For details of each process and calculation method, please refer to Reference 1.

[0063] In a practical state, the robot 401 can be made to walk by controlling it based on command values. However, in reality, the robot 401 may fall over due to unevenness on the floor, etc. Therefore, in order to stably control the robot 401, it is necessary to correct and control the command values ​​using (feedback from) the detection results of the sensors that the robot 401 has.

[0064] [Stabilization control of an inverted pendulum model] Next, the stabilization control of the inverted pendulum model will be described. FIG. 7 is a diagram for explaining the symbols used in the equation of state of an inverted pendulum. x is the position of the fulcrum in the x-axis direction, pL is the end position of the heel, pH is the end position of the toes, and b is the height from the floor (sole of the foot) to the center of gravity. Figure 7 shows an inverted pendulum model with a cart. The state equation of the inverted pendulum can be expressed by the following equation (8).

[0065]

number

[0066] In equation (8), p x is the ZMP input and is stabilized by the following equation (9). P , K. V is the gain.

[0067]

number

[0068] Since there is a ZMP constraint in this equation, the phase diagram of the inverted pendulum is as shown in Figure 8. P , K. V This is a phase diagram when stabilization control is performed by appropriately determining The dashed lines g152 and g162 are divergent components. The dashed lines g153 and g163 are convergent components. The slope of the divergent and convergent components is ω. Point g151 is pL, and point g161 is pH. The dashed lines g152, g153, g162, and g163 that divide the regions are also called dividing lines. The input of line g171 is pL=K P x+K V x · The slope is -K p / K V The input of line g172 is pH=K P x+K V x·.

[0069] The region g181 enclosed by the line g171 and the dashed line g153 is the convergence region. The region g182 surrounded by the lines g171 and g172 is a region where the input is not saturated. Region g183 is the region where the input is saturated. The region g184 surrounded by the dashed line 153 and the line 171 is a region where stabilization is not possible even if the input is started from a region where it is not saturated. The region g184 surrounded by the dashed line 163 and the line 172 is a region where stabilization is not possible even if the input is started from a region where it is not saturated.

[0070] Saturation refers to a state in which the curve with the arrow shows a change such as a parabola and does not converge to the origin. Therefore, in Fig. 8, the region g182 is the controllable region (controllable region), and the other regions are saturated regions. Also, the example in Fig. 8 is an example in which the slope of the controllable region does not match the slope of the dividing line (asymptote g153, g163).

[0071] In the control that feeds back the divergence component, K P , K. V satisfies the following equation (10).

[0072]

number

[0073] Substituting equation (12) into equation (11), p x is expressed as the following equation (11), (x+x · / x) is the divergent component.

[0074]

number

[0075] The control of the divergence component through feedback has the following advantages: I. It is the optimal regulator in the sense that it can maximize the regulation margin. II. The stabilization limit can be calculated analytically. For example, in the case of a bipedal robot, it can be used to determine the timing of taking a step to avoid falling, and in riding assistance, it can be applied to assessing the risk of falling.

[0076] However, as mentioned above, in the prior art, when the height is varied, the axis changes from moment to moment, making it difficult to define the divergence component of a general inverted pendulum. For information on height-variable inverted pendulum models, see, for example, Non-Patent Document 1, References 2 and 3.

[0077] Reference 2;Caron Stephane, “Biped Stabilization by Linear Feedback of the Variable-Height Inverted Pendulum Model”, IEEE Conference Proceedings, IEEE, 2020 Reference 3; Garcia-Chavez G., “A Control Approach for the Variable-Height Inverted Pendulum Based on Sliding Mode Control With Input Saturation”, IEEE Conference Proceedings, IEEE, 2019

[0078] For this reason, in this embodiment, the state quantity is in a parameter space (e.g., θ), and the divergent and convergent components are linearized by drawing a curved circle (FIG. 5) where x = x(θ) and y = y(θ) depending on the leg force f. Note that this curve is a line obtained by function calculation unit 33 calculating the conservation energy function of model 32 and then by conversion unit 34 converting the conservation energy function into a hyperbolic function form. In the case of this feedback control, the gradient ω is constant as shown in equation (5).

[0079] [Example of processing procedure] Next, a description will be given of an example of a processing procedure performed by the control device 3. Fig. 9 is a flowchart of the processing performed by the control device according to this embodiment.

[0080] (Step S1) The acquisition unit 31 acquires the detection result output by the robot 2.

[0081] (Step S2) The function calculation unit 33 uses the acquired detection results to obtain the ZMP position, the center of gravity position, and the like.

[0082] (Step S3) The function calculation unit 33 uses the calculated ZMP position, center of gravity position, etc. and the model 32 to calculate the energy function of the conservative system.

[0083] (Step S4) The conversion unit 34 converts the conservative energy function into, for example, an elliptical curve function format. Note that the curve function is not limited to an ellipse, and may be any other shape than a perfect circle with a variable height.

[0084] (Step S5) The feedback unit 35 calculates the divergent component and the convergent component (Equation (6)).

[0085] (Step S6) The feedback unit 35 performs feedback control of the divergent component and the convergent component as the convergent component and the divergent component of the model 32.

[0086] (Step S7) The control unit 36 ​​generates a control value, a control command, or a drive signal using the convergent and divergent components that are fed back. The control unit 36 ​​outputs the control value, the control command, or the drive signal to the robot 2 via the output unit 37.

[0087] 9 is an example, and is not limiting. For example, some of the processes may be performed in parallel.

[0088] As described above, in this embodiment, the tilt ω is constant, the state quantity is task-based (for example, x, y, z coordinates), and walking control is performed using a height-variable inverted pendulum model. In this embodiment, the tilt ω is constant, the state quantity is a parameter space (for example, θ), and walking is controlled using a height-variable inverted pendulum model. As described above, according to this embodiment, it is possible to realize a method for deriving a divergence component from an energy function and stabilization control using the same, even for a nonlinear inverted pendulum with a variable height, without performing iterative calculations as in the conventional technology.

[0089] In the prior art, in order to obtain an analytical solution for the divergent component, it was necessary to create a trajectory using a piecewise linear model. In contrast, according to the present embodiment, instead of creating the trajectory using a piecewise linear model, the trajectory of the center of gravity is expressed, for example, by an elliptical equation, and then linearized and approximated near the origin of the phase diagram, thereby making it possible to obtain an analytical solution consisting of divergent and convergent components.

[0090] <Second Example> [Generalization of divergent and convergent components] In the above example, the slope ω is constant. In the following example, the slope ω is generalized to be variable. As shown in Figure 10, even if the system is nonlinear, if it is a conservative system, the convergent and divergent components can be found using the energy formula. Figure 10 shows an example of a phase diagram for a nonlinear system. For this reason, in the following example, the divergent component is found in energy form.

[0091] First, let us look at the following equation (12) for conservation of energy: y is the height of the center of gravity.

[0092]

number

[0093] Transforming equation (12) into hyperbolic form gives the following equation (13).

[0094]

number

[0095] Asymptotes (separator lines) of the hyperbolic form (13) x · is expressed by the following equation (14). Note that the asymptote x · is a set of eigenvalue vectors as described above, and therefore corresponds to a divergent component and a convergent component.

[0096]

number

[0097] (q, q · ) = (0,0), the bankruptcy component of the nonlinear system must be determined strictly for the above example. Therefore, here q ·· = (g / c)x(q), if X(q) = ∫s(q)dq, the energy conservation equation can be transformed into the following equation (15), which can be transformed into the following equation (16) by converting equation (15) into hyperbolic form. Note that C is a constant.

[0098]

number

[0099]

number

[0100] Asymptote θ of hyperbolic form (16)· (g201, g203) are expressed by the following equation (17). Note that Ω(θ) is √(·)θ. Note that line g201 is the asymptote of the divergent component, and line g203 is the asymptote of the convergent component.

[0101]

number

[0102] In Figure 11, the horizontal axis is θ1 (rad / s) and the vertical axis is θ · 1 (rad / s). In FIG. 11, the dotted lines g202 and g204 are linearized.

[0103] Using equation (15) and symbol g110 in FIG. 5, the state equation of the inverted pendulum can be expressed as the following equation (18).

[0104]

number

[0105] In addition, τ θ is the input and τ θ =K P θ+K V Stabilize at θ. In this embodiment, the input constraint of the following equation (19) is set.

[0106]

number

[0107] [evaluation] Next, the above (q,q · We will explain the results of simulations conducted to evaluate the case where the gain is linearized near q = (0,0) (q is, for example, θ) and the case where the gain is set using a nonlinear divergence component. Note that the simulations use a nonlinear inverted pendulum model with a flywheel.

[0108] where (q,q ·)=(0,0) (q is, for example, θ) when linearized near the input τ θ is expressed by the following equation (20). In addition, when the gain is set using a nonlinear divergence component, the input τ θ is expressed by the following equation (21).

[0109]

number

[0110]

number

[0111] FIG. 12 shows the relationship between (θ, θ · 11 shows an example of a phase diagram when linearized near θ = (0,0). The horizontal and vertical axes are the same as in FIG. 11. Note that the example in FIG. 11 shows that the gradient of the controllable region and the gradient of the section line (asymptote) match.

[0112] In this case, as shown in Figure 12, (θ,θ · In the vicinity of θ1 = (0,0), the region where the input does not saturate is within the region g251 where the input can be stabilized. However, in the vicinity of θ1 = -0.5π, the region where the input does not saturate extends beyond the region where the input can be stabilized (symbol g252). In addition, in the vicinity of θ1 = 0.5π, the region where the input does not saturate extends beyond the region where the input can be stabilized (symbol g253). In this way, when (θ,θ · When linearized near )=(0,0), the nonlinearity can be stabilized and controlled within a specified range.

[0113] As a result, according to this embodiment, the analytical solution to the energy equation can be obtained with a smaller amount of calculation than in the prior art.

[0114] Fig. 13 is a diagram showing an example of a phase diagram when a gain is set using a nonlinear divergence component according to this embodiment. The horizontal and vertical axes are the same as those in Fig. 11. Note that the example in Fig. 13 is an example in which the slope of the controllable region and the slope of the division line (asymptote) coincide with each other.

[0115] In this case, as shown in Figure 13, (θ,θ · When θ1 is near θ=(0,0), and when θ1 is near -0.5π and 0.5π, the region where the input does not saturate falls within the region where it can be stabilized. In this way, setting the gain using a nonlinear divergence component allows for stable control over a wide input range.

[0116] The evaluation confirmed that, according to this embodiment, by performing more general-purpose energy feedback control than ZMP input on a nonlinear inverted pendulum model with a flywheel, it is possible to perform robust stabilization control for any trajectory. This embodiment is applicable even when the nonlinearity is large. According to this embodiment, although the divergence component changes depending on the angle, it is possible to obtain an analytical solution to the energy equation without the need for iteration calculations.

[0117] [Example of processing procedure] Next, a description will be given of an example of a processing procedure performed by the control device 3. Fig. 14 is a flowchart of the processing performed by the control device according to this embodiment.

[0118] (Step S11) The acquisition unit 31 acquires the detection result output by the robot 2.

[0119] (Step S12) The function calculation unit 33 uses the acquired detection results to obtain the ZMP position, the center of gravity position, and the like.

[0120] (Step S13) The function calculation unit 33 uses the calculated ZMP position, center of gravity position, etc. and the model 32 to calculate the energy function of the conservative system.

[0121] (Step S14) The conversion unit 34 converts the energy function of the conservative system into a hyperbolic function format.

[0122] (Step S15) The feedback unit 35 calculates the asymptote (separation line) of the hyperbola.

[0123] (Step S16) The feedback unit 35 performs feedback control of the asymptote as a convergent component and a divergent component of the model 32.

[0124] (Step S17) The control unit 36 ​​generates a control value, a control command, or a drive signal using the convergent and divergent components that are fed back. The control unit 36 ​​outputs the control value, the control command, or the drive signal to the robot 2 via the output unit 37.

[0125] In the embodiment, the tilt ω is constant, the state quantity is task-based (for example, x, y, z coordinates), and walking control is performed using a height-variable inverted pendulum model. In this embodiment, the tilt ω is variable and the state quantity is a parameter space (for example, θ), and walking is controlled using a height-variable inverted pendulum model.

[0126] As described above, according to this embodiment, it is possible to realize a method for deriving a divergence component from an energy function and stabilization control using the same, even for a nonlinear inverted pendulum with a variable height, without performing iterative calculations as in the conventional technology. In the prior art, to obtain an analytical solution for the divergence component, it was necessary to create a trajectory using a piecewise linear model. In contrast, according to the embodiment, by using a model capable of deriving a curved trajectory including straight lines, it is possible to obtain an analytical solution without creating a trajectory using a piecewise linear model.

[0127] In the above-described embodiments, the bipedal robot 2 has been described as an example of a moving body, but the moving body whose posture is controlled is not limited to this. The control methods of the embodiments can be applied to moving bodies such as two-wheeled vehicles (electric bicycles, motorbikes, etc.).

[0128] A program for implementing all or part of the functions of the control device 3 of the present invention may be recorded on a computer-readable recording medium, and the program recorded on the recording medium may be loaded into a computer system and executed to perform all or part of the processing performed by the control device 3. The term "computer system" as used herein includes hardware such as an OS and peripheral devices. The term "computer system" also includes a WWW system equipped with a homepage provision environment (or display environment). The term "computer-readable recording medium" refers to portable media such as flexible disks, optical magnetic disks, ROMs, and CD-ROMs, as well as storage devices such as hard disks built into computer systems. The term "computer-readable recording medium" also includes devices that retain a program for a certain period of time, such as volatile memory (RAM) within a computer system that acts as a server or client when the program is transmitted via a network such as the Internet or a communication line such as a telephone line. Alternatively, some or all of these components may be realized by LSI (Large Scale Integration) hardware (including circuitry) such as an ASIC (Application Specific Integrated Circuit), FPGA (Field-Programmable Gate Array), GPU (Graphics Processing Unit), or SOC (System On Chip), or may be realized by a combination of software and hardware.

[0129] The program may also be transmitted from a computer system storing the program in a storage device or the like to another computer system via a transmission medium or by transmission waves in the transmission medium. Here, the "transmission medium" that transmits the program refers to a medium that has the function of transmitting information, such as a network (communication network) such as the Internet or a communication line (communication line) such as a telephone line. The program may also be a program that realizes part of the above-mentioned functions. Furthermore, the program may be a so-called differential file (differential program) that can realize the above-mentioned functions in combination with a program already recorded in the computer system.

[0130] The above describes the form for carrying out the present invention using an embodiment, but the present invention is not limited to such an embodiment, and various modifications and substitutions can be made within the scope that does not deviate from the gist of the present invention. [Explanation of symbols]

[0131] 1...Robot control system, 2...Robot, 3...Control device, 21...First foot, 22...Second foot, 23...Sensor, 24...Body, 25...Control unit, 26...Communication unit, 27...Memory unit, 31...Acquisition unit, 32...Model, 33...Function calculation unit, 34...Conversion unit, 35...Feedback unit, 36...Control unit, 37...Output unit, 38...Memory unit

Claims

1. A control method for a nonlinear inverted pendulum model in which the height of the center of gravity is variable and the trajectory of the center of gravity is an energy conservation system, comprising: The control device determining a conservation energy function based on the center of gravity measurements; converting the conservation energy function into a curve function; A set of eigenvectors is calculated as convergent components and divergent components by approximation without iterative calculation of a controllable region in the phase diagram of the transformed curve function, and the calculated convergent components and divergent components are fed back. Attitude control method.

2. the orbit of the center of gravity is an ellipse or a hyperbola; The control device Linearizing the phase diagram of the elliptic curve function around the origin, and feeding back a set of linearized eigenvectors as convergent and divergent components; The attitude control method according to claim 1 .

3. the curve function is a hyperbolic function, The control device asymptote of the curve described by the hyperbolic function is fed back as a convergent component and a divergent component of the nonlinear inverted pendulum model; The attitude control method according to claim 1 .

4. The conservation energy function is set using parameters.

3. The attitude control method according to claim 1.

5. The set of linearized eigenvectors λ=±ω′, which are the convergent and divergent components, is obtained from the following equation: [Equation 1] The attitude control method according to claim 2 .

6. θ is the state of the parametrically expressed inverted pendulum, and θ ・ is the time derivative of θ, g is the gravitational acceleration, C is the denominator variable of the divergence component, X(θ) is a function, Ω(θ) is a function, x is the position in the direction of movement, The asymptote is calculated using the following formula: [Equation 2] The attitude control method according to claim 3 .

7. K Ｐ is the gain, and K Ｖ is the gain, θ is the state of the parametric inverted pendulum, and θ ・ is the time derivative of θ, and τ θ is the input to the system, ω is the gradient of the divergence component, and K Ｐ / K Ｖ and The controller performs the feedback based on the following equation: [Equation 3] The attitude control method according to claim 2 or 5.

8. K Ｐ is the gain, and K Ｖ is the gain, θ is the state of the parametric inverted pendulum, and θ ・ is the time derivative of θ, and τ θ is the input to the system, ω is the gradient of the divergence component, and K Ｐ / K Ｖ and Ω(θ) is a function of θ, The controller performs the feedback based on the following equation: [Equation 4] The attitude control method according to claim 3 or 6.

9. A control device for controlling the attitude of a moving body, A nonlinear inverted pendulum model in which the height of the center of gravity is variable and the trajectory of the center of gravity is an energy conservation system. a function calculation unit that calculates a conservation energy function based on the measurement value of the center of gravity; a transformation that transforms the conservation energy function into a curve function; a feedback unit that calculates a set of eigenvectors as convergent components and divergent components by approximation without iterative calculation of a controllable region in the phase diagram of the transformed curve function, and feeds back the calculated convergent components and divergent components; An attitude control device comprising:

10. a computer of a control device for controlling the attitude of a moving body, the computer having a nonlinear inverted pendulum model in which the height of the center of gravity is variable and the trajectory of the center of gravity is an energy conservation system; determining a conservation energy function based on the center of gravity measurements; converting the conservation energy function into a curve function; In the phase diagram of the transformed curve function, a set of eigenvectors is calculated as convergent components and divergent components by approximation without iterative calculation of the controllable region, and the calculated convergent components and divergent components are fed back. program.

Images