POSTURE CONTROL METHOD, POSTURE CONTROL DEVICE, AND STORAGE MEDIUM

Abstract

A posture control method is a posture control method of a non-linear inverted pendulum model, of which a height of a center of gravity is variable, and a trajectory of the center of gravity is an energy conserving system, using a control device, the posture control method including: obtaining a conservation energy function on the basis of a measured value of the center of gravity; converting the conservation energy function into a curve function; and calculating a set of eigenvectors through approximation as a convergent component and a divergent component without iterative calculation of a controllable area in a phase plot of the converted curve function and feeding back the convergent component and the divergent component that have been calculated.

Claims

1. A posture control method of a non-linear inverted pendulum model, of which a height of a center of gravity is variable, and a trajectory of the center of gravity is an energy conserving system, using a control device, the posture control method comprising: obtaining a conservation energy function on the basis of a measured value of the center of gravity; converting the conservation energy function into a curve function; and calculating a set of eigenvectors through approximation as a convergent component and a divergent component without iterative calculation of a controllable area in a phase plot of the converted curve function and feeding back the convergent component and the divergent component that have been calculated.

2. The posture control method according to claim 1, wherein the trajectory of the center of gravity is an ellipse or a hyperbola, and wherein the control device linearizes the set of eigenvectors around the origin of a phase plot of the curve function of the ellipse and feeds back the linearized set of eigenvectors as a convergent component and a divergent component.

3. The posture control method according to claim 1, wherein the curve function is a hyperbolic function, and wherein the control device feeds back an asymptote of a curve drawn by the hyperbolic function as a convergent component and a divergent component of the non-linear inverted pendulum model.

4. The posture control method according to claim 1, wherein the conservation energy function is set using a parametric variable.

5. The posture control method according to claim 2, wherein the linearized set = of eigenvectors that is the convergent component and the divergent component is obtained from the following equation: $\frac{d}{\mathrm{dt}}\left[\begin{array}{c}\\ \stackrel{.}{}\end{array}\right]=\left[\begin{array}{cc}0& 1\\ {}^2& 0\end{array}\right]\left[\begin{array}{c}\\ \stackrel{.}{}\end{array}\right].$

6. The posture control method according to claim 3, wherein is a state quantity of an inverted pendulum expressed in a parametric variable, .sup. is a time derivative of , g is a gravitational acceleration, C is a divergent component denominator variable, X() is a function, () is a function, and x is a position in a movement direction, and wherein the asymptote is obtained using the following equation: $\stackrel{.}{}=\sqrt{\frac{\mathrm{gX}\left(\right)}{C{}^2}}=\left(\right)x.$

7. The posture control method according to claim 2, wherein K.sub.P is a gain, K.sub.V is a gain, is a state quantity of an inverted pendulum expressed in a parametric variable, .sup. is a time derivative of , .sub. is an input to a system, and is a slope of the divergent component and is K.sub.P/K.sub.V, and wherein the control device performs the feedback on the basis of the following equation: ${}_{}=K_P+K_V\stackrel{.}{}=K_P\left(+\frac{1}{}\stackrel{.}{}\right).$

8. The posture control method according to claim 3, wherein K.sub.P is a gain, K.sub.V is a gain, is a state quantity of an inverted pendulum expressed in a parametric variable, .sup. is a time derivative of , .sub. is an input to a system, is a slope of the divergent component and is K.sub.P/K.sub.V, and () is a function of , and wherein the control device performs the feedback on the basis of the following equation: ${}_{}=K_P+K_V\stackrel{.}{}=K_P\left(+\frac{1}{\left(\right)}\stackrel{.}{}\right).$

9. A posture control device controlling a posture of a mobile body, the posture control device comprising: a non-linear inverted pendulum model of which a height of a center of gravity is variable, and a trajectory of the center of gravity is an energy conserving system; a function calculating unit obtaining a conservation energy function on the basis of a measured value of the center of gravity; a conversion unit converting the conservation energy function into a curve function; and a feedback unit calculating a set of eigenvectors through approximation as a convergent component and a divergent component without iterative calculation of a controllable area in a phase plot of the converted curve function and feeding back the convergent component and the divergent component that have been calculated.

10. A computer-readable non-transient storage medium storing a program causing a computer of a control device, which controls a posture of a mobile body, having a non-linear inverted pendulum model of which a height of a center of gravity is variable, and a trajectory of the center of gravity is an energy conserving system, to: obtain a conservation energy function on the basis of a measured value of the center of gravity; convert the conservation energy function into a curve function; and calculate a set of eigenvectors through approximation as a convergent component and a divergent component without iterative calculation of a controllable area in a phase plot of the converted curve function and feed back the convergent component and the divergent component that have been calculated.

Description

BRIEF DESCRIPTION OF THE DRAWINGS

[0020] FIG. 1 is a diagram for describing a phase plane, a divergent component, and a convergent component.

[0021] FIG. 2 is a diagram illustrating a configuration example of a robot control system according to an embodiment.

[0022] FIG. 3 is a diagram for describing a height-fixed inverted pendulum model and a height-variable inverted pendulum model.

[0023] FIG. 4 is an image diagram of a phase plot obtained in a case in which a divergent component of a height-variable inverted pendulum model is considered using a conventional technology.

[0024] FIG. 5 is a diagram for describing linearization of a divergent component and a convergent component.

[0025] FIG. 6 is a schematic block diagram of a stabilization control system.

[0026] FIG. 7 is a diagram for describing symbols used in a state equation of an inverted pendulum.

[0027] FIG. 8 is a phase plot obtained when stabilization control is performed by appropriately determining k.sub.P and K.sub.V.

[0028] FIG. 9 is a flowchart of a process performed by a control device according to a first embodiment.

[0029] FIG. 10 is a diagram illustrating an example of a phase plot of a non-linear system.

[0030] FIG. 11 is a phase plot in which the horizontal axis is .sub.1 (rad/s), and the vertical axis is .sup..sub.1 (rad/s).

[0031] FIG. 12 is a diagram illustrating an example of a phase plot obtained in a case in which linearization is performed near (, .sup.)=(0, 0) in accordance with an embodiment.

[0032] FIG. 13 is a diagram illustrating an example of a phase plot obtained in a case in which a gain is set using a non-linear divergent component according to an embodiment.

[0033] FIG. 14 is a flowchart of a process performed by a control device according to an embodiment.

DETAILED DESCRIPTION OF THE INVENTION

[0034] Hereinafter, an embodiment of the present invention will be described with reference to the drawings. In drawings used in the following description, in order to allow each member to have a recognizable size, the scales of respective members are appropriately changed.

[0035] In all the drawings used for describing an embodiment, the same reference signs will be used for members having the same function, and repetitive description will be omitted.

[0036] On the basis of XX in description in the present application means at least on the basis of XX, and a case of being on the basis of other elements in addition to XX is included therein. On the basis of XX is not limited to a case in which XX is directly used, and a case of being on the basis of a result obtained by performing an arithmetic operation or processing on XX is also included therein. XX is an arbitrary element (for example, arbitrary information).

[Overview]

[0037] In this embodiment, a walkable robot derives a divergent component from an energy function without performing iterative calculation for a height-variable non-linear inverted pendulum. In this embodiment, by using a model that is able to derive a curved trajectory including a straight line, as in a conventional technology, in order to obtain an analytical solution of a divergent component, the trajectory is controlled without generating the trajectory using a piecewise linearization model.

[Overview of Control Using Inverted Pendulum Model and Description of Terms]

[0038] First, terms, principles, and the like used in this embodiment will be schematically described.

[0039] As a walking model of a robot, for example, an inverted pendulum model in which the center of gravity is above a pivot point is known. In such a model, for example, the center of gravity is set on a body, and a pivot point is set on the sole of the feet. In a robot having foot parts, a zero moment point (ZMP) is frequently used in trajectory planning and control. The ZMP is synonymous with the center of pressure of the floor pressure.

[0040] In a conventional technology, the height of the center of gravity accompanied with walking is maintained to be constant using a leg strength f. In an inverted pendulum motion having a constant height, the equation of motion can be established by fixing the ZMP at the origin. In this equation of motion, the time derivative of the center of gravity position is a velocity, and the time derivative of the velocity is an acceleration. A vector such as a position or a velocity representing an instantaneous state of a target will be referred to as a state vector. For example, as in FIG. 1, in this state vector, a plane in which the position x (m) of the center of gravity is represented in the horizontal axis, and a velocity x.sup. (m/s) of the center of gravity is represented in the vertical axis will be referred to as a phase plane. FIG. 1 is a diagram for describing a phase plane, a divergent component, and a convergent component. In FIG. 1, dotted line g1 represents an asymptote (division line) of a divergent component (DCM), and dotted line g2 represents an asymptote (division line) of a convergent component (CCM). The divergent component is represented as x+(x.sup./). is the slope of the divergent component, =(g/b), b is a height from the floor to the center of gravity, and g is a gravitational acceleration (for example, see Reference Document 1). [0041] Reference Document 1: Shuji Kajita (Editor), Humanoid Robots (Revised 2nd Edition), Ohm Co., p 132-219, 2020

[Robot Control System]

[0042] Next, a configuration example of the robot control system 1 according to this embodiment will be described. FIG. 2 is a diagram illustrating a configuration example of a robot control system according to this embodiment.

[0043] The robot control system 1, for example, includes a robot 2 and a control device 3. The control device 3 may be included in the robot 2.

[0044] The robot 2, for example, includes a first foot part 21, a second foot part 22, a sensor 23, a body 24, a control unit 25, a communication unit 26, and a storage unit 27. The robot 2 includes a power supply and may include an arm, a head part, and the like.

[0045] The control device 3, for example, includes an acquisition unit 31, a model 32, a function calculating unit 33, a conversion unit 34, a feedback unit 35, a control unit 36, an output unit 37, and a storage unit 38.

(Robot)

[0046] The robot 2, for example, is a bipedal walking robot. The robot 2 performs transmission/reception of information to/from the control device 3 through a wired or wireless network NW.

[0047] In each of the first foot part 21 and the second foot part 22, a crotch is connected to a body through a joint. Each of the first foot part 21 and the second foot part 22, for example, includes a thigh, a lower leg, and a foot in a lower limb and includes a joint in each of a hip, a knee, and an ankle. In each joint, an encoder (a sensor 23) is included. Each of the first foot part 21 and the second foot part 22 includes an actuator in the joint.

[0048] The sensor 23, for example, is an encoder mounted in each joint, an acceleration sensor mounted in a leg or the like, a force sensing sensor, a gyro sensor, or the like. The sensor 23 detects, for example, the posture of a waist link, a ground reaction force, a joint angle, and the like.

[0049] The first foot part 21 and the second foot part 22 are connected to the body 24. An arm and a head part may be connected to the body 24.

[0050] The control unit 25 outputs a detection result detected by the sensor 23 to the control device 3 through the communication unit 26. The control unit 25 controls operations of foot parts (the first foot part 21 and the second foot part 22) in accordance with a control value or a control instruction output by the control device 3 or a drive signal.

[0051] The communication unit 26 outputs a detection value output by the control unit 25 to the control device 3. The communication unit 26 acquires a control value or a control instruction output by the control device 3 or a drive signal.

[0052] The storage unit 27 stores programs, thresholds, numerical expressions, and the like used in control and the like by the control unit 25.

(Control Device)

[0053] The control device 3, for example, is a personal computer or the like.

[0054] The acquisition unit 31 acquires a detection value output by the robot 2. The acquisition unit 31 may be configured to acquire an operation instruction and the like that have been set or input by an operator of the robot 2.

[0055] The model 32, for example, is a non-linear inverted pendulum model of which a trajectory is an energy conserving system.

[0056] The function calculating unit 33 obtains a ZMP position, a center of gravity position, and the like using a detection result detected by the sensor 23. The function calculating unit 33 obtains an energy function of the conservation system using the ZMP position, the center of gravity position, and the like that have been obtained and the model 32.

[0057] The conversion unit 34 converts the energy function of the conservation system into a curve (elliptic curve or hyperbola) function format. The curve may or may not include a power supply.

[0058] The feedback unit 35 linearizes the elliptic curve, for example, near the origin or calculates a non-linear asymptote of the hyperbola. The feedback unit 35 performs feedback control of a set of eigenvalue vectors or the asymptote that has been linearized as convergent/divergent components of the model 32.

[0059] The control unit 36 generates a control value, a control instruction, or a drive signal using the convergent/divergent components that have been fed back.

[0060] The output unit 37 outputs the control value, the control instruction, or the drive signal to the robot 2.

[0061] The storage unit 38 stores programs, thresholds, equations, and the like used by each unit of the control device 3.

[Height-Variable Inverted Pendulum Model]

[0062] Next, a height-variable inverted pendulum model used in this embodiment will be described.

[0063] FIG. 3 is a diagram for describing a height-fixed inverted pendulum model and a height-variable inverted pendulum model.

[0064] An image g10 is a diagram illustrating a trajectory of the center of gravity according to the height-fixed inverted pendulum model. Center of gravity g11 is illustrated, and dotted line g12 represents a trajectory. Like the dotted line g12, in the case of a height-fixed inverted pendulum model, the trajectory becomes a trajectory moving on an outer circumference of a circle.

[0065] An image g20 is a diagram illustrates a trajectory of the center of gravity according to a height-variable inverted pendulum model according to this embodiment. Circle g21 represents a point mass p.sub.pend, and dotted line g22 represents a trajectory of the point mass p.sub.pend. Circle g23 represents the center of gravity of a waist and a position p.sub.hip thereof, and dotted line g24 represents a trajectory of the center of gravity p.sub.hip of the waist. Circle g25 represents a mass point and a position p.sub.impend thereof, and dotted line g26 represents a trajectory of the mass point p.sub.impend.

[0066] In this embodiment, like the image g20 illustrated in FIG. 3, by actively changing the height, effective control is performed.

[0067] Control with a divergent component of a height-variable inverted pendulum model considered has been proposed (see Non-Patent Document 1). However, in such a conventional technology, since an analyzable phase plot cannot be drawn due to the presence of an integral term (3g.Math.dx) as in a conservation energy function of the following Equation (1), and the vertical axis and the horizontal axis change constantly over time as illustrated in FIG. 4, it is difficult to obtain an analytical solution, and a control input is determined through iterative calculation (repetitive calculation) such as Model Predictive Control (MPC) or Sequential Quadratic Programming (SQP). FIG. 4 is an image diagram of a phase plot obtained in a case in which a divergent component of a height-variable inverted pendulum model is considered using a conventional technology. An arrow of each curve illustrates a way in which a position and a velocity of the center of gravity on a phase plane change after a minimal time. In Equation (1), x.sub.G is the position of a center of mass (COM) in the x-axis direction, and h(x.sub.G) and f(x.sub.G) are functions of x.sub.G.

[00005]$\begin{array}{cc}E\left(x_G,{\stackrel{.}{x}}_G\right)=\frac{1}{2}{\stackrel{.}{x}}_G^2{h}^2\left(x_G\right)+{\mathrm{gx}}_G^{2}f\left(x_G\right)-3g{}_0^{x_G}f\left(x\right)\mathrm{xdx}& \left(1\right)\end{array}$

[Linearization of Divergent Component and Convergent Component]

[0068] Next, linearization of a divergent component and a convergent component will be described. FIG. 5 is a diagram for describing linearization of a divergent component and a convergent component.

[0069] A diagram g100 is a height-fixed inverted pendulum model of a conventional technology. Point O is a pivot point, x is a position in the x-axis direction, b is a height in the y-axis direction, and f is a leg force. In this case, in order to maintain the height to be constant using the leg force f, a state vector is represented in the following Equation (2).

[00006]$\begin{array}{cc}\frac{d}{\mathrm{dt}}\left[\begin{array}{c}x\\ \stackrel{.}{x}\end{array}\right]=\left[\begin{array}{cc}0& 1\\ {}^2& 0\end{array}\right]\left[\begin{array}{c}x\\ \stackrel{.}{x}\end{array}\right]& \left(2\right)\end{array}$

[0070] The divergent component and the convergent component are a set of eigenvectors for which x.sup.=x. is represented as in the following Equation (3).

[00007]$\begin{array}{cc}==\sqrt{\frac{g}{b}}& \left(3\right)\end{array}$

[0071] As in Equations (2) and (3), a solution of a height-fixed inverted pendulum model of a conventional technology cannot be obtained unless it is a linear system.

First Example

[0072] In contrast to this, in this embodiment, like an image g110 illustrated in FIG. 5, an ellipse g111 for which x=x() and y=y() in accordance with the leg force f is drawn. In this example, an example in which the trajectory of the center of gravity is an ellipse or a hyperbola will be described. Since the height of the center of gravity is variable, here, it is assumed that the trajectory of the center of gravity rides on the ellipse. Dotted line g112 is a curve in the case of a perfect circle. is a state quantity of the inverted pendulum expressed in a parametric variable.

[0073] Here, under conditions of (a.Math.sin(), b.Math.cos()) (ellipse), (a.Math.sin h(), b.Math.cos h(), and the like, the following Equation (4) is satisfied. In the conditions, a is a constant value, and is a constant value in the image g110 illustrated in FIG. 5. is an angle formed between the y-axis (the vertical direction) and the leg force f.

[00008]$\begin{array}{cc}\stackrel{.Math.}{}=\frac{g}{C}x\left(\right)& \left(4\right)\end{array}$

[0074] C is a divergent component denominator variable and is expressed as y(x/).Math.x(x/). Particularly, in a case in which parametric variable display is performed as in ellipse p=[x, y].sup.T (here, T is transpose)=A[e.sup.c30, e.sup.c30]T (c3 in the equation is c.sub.3) or a hyperbola p=A[sin h, cos h].sub.T, C becomes a constant, and the value thereof becomes 2|A|c.sub.3. A is a matrix with around ten rows/columns having complex numbers as its components, and c.sub.3 is a constant that includes a complex number. Also in the case of a linear trajectory p=A|+h.sub.x, h.sub.y|, p=A|+h.sub.x, +h.sub.y|, or the like, C becomes a constant, and each thereof becomes h.sub.y and h.sub.yh.sub.x.

[0075] In addition, linearization is performed around (, .sup.)=(0, 0).

[0076] By approximating Equation (4) as the following Equation (5), Equation (2) becomes the following Equation (6).

[00009]$\begin{array}{cc}\frac{g}{C}x\left(\right){}^2& \left(5\right)\end{array}$$\begin{array}{cc}\frac{d}{\mathrm{dt}}\left[\begin{array}{c}\\ \stackrel{.}{}\end{array}\right]=\left[\begin{array}{cc}0& 1\\ w^2& 0\end{array}\right]\left[\begin{array}{c}\\ \stackrel{.}{}\end{array}\right]& \left(6\right)\end{array}$

[0077] From Equation (6), a set of eigenvalue vectors becomes as in the following Equation (7). This set of eigenvectors is a divergent component and a convergent component.

[00010]$\begin{array}{cc}={}^{}& \left(7\right)\end{array}$

[0078] In this case, although linear approximation can be performed near (q, q.sup.)=(0, 0), when the position becomes far therefrom, approximation cannot be performed.

[Stabilization Control of Walking Robot]

[0079] Here, a control system of a walking robot will be schematically described.

[0080] FIG. 6 is a schematic block diagram of a stabilization control system. As illustrated in FIG. 6, the stabilization control system of a walking robot, for example, includes blocks of a robot 401, a center of gravity and ZMP measuring unit 402, a center of gravity and ZMP feedback unit 403, a ZMP distributing unit 404, and a floor reaction force control unit 405.

[0081] The robot 401 is a bipedal walkable robot having sensors.

[0082] The center of gravity and ZMP measuring unit 402 calculates and obtains a ZMP and the center of gravity also following detection results (a waist link posture, a floor reaction force, and a joint angle) detected by the sensors of the robot 401.

[0083] The center of gravity and ZMP feedback unit 403 calculates a target ZMP on the basis of a center of gravity ZMP plan trajectory included in an instruction value and the center of gravity and the ZMP based on actual measurement values of the robot 401.

[0084] The ZMP distributing unit 404 converts the target ZMP into a target floor reaction force.

[0085] The floor reaction force control unit 405 calculates a target joint angle using inverse kinematics on the basis of a joint angle pattern included in the instruction value and the target reaction force and controls the operation of the robot 401.

[0086] See Reference Document 1 for details of each process, calculation methods, and the like.

[0087] In a usable state, by performing control on the basis of an instruction value, the robot 401 can be caused to walk. However, in reality, the robot 401 falls due to presence of regularities and the like or the like on the floor. For this reason, in order to stably control the robot 401, it is necessary to perform control by correcting the instruction value using (feeding back) detection results acquired by the sensors included in the robot 401.

[Stabilization Control of Inverted Pendulum Model]

[0088] Next, stabilization control of an inverted pendulum model will be described.

[0089] FIG. 7 is a diagram for describing symbols used in a state equation of an inverted pendulum. p.sub.x is the position of a pivot point in the x-axis direction, pL is an end position of the heel end of the foot, pH is an end position of the tip of the toe, and b is a height from the floor (foot sole) to the center of gravity. FIG. 7 illustrates an inverted pendulum model mounted in a cart. A state equation of the inverted pendulum can be represented using the following Equation (8).

[00011]$\begin{array}{cc}\frac{d}{\mathrm{dt}}\left[\begin{array}{c}x\\ \stackrel{.}{x}\end{array}\right]=\left[\begin{array}{cc}0& 1\\ {}^2& 0\end{array}\right]\left[\begin{array}{c}x\\ \stackrel{.}{x}\end{array}\right]+\left[\begin{array}{c}0\\ -{}^2\end{array}\right]p_x& \left(8\right)\end{array}$

[0090] In Equation (8), p.sub.x is an ZMP input and is stabilized by the following Equation (9). K.sub.P and K.sub.V are gains.

[00012]$\begin{array}{cc}{p}_x=K_{P}x+K_V\stackrel{.}{x}& \left(9\right)\end{array}$

[0091] In this equation, since a ZMP constraint is present, a phase plot of the inverted pendulum is as illustrated in FIG. 8, and FIG. 8 is a phase plot obtained when stabilization control is performed by appropriately determining k.sub.P and K.sub.V.

[0092] Dotted lines g152 and g162 represent divergent components. Dotted lines g153 and g163 represent convergent components. The slopes of the divergent components and the convergent components are . Point g151 represents pL, and Point g161 represents pH. Dotted lines g152, g153, g162, and g163 dividing areas will be referred to as division lines.

[0093] An input of Line g171 is pL=K.sub.Px+K.sub.Vx.sup.. The slope is K.sub.p/K.sub.V. An input of Line g172 is pH=K.sub.Px+K.sub.Vx.sup..

[0094] Area g181 surrounded by Line g171 and Dotted line g153 is a convergent area.

[0095] Area g182 surrounded by Line g171 and Line 172 is an area in which the input is not saturated.

[0096] Ares g183 is an area in which the input is saturated.

[0097] Area g184 surrounded by Dotted line 153 and Line 171 is an area that cannot be stabilized even when it starts from an area in which the input is not saturated.

[0098] Area g184 surrounded by Dotted line 163 and Line 172 is an area that cannot be stabilized even when it starts from an area in which the input is not saturated.

[0099] Saturation represents a state in which an arrow-attached curve, for example, exhibits a change of a parabola or the like and does not converge to the origin. For this reason, in FIG. 8, Area g182 is a controllable area (control-possible area), and the other areas are saturated area. The example illustrated in FIG. 8 is an example in which the slope of the controllable area and the slope of the division lines (the asymptotes g153 and g163) do not coincide with each other.

[0100] In control in which a divergent component is fed back, K.sub.P and K.sub.V satisfy the following Equation (10).

[00013]$\begin{array}{cc}\frac{K_P}{K_V}=\left(K_V=\begin{array}{c}{K}_P\\ \end{array}\right)& \left(10\right)\end{array}$

[0101] When Equation (12) is substituted into Equation (11), p.sub.x becomes as in the following Equation (11), and (x+x.sup./x) is a divergent component.

[00014]$\begin{array}{cc}{p}_x=K_{P}x+\frac{K_P}{}\stackrel{.}{x}=K_P\left(x+\frac{\stackrel{.}{x}}{x}\right)& \left(11\right)\end{array}$

[0102] In control in which a divergent component is fed back, there are advantages as below. [0103] I. An optimal regulator in the sense that a stabilization margin can be maximized [0104] II. The stabilization limit can be analytically calculated.

[0105] For example, in the case of a bipedal robot, it can be used for determining a timing for taking a step forward to avoid falling. In riding assist, it can be applied to assess the risk of falling.

[0106] However, as described above, in a conventional technology, in a case in which the height is configured to be variable, the axis changes constantly over time, and thus it is difficult to define a divergent component of a general inverted pendulum. For a height-variable inverted pendulum model, for example, see Non-Patent Document 1 and Reference Documents 2 and 3.

[0107] Reference Document 2; Caron Stephane, Biped Stabilization by Linear Feedback of the Variable-Height Inverted Pendulum Model, IEEE Conference Proceedings, IEEE, 2020

[0108] Reference Document 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

[0109] For this reason, in this embodiment, a curve (FIG. 5) of which the state quantity is a parametric variable space (for example, ) and which becomes x=x() and y=y() in accordance with the leg force f is caused to be drawn, and linearization of a divergent component and a convergent component is performed. This curve is a line obtained by acquiring a conservation energy function of the model 32 using the function calculating unit 33 and converting the conservation energy function into a hyperbolic function format using the conversion unit 34.

[0110] In the case of this feedback control, as represented in Equation (5), the slope is constant.

Process Sequence Example

[0111] Next, an example of a process sequence performed by the control device 3 will be described. FIG. 9 is a flowchart of a process performed by the control device according to this embodiment.

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

[0113] (Step S2) The function calculating unit 33 acquires a ZMP position, a center of gravity position, and the like using the acquired detection result.

[0114] (Step S3) The function calculating unit 33 calculates an energy function of the conservation system using the ZMP position, the center of gravity position, and the like that have been acquired and the model 32.

[0115] (Step S4) The conversion unit 34 converts the energy function of the conservation system, for example, into a curve function format of an ellipse. The curve function is not limited to the ellipse and may be any shape other than a perfect circle of which the height is variable.

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

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

[0118] (Step S7) The control unit 36 generates a control value or a control instruction or a drive signal using the convergent component/divergent component that have been fed back. The control unit 36 outputs the control value or the control instruction or the drive signal to the robot 2 through the output unit 37.

[0119] The process illustrated in FIG. 9 is one example but is not limited thereto. For example, parts of the process may be performed in parallel with each other.

[0120] As described above, in this embodiment, the slope is constant, the state quantity is a task base (for example, x, y, and z coordinates), and walking is controlled using a height-variable inverted pendulum model.

[0121] In this embodiment, the slope is constant, the state quantity is a parametric variable space (for example, ), and walking is controlled using a height-variable inverted pendulum model.

[0122] As described above, according to this embodiment, also for a height-variable non-linear inverted pendulum, a technique for deriving a divergent component from an energy function and stabilization control using the technique can be realized without performing iterative calculation unlike a conventional technology.

[0123] In a conventional technology, in order to acquire an analytical solution of a divergent component, the trajectory needs to be generated using a piecewise linearization model.

[0124] In contrast to this, according to this embodiment, the trajectory of the center of gravity is represented, for example, using an equation of an ellipse without generating the trajectory using a piecewise linearization model and is approximated by performing linearization near the origin of the phase plot, whereby an analytical solution that is a divergent component and a convergent component can be acquired.

Second Example

[Generalization of Divergent Component and Convergent Component]

[0125] In the example described above, although an example in which the slope is constant has been described, the slope is generalized to be variable in the following example.

[0126] As illustrated in FIG. 10, in a case in which it is a conservation system even when it is a non-linear system, a convergent component and a divergent component can be acquired using an equation of energy. FIG. 10 is a diagram illustrating an example of a phase plot of a non-linear system. For this reason, in the following example, a divergent component is acquired in the format of energy.

[0127] First, the following Equation (12) of energy conservation will be focused. y is a center of gravity height.

[00015]$\begin{array}{cc}\frac{1}{2}{\stackrel{.}{x}}^2-\frac{g}{2y}{x}^2=\mathrm{Constant}E& \left(12\right)\end{array}$

[0128] By converting Equation (12) into a hyperbolic format, the following Equation (13) is obtained.

[00016]$\begin{array}{cc}\frac{{\stackrel{.}{x}}^2}{2E}-\frac{x^2}{\frac{2y}{g}E}=1& \left(13\right)\end{array}$

[0129] An asymptote (a division line) x of the hyperbolic format (13) is represented in the following Equation (14). As described above, since the asymptote x.sup. is a set of eigenvalue vectors as described above, it corresponds to a divergent component and a convergent component.

[00017]$\begin{array}{cc}\stackrel{.}{x}=\sqrt{\frac{g}{y}}x=x& \left(14\right)\end{array}$

[0130] In the case of being away from (q, q.sup.)=(0, 0), the divergent component of the non-linear system needs to be obtained more accurately for the example described above. For this reason, here, if X(q)=s(q)dq when q.sup.=(g/c).Math.x(q), the equation of the energy conservation can be converted into the following Equation (15), and, it can be converted into the following Equation (16) by converting Equation (15) into a hyperbolic format. Here, C is a constant.

[00018]$\begin{array}{cc}\frac{1}{2}{\stackrel{.}{}}^2-\frac{g}{2C}X\left(\right)=E& \left(15\right)\end{array}$$\begin{array}{cc}\frac{1}{2E}{\stackrel{.}{}}^2-\frac{g}{\frac{2C{}^2}{\mathrm{gX}\left(\right)}}{}^2=E& \left(16\right)\end{array}$

[0131] The asymptote .sup.(g201, g203) of the hyperbolic format (16) is the following Equation (17). Here, () is (). Line g201 is an asymptote of the divergent component, and Line g203 is an asymptote of the convergent component.

[00019]$\begin{array}{cc}\stackrel{.}{}=\sqrt{\frac{\mathrm{gX}\left(\right)}{C{}^2}}=\left(\right)x& \left(17\right)\end{array}$

[0132] FIG. 11 is a phase plot in which the horizontal axis is .sub.1 (rad/s), and the vertical axis is .sup..sub.1 (rad/s). In FIG. 11, asymptotes g202 and g204 are linearized.

[0133] The state equation of the inverted pendulum in Equation (15) can be represented as in the following Equation (18) using the image g110 illustrated in FIG. 5.

[00020]$\begin{array}{cc}\frac{d}{\mathrm{dt}}\left[\begin{array}{c}\\ \stackrel{.}{}\end{array}\right]=\left[\begin{array}{c}\stackrel{.}{}\\ \frac{g}{b}\sin \end{array}\right]+\left[\begin{array}{c}0\\ -{}^2\end{array}\right]{}_{}& \left(18\right)\end{array}$

[0134] .sub. is an input and is stabilized as .sub.=K.sub.P+K.sub.V.

[0135] Here, in this embodiment, an input constraint of the following Equation (19) is applied.

[00021]$\begin{array}{cc}{}_{}=\{\begin{array}{cc}{}_L& \left(K_P+K_V\stackrel{.}{}>{}_L\right)\\ K_P+K_V\stackrel{.}{}& \left({}_L<K_P+K_V\stackrel{.}{}<{}_H\right)\\ {}_H& \left(K_P+K_V\stackrel{.}{}<{}_H\right)\end{array}& \left(19\right)\end{array}$

[Assessment]

[0136] Next, results of assessments of the above-described case in which linearization is performed near (q, q.sup.)=(0, 0) (here, for example, q is ) and a case in which a gain is set using a non-linear divergent component through a simulation will be described. In the simulation, a non-linear inverted pendulum model having a flywheel is used.

[0137] Here, an input the of a case in which linearization is performed near (q, q.sup.)=(0, 0) (here, for example, q is ) is as in the following Equation (20). An input .sub. of a case in which a gain is set using a non-linear divergent component is as in the following Equation (21).

[00022]$\begin{array}{cc}{}_{}=K_P+K_V\stackrel{.}{}=K_P\left(+\frac{1}{}\stackrel{.}{}\right)& \left(20\right)\end{array}$$\begin{array}{cc}{}_{}=K_P+K_V\stackrel{.}{}=K_P\left(+\frac{1}{\left(\right)}\stackrel{.}{}\right)& \left(21\right)\end{array}$

[0138] FIG. 12 is a diagram illustrating an example of a phase plot obtained in a case in which linearization is performed near (, .sup.)=(0, 0) in accordance with this embodiment. The horizontal axis and the vertical axis are similar to those illustrated in FIG. 11. The example illustrated in FIG. 11 is an example in which the slope of a controllable area and the slope of a division line (an asymptote) coincide with each other.

[0139] In this case, as in FIG. 12, near (, .sup.)=(0, 0), an area in which the input is not saturated enters Area g251 that can be stabilized. However, near a position at which .sub.1 is near 0.5, an area in which the input is not saturated protrudes from an area that can be stabilized (reference sign g252). Near a position at which .sub.1 is near 0.5, an area in which the input is not saturated protrudes from an area that can be stabilized (reference sign g253). In this way, in a case in which linearization is performed near (, .sup.)=(0, 0), a non-linear shallow can be controlled such that it is stabilized within a predetermined range.

[0140] In accordance with this, according to this embodiment, an analytical solution of an energy equation can be obtained with the amount of calculation decreased to be less than that of a conventional technology.

[0141] FIG. 13 is a diagram illustrating an example of a phase plot obtained in a case in which a gain is set using a non-linear divergent component according to this embodiment. The horizontal axis and the vertical axis are similar to those illustrated in FIG. 11. The example illustrated in FIG. 13 is an example in which the slope of a controllable area and the slope of a division line (an asymptote) coincide with each other.

[0142] In this case, as in FIG. 13, near (, .sup.)=(0, 0) and in a case in which .sub.1 is near-0.5 and near 0.5, an area in which the input is not saturated enters an area that can be stabilized. In this way, in a case in which a gain is set using a non-linear divergent component, control can be performed with being stabilized in a wide input range.

[0143] According to this embodiment, by using the assessment, it has been checked that stabilization control can be performed robustly for an arbitrary trajectory by performing general energy feedback control for a non-linear inverted pendulum model having a flywheel using a ZMP input.

[0144] This embodiment can be applied also to a case in which non-linearity is strong. According to this embodiment, although a divergent component is changed in accordance with an angle, iterative calculation is unnecessary, and an analytical solution of an energy equation can be acquired.

Process Sequence Example

[0145] Next, an example of a process sequence performed by the control device 3 will be described. FIG. 14 is a flowchart of a process performed by the control device according to this embodiment.

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

[0147] (Step S12) The function calculating unit 33 acquires a ZMP position, a center of gravity position, and the like using the acquired detection result.

[0148] (Step S13) The function calculating unit 33 calculates an energy function of the conservation system using the ZMP position, the center of gravity position, and the like that have been acquired and the model 32.

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

[0150] (Step S15) The feedback unit 35 calculates an asymptote (division line) of the hyperbola.

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

[0152] (Step S17) The control unit 36 generates a control value or a control instruction or a drive signal using the convergent component/divergent component that have been fed back. The control unit 36 outputs the control value or the control instruction or the drive signal to the robot 2 through the output unit 37.

[0153] In this embodiment, the slope w is constant, the state quantity is a task base (for example, x, y, and z coordinates), and walking is controlled using a height-variable inverted pendulum model.

[0154] In this embodiment, the slope is variable, the state quantity is a parametric variable space (for example, ), and walking is controlled using a height-variable inverted pendulum model.

[0155] As described above, according to this embodiment, also for a height-variable non-linear inverted pendulum, a technique for deriving a divergent component from an energy function and stabilization control using the technique can be realized without performing iterative calculation unlike a conventional technology.

[0156] In a conventional technology, in order to acquire an analytical solution of a divergent component, the trajectory needs to be generated using a piecewise linearization model. In contrast to this, according to this embodiment, by using a model that is able to derive a curved trajectory including a straight line, an analytical solution can be acquired without generating a trajectory using a piecewise linearization model.

[0157] In each embodiment described above, although the bipedal walkable robot 2 has been described as one example of a mobile body, a mobile body controlling the posture is not limited thereto. The control method according to the embodiment can be applied also to a case in which the mobile body, for example, is a two-wheeled vehicle (an electric bicycle, a motorcycle, or the like) or the like.

[0158] In addition, all or some of the processes performed by the control device 3 may be performed by recording a program used for realizing all or some of the functions of the control device 3 according to the present invention on a computer readable recording medium and causing a computer system to read and execute the program recorded on this recording medium. A computer system described here includes an OS and hardware such as peripheral devices. In addition, the computer system also includes a WWW system having a home page providing environment (or a display environment). A computer-readable recording medium represents a storage device including a portable medium such as a flexible disk, a magneto-optical disc, a ROM, or a CD-ROM, a hard disk built in a computer system, and the like. Furthermore, a computer-readable recording medium includes a server in a case in which a program is transmitted through a network such as the Internet or a communication line such as a telephone line or a device such as a volatile memory (RAM) disposed inside a computer system that serves as a client that stores a program for a predetermined time.

[0159] Alternatively, some or all of such constituent elements may be realized by hardware (a circuit unit; includes circuitry) such as a large scale integration (LSI) including an application specific integrated circuit (ASIC), a field-programmable gate array (FPGA), a graphics processing unit (GPU), and the like or may be realized by software and hardware in cooperation.

[0160] The program described above may be transmitted from a computer system storing this program in a storage device or the like to another computer system through a transmission medium or a transmission wave in a transmission medium. Here, the transmission medium transmitting a program represents a medium having an information transmitting function such as a network (communication network) including the Internet and the like or a communication line (communication wire) including a telephone line. The program described above may be used for realizing a part of the functions described above. In addition, the program described above may be a program realizing the functions described above by being combined with a program recorded in the computer system in advance, a so-called a differential file (differential program).

[0161] As above, although forms for performing the present invention have been described using the embodiments, the present invention is not limited to such embodiments at all, and various modifications and substitutions may be applied within a range not departing from the concept of the present invention.