JP6447278B2 - Multipoint contact robot, control method and program for multipoint contact robot - Google Patents

Description

  The present invention relates to control of a multipoint contact robot. More specifically, the present invention relates to a control technique for moving a multipoint contact robot stably and with high accuracy.

  Robots having a function of moving autonomously have been developed. For example, there are robots that operate by multipoint contact control. In order to operate the robot stably with multi-point contact, (A) a technique for generating a stable trajectory in the long term, and (B) a desired contact force at each contact point in order to make the robot asymptotic to the generated trajectory It is necessary to consider three types of techniques, that is, a stabilization technique for calculating the joint force, and (C) a force control technique for driving each joint so as to achieve the calculated contact force target value. By using these three techniques in the order of (A), (B), and (C), a stable multipoint contact operation can be realized. That is, first, (A) a stable trajectory for a relatively long time, that is, a stable trajectory for a certain degree of future, that is, a trajectory without divergence of the center of gravity or failure of the motion is generated, and then (B) In addition, the force command value at each contact point of the robot is determined and corrected so as to make the body posture, etc. asymptotic to the trajectory, and then (C) force control so that the reaction force at each contact point follows the force command value. I do.

  Here, paying attention to the stabilization technique (B), Patent Document 1 and Patent Document 2 can be cited as prior art applicable to multipoint contact control. Patent Document 1 is an extension of the conventional composite compliance control, which is a stabilization technology for biped walking, so that it can be used for multi-legged robots having three or more grounding points including the floor surface. Patent Document 2 discloses a stabilization technique that can cope with multipoint contact control having contact points other than the floor surface.

Japanese Patent No. 4126064 Japanese Patent No. 5398592

  The composite compliance control of Patent Document 1 is a stabilization technique using ZMP. ZMP (zero moment point) is a moment balance position (position where the entire moment is balanced and exactly 0) on a predetermined ZMP plane (usually the floor). Stabilization technology using ZMP is suitable for movements where the point of contact with the environment is almost limited to the floor, such as walking, but for movements that make contact with the environment at any number of points of contact. Is not suitable. In other words, Patent Document 1 is a technique for a robot having a contact point on the floor surface (that is, a multi-legged robot) although it is compatible with multi-point contact.

  On the other hand, the stabilization technique of Patent Document 2 does not use ZMP, and can cope with multipoint contact control of a robot having contact points other than the floor surface. However, in the algorithm disclosed in Patent Document 2, the control law is assembled on the assumption that the surface to which each contact point belongs is projected onto a preset virtual surface. In other words, the control law is assembled using a virtual surface group gimmick without directly calculating the force distribution on the contact surface such as the wall or floor where the contact points such as hands and feet are actually in contact. .

  The algorithm of Patent Document 2 is based on a surface group (virtual surface group) in which the surfaces face each other orthogonally or in parallel. For example, when a desired translational force cannot be obtained due to a shortage of the frictional force at the contact point, the frictional force is increased by increasing the pressing force (vertical drag) on each parallel virtual surface. The compensation method is used. Here, if the virtual surfaces do not face each other in parallel and have an arbitrary inclination, the directions of the normal forces generated on the surfaces do not coincide with each other, so that the normal force is simply increased as described above. Such a logic cannot solve this problem. In other words, the stabilization technique of Patent Document 2 requires that the entire control logic be based on a group of surfaces whose surfaces face each other orthogonally or in parallel. Therefore, a virtual surface is set instead of an actual contact surface.

  For this reason, the stabilization technique of Patent Document 2 has a problem that applicability to a contact surface group having an arbitrary angle (for example, a plurality of surfaces inclined relatively by 45 degrees) is extremely low. Moreover, even if it is a case where it can stabilize by utilizing a frictional force and internal force, it also has the problem of causing the misjudgment that a motion state is inappropriate.

  Further, in the stabilization technique of Patent Document 2, feedback control of the center of gravity and posture angle is configured by known PD control. In the PD control, information on the position and orientation of each contact point (for example, information on what direction the force is likely to be generated and how much force can be stably generated in which direction) is not considered at all.

  The present invention has been made in view of these problems, and an object of the present invention is to provide a multipoint contact robot having high stabilization performance, a control method for the multipoint contact robot, and a program.

The multipoint contact robot according to the present invention is:
A multi-point contact robot having a plurality of contact points,
A trajectory generator for generating a target gravity center trajectory and a reaction force target value for each of the contact points;
A state estimation unit that estimates a current center of gravity and calculates an error between the target center of gravity trajectory and the current center of gravity;
A state stabilizing unit that corrects the reaction force target value so that the error converges to the target gravity center trajectory,
The trajectory generation unit calculates a feedback gain for converging the error to the target gravity center trajectory in a first period,
The state stabilizing unit corrects the reaction force target value based on the error and the feedback gain in a second period shorter than the first period.

The control method of the multipoint contact robot according to the present invention is as follows.
A trajectory generating step for generating a target center-of-gravity trajectory of a multi-point contact robot having a plurality of contact points and a reaction force target value of each of the contact points;
An error calculating step of estimating a current center of gravity and calculating an error between the target center of gravity trajectory and the current center of gravity;
An optimal gain calculating step for calculating a feedback gain for converging the error to the target gravity center trajectory in a first period;
And a state feedback step of correcting the reaction force target value based on the error and the feedback gain in a second period shorter than the first period.

  The program concerning this invention is a program for making a computer perform the control method of the said multipoint contact robot.

  ADVANTAGE OF THE INVENTION According to this invention, the control method and program of a multipoint contact robot which introduced the multipoint contact control technique which has high stabilization performance, and a multipoint contact robot can be provided.

It is a figure which shows the structure of the multipoint contact robot 100 concerning embodiment of this invention. FIG. 4 is a diagram showing a contact force applied to the multipoint contact robot 100. 2 is a diagram illustrating a coordinate system and a contact polygon of a contact point of the multipoint contact robot 100. FIG. FIG. 4 is a diagram showing a stabilization condition for contact points of the multipoint contact robot 100. 2 is a diagram showing a control framework of the multipoint contact robot 100. FIG. 2 is a diagram showing a control framework of the multipoint contact robot 100. FIG. 2 is a diagram illustrating a control method of the multipoint contact robot 100. FIG. 2 is a diagram illustrating a control method of the multipoint contact robot 100. FIG. 1 is a diagram showing a configuration of a multipoint contact robot 100. FIG. 1 is a diagram showing a configuration of a multipoint contact robot 100. FIG.

  An embodiment of the present invention will be illustrated and described with reference to reference numerals attached to elements in the drawing.

  This embodiment is characterized by a multi-point contact robot control method, specifically, a multi-point contact control technology for controlling the movement operation of the multi-point contact robot. Before describing the contact control method, the hardware configuration of the multipoint contact robot to be controlled will be described in advance.

  FIG. 9 is a diagram illustrating an example of a mechanical configuration of the multipoint contact robot. The multipoint contact robot 100 has three axes for the hip joint, one axis for the knee joint, two axes for the ankle joint, three axes for the shoulder joint (shoulder pitch, shoulder roll, shoulder yaw), and one axis for the elbow joint (elbow pitch). ) And the wrist joint is composed of three axes (wrist yaw, wrist pitch, wrist roll). The mechanical configuration of the multipoint contact robot is not limited to this, but the degree of freedom of the hand (arm) is 6 or more and the degree of freedom of the foot (leg) is 6 or more.

  The multipoint contact robot 100 has motors 1, 2,..., 28 with encoders at each joint. Motors (joint driving means) 1a, 2a,..., 28a (FIG. 10) of each joint can adjust the joint angles θ1, θ2,. On the other hand, encoders (joint angle detection means) 1b, 2b,..., 28b of each joint can measure joint angles θ1, θ2,.

Further, the multipoint contact robot 100 has a contact force sensor 25 at a tip part (foot part) and a hand part (palm part). Here, the contact force is a six-axis force. As shown in FIG. 2, a set of forces f in the x-axis, y-axis, and z-axis directions (f _x , f _y , f _z ) ^T , A set of forces τ around the y-axis and the z-axis (τ _x , τ _y , τ _z ) ^T. Note that the x-axis and the y-axis are axes orthogonal to each other in a plane perpendicular to the z-axis that is the vertical direction.

  The multipoint contact robot moves while contacting a plurality of contact points such as a right foot, a left foot, a right hand, or a left hand with a contact surface such as a floor, a wall, or a table.

  FIG. 10 is a functional block diagram of the multipoint contact robot 100. The multipoint contact robot 100 includes motors 1a to 24a and encoders 1b to 24b of each joint, a force sensor 25, and a controller 210.

  Sensor detection values are input to the controller 210 from the encoders 1b to 24b and the force sensor 25 of each joint. Further, the controller 210 outputs a drive signal to the motors 1a to 24a of each joint.

  The controller 210 includes, as main hardware configurations, a CPU (Central Processing Unit) 210a that performs control processing, arithmetic processing, and the like, and a ROM (Read Only Memory) 210b that stores control programs, arithmetic programs, and the like executed by the CPU 210a. And a RAM (Random Access Memory) 210c that temporarily stores processing data and the like. The CPU 210a, ROM 210b, and RAM 210c are connected to each other by a data bus 210d. Necessary programs may be recorded on a non-volatile recording medium and installed as necessary.

  Here, the functional configuration of the controller 210 will be described with reference to the functional block diagram of FIG. At the same time, the generation and control framework of the multipoint contact operation in the multipoint contact robot will be described with reference to FIG.

(A) Contact Point Planning The contact point planning unit 211 plans an appropriate position and posture of a contact point for executing a task (including movement) with a multipoint contact robot. The contact point plan may be input by the user.

(B) Trajectory generation The trajectory generation unit 212 is based on the contact point position and posture planned by the contact point planning unit 211, and stable center of gravity trajectory, force command value trajectory, positions of the hand and the tip of the foot, which do not diverge and fail in the future. , And generate a posture trajectory.

(C) State Estimation The state estimation unit 213 uses an IMU (3-axis gyro, 3-axis accelerometer) mounted on the body of a multi-point contact robot, a force sensor of each contact point (for example, a hand or a tip), and the like. Thus, the current center of gravity position, speed, and body posture are estimated.

(D) State Stabilization The state stabilization unit 214 provides state feedback so that the difference between the state of the multipoint contact robot estimated by the state estimation unit 213 and the target trajectory generated by the trajectory generation unit 212 is asymptotic to zero. To correct the 6-axis force command value of each contact point.

(E) Contact force control The contact force control unit 215 performs impedance control using a force sensor so as to realize a six-axis force command value of each contact point, and determines the position of the contact point (for example, a hand or a tip). Correct posture.

(F) Whole body IK
The whole body IK unit 216 performs inverse kinematics (IK: inverse kinematics) so as to achieve command values such as the position of the center of gravity calculated by the contact force control unit 215, the positions and postures of the hands and toes, and the angular momentum around the center of gravity. ) To obtain the joint angle of the whole body.

(G) Joint Servo The joint servo unit 217 performs position servo control on each axis based on the joint angle command value obtained by the whole body IK unit 216.

  Here, since each processing itself related to the above (a) to (g) can be realized by using a known technique or the like, detailed description is omitted in the present embodiment.

  The controller 210 typically processes (a) contact point planning, (b) trajectory generation, (d) state stabilization, (e) contact force control, (f) whole body IK, and (g) joint servo in this order. To control the operation of the multipoint contact robot. The important point is that the controller 210 performs feedback processing of (b) trajectory generation and (c) state estimation. Here, the trajectory generation unit 212 generates a target trajectory for the future starting from the current state of the multipoint contact robot at regular intervals. Similarly, a feedback loop exists between (d) state stabilization and (c) state estimation. Here, the state stabilization unit 214 corrects the force command value so that the difference between the current state of the multipoint contact robot and the target trajectory gradually approaches zero.

  The feature of this embodiment is that the periods of these two feedback loops are different. That is, the state estimation unit 213 feeds back the state estimation result of the multipoint contact robot to the trajectory generation unit 212 and the state stabilization unit 214 at different periods. The controller 210 executes a feedback loop between the trajectory generation unit 212 and the state estimation unit 213 with a relatively long cycle (for example, 20 ms). On the other hand, the feedback loop between the state stabilization unit 214 and the state estimation unit 213 is executed with a relatively short period (for example, 1 ms).

  The reason why the feedback period may be different is as follows. (D) State stabilization processing is control for correcting the difference between the current robot state and the target trajectory. If there is a difference between the actual state of the multipoint contact robot and the target trajectory for some reason, the motion of the multipoint contact robot may fail and fall. Therefore, feedback processing between (c) state estimation and (d) state stabilization is performed in as short a time (cycle) as possible, and posture control by (e) to (g) is performed as quickly as possible. It is necessary to stabilize the state of the contact robot. On the other hand, (b) trajectory generation processing is processing for generating a stable trajectory for the future from the initial position of the center of gravity, speed, etc. of the current multipoint contact robot. This process does not necessarily need to be performed with a cycle as short as (d).

  Thus, the controller 210 can execute (b) trajectory generation processing and real-time posture control processing according to (d) to (g) at different periods. Needless to say, the amount of calculation allowed is different between the trajectory generation process and the real-time attitude control process. Since real-time attitude control processing needs to be executed in a short time, it is necessary to devise a technique for minimizing the amount of calculation.

  Next, an example of an algorithm for multipoint contact control processing according to the present embodiment will be described. First, the concept of the multipoint contact control process according to the present embodiment will be described.

と表される。 In the present embodiment, a model in which the inertia of the entire multipoint contact robot is represented by one center of gravity and a six-axis force is defined for each contact point (FIG. 2). At this time, if the translational momentum of the center of gravity is P, the rotational momentum around the center of gravity is L, and the number of contact points is n, the equation of motion is

It is expressed.

とし、誤差量を導入して

とすると、式（１）は

と表される。 Here, the command values for “center of gravity position, fuselage posture angle, angular momentum around the center of gravity, force at each contact point, moment at each contact point” are

And introduce the error amount

Then, equation (1) becomes

It is expressed.

が成り立っているので、上式（数４）と差を取れば

となる。ただし、誤差の２乗項は十分小さいとして近似的に

とした。 The command value generated by the trajectory generator 212 naturally satisfies the formula (1).

Therefore, if we take the difference from the above equation (Equation 4)

It becomes. However, the error square term is small enough to approximate

It was.

となる。ただし、

とする。 If equation (2) is rewritten into matrix representation

It becomes. However,

And

とおけば、式（３）は簡単に

と書くことができる。ここで、ηは多点接触ロボットと目標軌道とのずれ量、ｕは接触点における荷重を示す。Ａ及びＢをシステム行列という。 further,

Then, formula (3) is easy

Can be written. Here, η indicates the amount of deviation between the multipoint contact robot and the target trajectory, and u indicates the load at the contact point. A and B are called system matrices.

が状態安定化部２１４に入力されるものとする。 Here, as a result estimated by the state estimation unit 213, an estimated value of “center of gravity position, speed, body posture angle, angular momentum around the center of gravity”

Is input to the state stabilization unit 214.

を達成することである。 The purpose of the state stabilization unit 214 is

Is to achieve.

と定義する。ただし、ｋは姿勢角誤差のフィードバックゲイン、ｅ_Φは瞬間的な姿勢角誤差をグローバル座標系で表現したベクトルである。ｅ_Φは、オイラー角とクォータニオンとの変換関数をＣ_ｑｕａｔとすれば次式で表される。

Where the error is

It is defined as Here, k is a feedback gain of the posture angle error, and _eΦ is a vector expressing the instantaneous posture angle error in the global coordinate system. e _Φ is expressed by the following equation when the conversion function between the Euler angle and the quaternion is _Cquat .

  The second equation of equation (6) is obtained by multiplying the error of the body posture angle by a gain and adding it to the angular momentum error around the center of gravity, which is approximately integrating the angular momentum error around the center of gravity. Can also be interpreted. That is, when feedback is performed so as to reduce the angular momentum error around the center of gravity, it can be roughly interpreted as equivalent to performing PI control.

となれば良い。 Therefore, the state stabilizing unit 214 can achieve the purpose of the equation (5) by feeding back the error of the equation (6) and performing control so that the error is reduced. That is, when the error is defined by equation (6),

It will be good.

この最適レギュレータ問題は従来から非常に多くの分野で使われており、解もよく知られている。

ここで、Ｐはリッカチ方程式の解

である。また、Ｋはずれ量ηを打ち消す荷重ｕを得るためのゲインを示す。 Therefore, in the present embodiment, an optimum regulator represented by the following evaluation function is applied for the purpose of asymptotic convergence of error and distribution of load to each contact force.

This optimal regulator problem has been used in many fields in the past, and its solution is well known.

Where P is the solution of the Riccati equation

It is. K represents a gain for obtaining a load u that cancels out the shift amount η.

  As described above, the state stabilization unit 214 can calculate the correction force and the moment that achieve Equation (7) by feeding back the error amount (state amount) observed by the state estimation unit 213.

Here, the following two problems arise.
(A) The system equation (4) changes as the number of contact points and contact parts change. For this reason, the Riccati equation cannot be solved in advance. On the other hand, since solving each time in real time requires calculation time, the upper limit of the control cycle is limited.
(A) In the equation (8) indicating the state feedback, the limitation of the correction force and the moment is not considered. Therefore, the correction may cause contact instability such as the contact part slipping or peeling off at the contact point.

は、接触面の姿勢行列

（ｒｏｔ（）はオイラー角と姿勢行列に変換する関数）を用いて、

と表される。 First, consider the solution to issue (b). FIG. 3 shows the coordinate system of the contact point (with the superscript l) and the contact polygon (support polygon of the contact point). Contact point coordinates is assumed to be defined in accordance with the orientation r _i of the contact surface of the contact point as the origin. At this time, the 6-axis force defined in the contact coordinate system

Is the posture matrix of the contact surface

(Rot () is a function that converts Euler angles and attitude matrices)

It is expressed.

As shown in FIG. 4, the following three conditions must be satisfied in order for the contact point to stably maintain contact.
(1) The contact point does not move away from the contact surface. (2) The contact point does not slip on the contact surface. (3) The contact point does not peel off from the contact surface.

(1) In order for the contact point not to be separated from the contact surface, the vertical force on the contact surface may be positive. That is, it is necessary to satisfy the following formula.

(2) In order for the contact point not to slip on the contact surface, the biaxial force parallel to the contact surface may be equal to or less than the friction force. That is, the following equation is the condition. However, the friction coefficient of the contact surface is μ _i .

(3) The conditions for preventing the contact point from peeling off are as follows using h vertex coordinates (x ^l _il , y ^l _il ), ..., (x ^l _ih , y ^l _ih ) of the contact polygon. (Note that the vertices of the contact polygon are given in order counterclockwise).

となる。

の関係を用いて更に式（１３）を書き直せば、

となる。ただし、

とした。 Summarizing equations (9), (10), (11) and (12)

It becomes.

If the equation (13) is rewritten using the relationship of

It becomes. However,

It was.

と表すことができる。ただし、

とする。 When formula (14) is written for all contact points,

It can be expressed as. However,

And

を最小化するようなフィードバックゲインを算出する。 The problem (b) is that, as a result of the feedback of the equation (8), the equation (15) is not satisfied, so the conditional equations (10), (11) and (12) for keeping the contact point stable are not satisfied. This is a problem that the contact part causes slipping or peeling at the contact point. Therefore, in the present embodiment, the evaluation function is satisfied while satisfying the expression (15) most recently.

Calculate a feedback gain that minimizes.

ただし、η_ｎは最も直近で得られた誤差値である。 This problem can be obtained by solving the following optimization problem using LMI (Linear Matrix Inequality).

However, η _n is the error value obtained most recently.

のようにフィードバックゲインが計算される。
接触点における摩擦力又は鉛直力を含む制約条件を課すことにより、補正によって接触点におけるすべりやはがれなどの不安定性を考慮した状態安定を行うことができる。 It is well known that the optimization problem in the form of equation (16) can be efficiently obtained using an interior point method which is a solution method by convergence calculation. Using the solution result P ^* of equation (16)

The feedback gain is calculated as follows.
By imposing a constraint condition including frictional force or vertical force at the contact point, it is possible to perform state stabilization in consideration of instability such as slip or peeling at the contact point by correction.

  Next, the solution to the problem (a) is examined. Although the above optimization problem can be solved at high speed, it requires a certain amount of calculation time. Therefore, in the present embodiment, a method for solving the problem (a) is proposed by performing the calculation of the feedback gain at the same time as (b) trajectory generation. The calculation of the feedback gain may be performed at a cycle slower than that of the feedback loop in (d) state stabilization, and is not necessarily performed at the same cycle as that of the feedback loop for (b) trajectory generation.

  That is, as shown in FIG. 5, in the present embodiment, the optimal feedback gain K is calculated by solving the equations (16) and (17) based on the current state estimation amount in a cycle of 20 ms, and the obtained feedback A framework is adopted in which state feedback is performed at a 1 ms period using gain K. Since the calculation of the state feedback itself simply multiplies the error amount calculated from the feedback gain and the current state estimation amount, the correction force and moment can be calculated with a very small amount of calculation. Since the calculation of the state feedback is limited to the calculation of adding the error and the feedback gain, it can be calculated in a short period.

  The characteristic part of this embodiment will be described again with reference to FIGS. FIG. 6 is a block diagram showing the framework of the present embodiment. FIG. 7 is a flowchart showing the operation of the optimum gain calculator 2122. FIG. 8 is a flowchart showing the operations of the error calculator 2132 and the state feedback 2142.

The block diagram in FIG. 6 includes a plurality of functional blocks and arrows indicating input / output of data between the functional blocks. The trajectory generator 2121 and the optimum gain calculator 2122 are functional blocks included in the trajectory generator 212. The state estimator 2131 and the error calculator 2132 are included in the state estimator 213. The state feedback 2142 is included in the state stabilization unit 214. Further, in FIG. 6, the contact point parameters ^{_{s i = (μ i, (}} x l il, y l il), ···, (x l ih, y l ih)) is the friction coefficient of the contact points and contact polygon .

Among these, the trajectory generator 2121 and the optimum gain calculator 2122 execute processing in a long cycle, typically a 20 ms cycle. On the other hand, the state estimator 2131, the error calculator 2132, and the state feedback 2142 execute processing in a short cycle, typically 1 ms.

First, the operation of a functional block in which processing is executed in a long cycle, typically a 20 ms cycle will be described. Based on the contact point position and orientation p _i [k], r _i [k] and the contact point parameter s _i [k] planned by the contact point planning unit 211 or the host device, the trajectory generator 2121 Velocity q _d [k], q _d [k], fuselage posture angle φ _d [k], center of gravity angular momentum L _d [k], force and moment of contact point f _id [k], τ _id [k], A time series trajectory of the hand and foot tip positions and postures (including contact points) p ^t _i [k] and r ^t _i [k] is generated and stored in the trajectory buffer. Here, as the trajectory generator, for example, the one described in Japanese Patent Application No. 2013-254899 can be used.

The optimum gain calculator 2122 receives from the trajectory generator 2121 the center of gravity position, the angular momentum around the center of gravity, the current target values q _d , L _d , f _id and τ _id of the six contact point forces, and the contact point positions pi and Contact point parameter s _i is acquired (FIG. 7, S101). Then, using these acquired values, system matrices A and B are obtained according to Equation 10 (S102). Further, the inequality constraint conditions G and w shown in Expression 31 are obtained (S103).

The optimum gain calculator 2122 acquires the latest value (current value) η _n of the state error value from the error calculator 2132 (S104 in FIG. 7). Then, a solution P ^* for the optimization problem is obtained from equation (16) (S105). Finally, the optimum feedback gain K is calculated and output using Expression (17) and P ^* (S106).

  Next, the operation of a functional block in which processing is executed in a short cycle, typically 1 ms, will be described. The state estimator 2131 outputs the current center-of-gravity position and velocity q, q, the body posture angle φ, and the center-of-gravity angular momentum L using each sensor value. As the state estimator 2131, for example, the one described in Japanese Patent Application No. 2014-089943 can be used.

The error calculator 2132 obtains target values q _d , q _d , φ _d , and L _d at the current time from the trajectory buffer (FIG. 8, S202). In addition, state estimation values q, q, φ, and L are acquired from the state estimator 2131 (S203). Then, the error calculator 2132 uses these values to calculate the current error vector η based on Equation (6).

The state feedback 2142 acquires the optimum feedback gain K from the optimum gain calculator 2122 (S201 in FIG. 8). Further, the error vector η is acquired from the error calculator 2132 (S204). Then, the correction force and moments Δf _i and Δτ _i of the contact point are calculated by multiplying K and η (S205). Further, a six-axis force command is obtained by adding the correction amount to the target six-axis force at the current time as f ^ref _i = f _id [j] + Δf _i , τ ^ref _i = τ _id [j] + Δτ _i. A value is output (S206).

  According to the present embodiment, the trajectory generation processing (including optimal gain calculation) in the trajectory generation unit 212 and the attitude control processing (including error feedback) by the state stabilization unit 214 are executed at different periods. As a result, the state stabilization unit 214 can execute the error feedback process in a relatively short time without being affected by the optimum gain calculation process that requires a relatively large amount of calculation, that is, that requires time for the calculation. Therefore, posture control can be performed at an early cycle, and the center of gravity trajectory tracking / stabilization performance can be improved in an arbitrary multipoint contact state.

  Further, according to the present embodiment, the state stabilization unit 214 reduces the trajectory tracking error to zero while maintaining the stabilization of each contact point by the formulation according to the equation (16) and the framework shown in FIG. The asymptotic process can be executed at high speed with low calculation cost. Thereby, the state stabilization part 214 can perform attitude | position control with an early period, and can improve center-of-gravity track following and stabilization performance in arbitrary multipoint contact states.

のような条件を追加し、式（１３）を

とし、式（１４）を

とすることも可能である。 Note that the present invention is not limited to the above-described embodiment, and can be changed as appropriate without departing from the spirit of the present invention. For example, in the above-described embodiment, the expression (13) is configured only under the condition for guaranteeing the contact stability of the contact point. For example, the upper limit value of the vertical contact force is set.

And add the condition like

And formula (14)

It is also possible.

  In the above-described embodiment, the present invention has been mainly described as a hardware configuration. However, the present invention is not limited to this, and a CPU (Central Processing Unit) executes a computer program for arbitrary processing. Can also be realized. In this case, the computer program can be stored and provided to the computer using various types of non-transitory computer readable media. Non-transitory computer readable media include various types of tangible storage media. Examples of non-transitory computer-readable media include magnetic recording media (for example, flexible disks, magnetic tapes, hard disk drives), magneto-optical recording media (for example, magneto-optical disks), CD-ROMs (Read Only Memory), CD-Rs, CD-R / W, semiconductor memory (for example, mask ROM, PROM (Programmable ROM), EPROM (Erasable PROM), flash ROM, RAM (Random Access Memory)). The program may also be supplied to the computer by various types of transitory computer readable media. Examples of transitory computer readable media include electrical signals, optical signals, and electromagnetic waves. The temporary computer-readable medium can supply the program to the computer via a wired communication path such as an electric wire and an optical fiber, or a wireless communication path.

DESCRIPTION OF SYMBOLS 100 Multipoint contact robot 210 Controller 211 Contact point plan part 212 Trajectory generation part 2121 Trajectory generator 2122 Optimal gain calculator 213 State estimation part 2131 State estimator 214 State stabilization part 2132 Error calculator 2142 State feedback 215 Contact force control part 216 Whole body IK part 217 Joint servo part

Claims (7)

A multi-point contact robot that has a plurality of contact points, moves the contact points in contact with both the floor and other than the floor,
A trajectory generator for generating a target gravity center trajectory and a reaction force target value for each of the contact points, wherein the target gravity center trajectory and the reaction force target value for the future are set in a first cycle, with the current state as an initial state. A trajectory generator to generate,
A state estimation unit that estimates a current center of gravity and calculates an error between the target center of gravity trajectory and the current center of gravity;
A state stabilizing unit that corrects the reaction force target value so that the error converges to the target gravity center trajectory,
The trajectory generator applies an optimal regulator that minimizes an evaluation function in the first period, and calculates a feedback gain for converging the error to the target centroid trajectory,
The state stabilizing unit corrects the reaction force target value based on the error and the feedback gain in a second period shorter than the first period;
Multi-point contact robot. The multipoint contact robot according to claim 1, wherein the trajectory generation unit calculates the feedback gain based on a constraint condition including a frictional force or a vertical force at the contact point. The multipoint contact robot according to claim 1, wherein the state stabilization unit corrects the reaction force target value by multiplying the error and the feedback gain. A control method for a multi-point contact robot that has a plurality of contact points, moves the contact points in contact with both the floor and other than the floor,
The current state as an initial state, the trajectory generation step of generating a reaction force target value of the target barycentric trajectory and the contact points each of said multipoint contact robot the future in the first period,
An error calculating step of estimating a current center of gravity and calculating an error between the target center of gravity trajectory and the current center of gravity;
Applying an optimal regulator that minimizes the evaluation function, and calculating an optimum gain calculating step for calculating a feedback gain for converging the error to the target centroid trajectory in the first period;
A state feedback step of correcting the reaction force target value based on the error and the feedback gain in a second period shorter than the first period. The multipoint contact robot control method according to claim 4, wherein the optimum gain calculating step calculates the feedback gain based on a constraint condition including a frictional force or a vertical force at the contact point. The multipoint contact robot control method according to claim 4, wherein the state feedback step corrects the reaction force target value by multiplying the error and the feedback gain.   A program for causing a computer to execute the control method for a multipoint contact robot according to any one of claims 4 to 6.

Images