JPWO2015087504A1 - Mobile robot movement control method and mobile robot - Google Patents

Abstract

In the contact point plan setting step, a contact point plan is set in which the position of the contact point for grounding the moving means and the posture of the moving means for grounding are time-series data. In the trajectory generation process, a prediction model is constructed that inputs the differential value of the contact force when the moving means is grounded, and this prediction model represents a change in the state of the mobile robot in the prediction interval of a predetermined time width. The time series data of the differential value of the contact force is obtained using a predetermined evaluation standard, and the center of gravity trajectory of the mobile robot is obtained from the time series data of the obtained differential value of the contact force and the prediction model. The evaluation standard includes a standard that “a contact force is distributed to each contact point based on a weight set corresponding to each contact point”.

Description

  The present invention relates to a mobile robot movement control method and a mobile robot. More specifically, the present invention relates to a trajectory generation technique for generating a trajectory that allows a mobile robot that moves while touching multiple points to move stably.

  Robots having a function of moving autonomously have been developed, and typically there are legged robots that walk biped. In a biped robot, stable bipedal movement is realized by trajectory generation and operation control using ZMP (for example, Patent Document 1).

Japanese Patent Laid-Open No. 2005-17784 Japanese Patent No. 5034235

  Future robots are expected to play an active role in various environments. For example, service robots are expected to be active in the same living space as people. In order for the robot to move in an environment such as a living space, it is considered that it is not sufficient to simply perform legged bipedal walking. For example, the situation as shown in FIG. In FIG. 1, the table is near the wall, and the bottle is placed closer to the wall on the far side of the table. In order for the robot to grab the bottle, it is impossible to simply walk on two legs. Robots must move while putting their hands on a wall or table, or tilt their bodies and extend their hands. As described above, the service robot needs to have an ability to exercise while making contact with the environment at a plurality of arbitrary points.

  For future explanation, movement (movement) while contacting the environment at a plurality of arbitrary points will be referred to as “multi-point contact movement” in this specification.

  Most of the conventional methods for stabilizing humanoid robots are technologies related to stabilization of biped walking, and technologies for dealing with multipoint contact movement have not been developed so far. Biped walking is also a type of multipoint contact movement because it is an action of moving while switching between a single leg state and a both leg state. However, there are significant differences between the bipedal walking technique and the multipoint contact movement technique. In many cases, biped walking technology (ZMP is a representative example) is based on the premise that contact points (left foot and right foot in terms of two feet) are included in the same plane. (This is also described in, for example, paragraph 0012 of Patent Document 2.) That is, in ZMP control, only two-dimensional information can be expressed out of forces and moments that are essentially six dimensions in total. However, only very limited contact state transitions can be handled.

  A control system developed to cope with multi-point contact movement is disclosed in Patent Document 2. However, the technology disclosed in Patent Document 2 has the following points as a fundamental problem. That is, the technique disclosed in Patent Document 2 has a problem that a stable trajectory cannot be created unless the target center of gravity position and the target momentum are given, in addition to the planning of the contact point. It is desirable for the user to give only a contact point plan to the robot, and then automatically generate a stable center of gravity trajectory based on the set contact point plan and move autonomously.

  In the first place, the future target center-of-gravity position cannot be known in advance, and it is impossible for the user to set the future center-of-gravity position that should be unknown as the control target value. (In other words, in the case of bipedal walking, the target ZMP trajectory is first, and the center of gravity trajectory is generated based on this target ZMP trajectory, and the target center of gravity position cannot be known in advance. .)

  In addition, the technique disclosed in Patent Document 2 has a problem that the force change accompanying the switching of the contact point (contact state transition) becomes steep, and smooth force transition cannot be realized.

The above problems are common to not only service robots that are active in the living environment but also mobile robots that are active in narrow spaces or spaces with many obstacles.
In view of the above problems, an object of the present invention is to provide a mobile robot movement control method that automatically generates a stable trajectory based on a contact point plan.

The mobile robot movement control method of the present invention comprises:
A movement control method of a mobile robot that moves while alternately grounding two or more moving means,
A contact point plan setting step for setting a contact point plan with time-series data of the position of the contact point for grounding the moving means and the posture of the moving means for grounding;
A trajectory generating step for generating a trajectory for moving while moving the moving means to the contact point as set in the contact point plan setting step,
The trajectory generation step includes
By constructing a prediction model that receives as input a differential value of the contact force when the moving means is grounded, the prediction model represents a change in the state of the mobile robot in a prediction section of a predetermined time width,
In the prediction section, using a predetermined evaluation criterion, to obtain time series data of the differential value of the contact force,
From the time series data of the differential value of the obtained contact force and the prediction model, the center of gravity trajectory of the mobile robot is obtained.
The evaluation criteria include
Based on the weight set corresponding to each contact point, a criterion for allocating contact force to each contact point is included.

In the present invention,
The evaluation criteria further includes:
It is preferable that a criterion for allocating a differential value of the contact force to each contact point based on the weight set corresponding to each contact point is included.

In the present invention,
The evaluation criterion is preferably to minimize an evaluation function J including a contact force and a square sum of a differential value of the contact force within a prediction interval.

In the present invention,
In solving the minimization problem of the evaluation function J under constraint conditions,
The constraint equation includes a vertical trajectory and a moment trajectory around the center of gravity of the mobile robot.
The trajectory generation step includes
In the prediction interval, a process of temporarily setting the center of gravity vertical trajectory and the center of gravity moment trajectory of the mobile robot once;
After solving the minimization problem of the evaluation function J under the constraint condition, the center of gravity vertical trajectory and the center of gravity moment trajectory are corrected so that the motion of the mobile robot is stabilized within the prediction interval. And
Preferably, the minimization problem of the evaluation function J is recalculated at least once using the corrected center-of-gravity vertical trajectory and the center-of-gravity moment trajectory.

In the present invention,
A weight is set for each moving means,
In the evaluation function J, it is preferable to multiply the contact force and the differential value of the contact force by weights.

In the present invention,
In the trajectory generation step, the trajectory is determined within the prediction interval,
The mobile robot is driven using the data of the first point in this prediction interval as the current input value,
It is preferable to repeat the trajectory generation step by advancing the prediction interval by a minute time.

The mobile robot of the present invention is
A mobile robot that moves while grounding two or more moving means alternately,
A contact point plan setting unit for setting a contact point plan with time-series data of the position of the contact point for grounding the moving means and the posture of the moving means for grounding;
A trajectory generator for generating a trajectory for moving while moving the moving means to the contact point as set in the contact point plan setting unit,
The trajectory generator is
By constructing a prediction model that receives as input a differential value of the contact force when the moving means is grounded, the prediction model represents a change in the state of the mobile robot in a prediction section of a predetermined time width,
In the prediction section, using a predetermined evaluation criterion, to obtain time series data of the differential value of the contact force,
From the time series data of the differential value of the obtained contact force and the prediction model, the center of gravity trajectory of the mobile robot is obtained.
The evaluation criteria include
Based on the weight set corresponding to each contact point, a criterion for allocating contact force to each contact point is included.

  According to the present invention, the mobile robot can automatically generate a stable center-of-gravity trajectory and move autonomously based on the set plan of contact points. At this time, the force change accompanying the switching of the contact point (transition of the contact state) becomes smooth, and a stable and smooth movement of the mobile robot is realized.

The figure which shows an example of operation | movement of a robot. The figure which shows an example of the machine structure of a mobile robot. The functional block diagram of a mobile robot. The figure which shows 6 axial force. Functional block diagram of the controller. The figure which shows an example of the outline | summary of a contact point plan. The figure which shows a contact point plan. The figure which shows the example of a prediction area. The figure showing the movement in a prediction area. The figure for demonstrating the shift of a prediction area. The figure which shows the model of the mobile robot used for prediction. The figure for demonstrating the discretization of a prediction area. The figure which shows the coordinate system (with the superscript l (el)) of a contact point, and a contact polygon (support polygon of a contact point). The figure which illustrated the case where a contact point became unstable. The figure which shows the whole flow of the movement control method of a mobile robot. The detailed flowchart for demonstrating the procedure of a track | orbit production | generation process. The detailed flowchart for demonstrating the procedure of a track | orbit production | generation process. The detailed flowchart for demonstrating the procedure of a track | orbit production | generation process. The figure which shows the multi-mass point model of a mobile robot. The figure which shows the multi-mass point model of a mobile robot. The figure which shows a mode that the trajectory of a hand tip and a foot tip was interpolated. The figure which shows the experimental example in the case of bipedal walking. FIG. 22 is a diagram showing a snapshot of the execution result. The graph which showed the gravity center position at the time of bipedal walk execution, the position of reaction force center, and the vertical load of a contact point. The graph which showed the gravity center position at the time of bipedal walk execution, the position of reaction force center, and the vertical load of a contact point. The graph which showed the gravity center position at the time of bipedal walk execution, the position of reaction force center, and the vertical load of a contact point. The figure which shows the experimental example in the case of four-legged walking. This is a snapshot of the execution result. The graph which showed the gravity center position at the time of quadruped walking execution, the position of reaction force center, and the vertical direction load of a contact point. The graph which showed the gravity center position at the time of quadruped walking execution, the position of reaction force center, and the vertical direction load of a contact point. The graph which showed the gravity center position at the time of quadruped walking execution, the position of reaction force center, and the vertical direction load of a contact point. The figure which shows the example of an experiment at the time of making the way of limbs random by walking on 4 legs. This is a snapshot of the execution result. The graph which showed the gravity center position at the time of random quadruped walking execution, the position of reaction force center, and the vertical direction load of a contact point. The graph which showed the gravity center position at the time of random quadruped walking execution, the position of reaction force center, and the vertical direction load of a contact point. The graph which showed the gravity center position at the time of random quadruped walking execution, the position of reaction force center, and the vertical direction load of a contact point. The figure which shows a modification.

An embodiment of the present invention will be illustrated and described with reference to reference numerals attached to elements in the drawing.
(First embodiment)
This embodiment has a feature in the movement control method of the mobile robot. Specifically, it has a feature in the generation of a trajectory for controlling the movement operation of the mobile robot. Specific control (trajectory generation) will be described. Prior to this, the hardware configuration of the mobile robot to be controlled will be described in advance.

FIG. 2 is a diagram illustrating an example of the mechanical configuration of the mobile robot.
The mobile robot 100 has three axes for the hip joint, one axis for the knee joint, and two axes for the ankle joint. Furthermore, the shoulder joint is composed of 3 axes (shoulder pitch, shoulder roll, shoulder yaw), the elbow joint is composed of 1 axis (elbow pitch), and the wrist joint is composed of 3 axes (wrist yaw, wrist pitch, wrist roll). ing. (The mechanical configuration of the mobile 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 mobile robot 100 has motors 1, 2,..., 28 with encoders at each joint. Motors (joint driving means) 1a, 2a,..., 28a (FIG. 3) 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,.

In addition, the mobile robot 100 has a contact force sensor 25 at a foot tip (foot portion) and a hand tip (palm portion). Here, the contact force is a six-axis force. As shown in FIG. 4, 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.

The mobile robot moves while contacting one or more of the right foot, left foot, right hand, and left hand with the floor, wall, table, or the like. Therefore, in the following description of the present specification, the right foot, the left foot, the right hand, and the left hand may be referred to as contact point candidates.
In addition, the hand and foot are the moving means.

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

  Sensor detection values are input to the controller 210 from the encoders 1b to 28b and the contact force sensor 25 of each joint. Moreover, the controller 210 outputs a drive signal to the motors 1a to 28a 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.

A functional block diagram of the controller 210 is shown in FIG.
The controller 210 includes a contact point plan setting unit 221 that stores a contact point plan instructed by a user, a trajectory generation unit 222 that generates a trajectory that can stably execute an operation according to the contact point plan, and a generated trajectory. And an operation control unit 223 that executes the whole body operation of the mobile robot 100. Specific processing operations of these functional units will be described later.

  In order for a mobile robot to perform multi-point contact movements stably, technology is needed to distribute contact force smoothly and appropriately according to contact points that change from moment to moment, and to generate a stable center of gravity trajectory. It is. For this purpose, the present inventor tried to use model predictive control. First, an outline of model predictive control will be described.

(Overview)
For example, assume that the robot wants to perform a moving operation as shown in FIG. Here, a series of operations are assumed in which a humanoid robot having two arms and two legs holds a bottle on the back side of the table. In this case, the user creates a contact point plan that can execute the series of operations (tasks). That is, as shown in FIG. 6, a plan is created for how and where to get the hands and feet. In FIG. 6, the floor, the wall, and the table are marked at locations where the feet and the hands are brought into contact.

This contact point plan is specifically as shown in FIG.
The contact point plan is time-series data regarding which order, where, and how to arrive for the left hand (LH), right hand (RH), left foot (LF), and right foot (RF).
The correspondence between FIGS. 1, 6 and 7 will be briefly described.
Initially (t0) Stand with only one left foot, swing out the right foot, which is a free leg, and land the right foot (t1). In order to instruct the mobile robot to make this motion plan, the coordinate PLF1 of the contact point on the floor where the left foot first lands, the posture rLF1 of the left foot at that time, and the coordinate PRF1 of the contact point on the floor where the right foot lands It is necessary to specify the posture rRF1 of the right foot at that time. Here, the coordinates of the contact point are represented as a set of P = (P _x , P _y , P _z ) as spatial coordinates. Further, the posture is the direction of the back surface of the foot when landing on the contact point, and is represented as, for example, r = (r _x , r _y , r _z ) as a set of Euler angles. (In other words, r _x , r _y, and r _z represent roll, pitch, and yaw angle, respectively.) Since the format for instructing the coordinates of the contact point on the foot and the posture at that time is the same in the following description, it will be appropriately determined thereafter. Description is omitted.

After standing with both feet, swing out the left foot (t ₂ ) and land the left foot forward (t ₄ ). In the meantime, the left hand is put on the wall (t ₃ ). Here, the coordinates P _LH1 of the contact point on the wall that _wears the left hand and the posture r _LH1 of the left hand at that time are designated. The coordinates of the contact point are expressed as a set of P = (P _x , P _y , P _z ) as spatial coordinates, and the posture is set as a set of Euler angles as a set of Euler angles when reaching the contact point. , Ry, rz).

  Subsequent contact point plans will be omitted if they can be understood by comparing FIGS. 1, 6 and 7. FIG. In this way, the contact point plan is created as time-series data by the user.

  When the contact point plan thus created is input to the mobile robot, the mobile robot generates a trajectory and moves autonomously so as to realize the contact point plan. At this time, the mobile robot performs model predictive control in generating the trajectory. That is, a trajectory on which the mobile robot can stably move within a prediction interval having a certain time width is generated, and a trajectory that can perform a stable operation is sequentially updated while shifting the prediction interval by a minute time (Δt). For example, FIG. 8 shows an example of a prediction interval. The prediction interval is set from the present to the future ahead of a predetermined time (for example, 1.6 seconds).

  A stable trajectory is generated so as not to diverge between the prediction intervals. FIG. 9 is an image of the motion in the prediction interval. In this way, after generating a stable trajectory in a prediction interval having a certain time width, only the first point is used as the current input value.

  In the next orbit update period (after Δt seconds), the prediction interval is shifted, and a stable orbit is similarly generated in the new prediction interval (see FIG. 10).

  Instead of looking only at the present or only from the present to the next control period (Δt seconds), as described above, a certain future is assumed to be a prediction interval, and a trajectory that does not diverge within this prediction interval is generated. To. By repeating this, the mobile robot can move stably.

  Now, the problem here is to generate a stable center of gravity trajectory that distributes the contact force smoothly and appropriately according to the contact points that change from moment to moment in the prediction interval with a certain time width. What should I do?

  The present inventors have come up with the idea of reducing the problem of generating stable trajectories in a certain prediction interval to a quadratic programming problem (Quadratic Programming: QP). Specifically, by solving the problem of minimizing the evaluation function J including the sum of squares of the contact force at each contact point and the square sum of the differential values of the six-axis forces, the multipoint contact movement can be stabilized. Find the trajectory.

  Therefore, the derivation of the evaluation function J and its solution (reduction to the convex quadratic programming problem) will be described next. This solution shows that time series data of the center of gravity position, the center of gravity speed, the contact force, and the differential value of the contact force can be obtained in order to realize stable multipoint contact movement within a certain prediction section.

  A model of the mobile robot used for the prediction is shown again in FIG. The inertia of the entire mobile robot is represented by one center of gravity G. A 6-axis force is defined for each contact point.

  At this time, if the translational momentum of the center of gravity G is P, the rotational momentum (angular momentum) around the center of gravity is L, and the number of contact points is n, the equation of motion can be written as follows.

  The subscript i represents the index of the contact point. For example, if the contact point candidates are four points of the left hand, right hand, left foot, and right foot, n = 4 (left hand: LH = 1, right hand: RH = 2, left foot: LF = 3, right foot: RF = 4). That's fine. However, for the contact point candidates that are not in contact with the floor or wall, the constraint condition is set so that the contact force is zero. For example, in the example of FIG.

  Differentiating the first expression and the second expression of the expression (1) yields the following expression. (Equation (1) is expressed as a vector, but after decomposing it into x, y, and z, they are called the first equation, the second equation,...

  In this embodiment, these two types are used as a system. Then, Formulas 3 to 5 of Formula (1) are formulated as constraint conditions.

  Further, the prediction interval is divided into N intervals as shown in FIG. 12, and equations (3) and (4) are discretized. The equation (3) is discretized as follows.

  Further, if the constraint of equation (4) always holds at the sampling point, equation (4) is discretized as follows.

  Here, the parameters are set as follows.

θ _i is a vector in which contact forces as six-axis forces are arranged. X is an x coordinate of the center of gravity G, an x-axis direction speed of the center of gravity G, a y-coordinate of the center of gravity G, a y-axis direction speed of the center of gravity G, and a contact force (six-axis force) at each contact point. Is a vector. This x is referred to as a state variable x. Furthermore, u is a vector in which the differential values of the contact force (six-axis force) are arranged.

  When parameters are set in this way, equation (5) can be described as follows.

  This equation (8) indicates that the state variable x at the time of (j + 1) can be described in the previous state. When the state variable x in the prediction interval is calculated in order using the equation (8), it is as follows.

  Therefore, when the state variables x obtained in time series are arranged and represented by capital letters X, the time series data X of the state variables can be represented as follows.

This equation (10) is a prediction model that represents the state transition of the mobile robot within a certain prediction section with the contact force differential value (U [k]) as an input.
Now, the present inventors have introduced the following evaluation function J in order to generate a stable trajectory within the prediction interval.

Qi and Ri are weights set as appropriate. The concept of the meaning of the weight and how to distribute the weight to the contact point candidates will be described in detail later. Here, for example, assuming that the force is equally distributed to all the contact point candidates, it is only necessary to imagine that Qi is all 1 and Ri is 1 × 10 ⁻⁶ .

Here, θ _i is a vector in which components of contact force as 6-axis force are arranged. θ _i (·) is a vector in which the differential values of the contact force components are arranged. (The dot in parentheses after the interpretation should be interpreted as representing differentiation.)
Therefore, the expression (11) is an expression that means “minimize the sum of squares of the contact force (6-axis force) and the contact force (6-axis force) differential value within the prediction interval”. The first term in equation (11) means minimizing the sum of squares of the contact force (six-axis force).
This first term includes the following actions.
(1) Distribute the contact force to each contact point evenly. This has the effect of moving the center of gravity to the most stable position possible.
(2) To cancel unnecessary internal forces.
(3) To improve the grounding stability of the contact point. That is, there is an effect that the reaction force center point in the contact surface is set to the center of the contact surface.

Further, the second term of the equation (11) means that the sum of squares of the contact force (6-axis force) differential value (time change rate of 6-axis force) is minimized.
This second term includes the following actions.
(1) To suppress the divergence of the center of gravity.
(2) Switching contact force smoothly.

Minimizing this evaluation function J by adding these appropriately by the weights of Q and R means that
"Stable center of gravity trajectory and contact force at each contact point are output while satisfying conditions such as high contact stability, smooth contact force transition, and minimum internal force"
It means that.

  When the equation (11) is discretized, the following equation is obtained.

  Here, since the contact force (six-axis force) is included in the state variable x, the equation (7) is modified as follows.

  Therefore, the calculation of the first sigma in equation (12) is as follows.

  Therefore, equation (12) is as follows.

  Furthermore, the evaluation function J can be formulated as follows by using the equation (10).

Next, let us consider the constraint conditions.
As a constraint condition,
The constraint that the 6-axis force is 0 with respect to the non-contact point candidate as shown in equation (2),
(6) vertical force balance constraint of equation, and
Constraint on the balance of moments about the xy axis in equation (6),
Must hold over all sampling points in the prediction interval.
Here, for example, it is assumed that the i-th and i + 2th contact points are non-contact at a certain sampling point j. At this time, the equations (2) and (6) can be described as follows.

  Here, although the components of the coefficient matrices C [j] and d [j] differ depending on the sampling points, information such as contact / non-contact of contact point candidates and contact point positions are already given.

The center-of-gravity vertical trajectory (Gz [j]) and center-of-gravity moments (Lx (•) [j], Ly (•) [j]) will be described in detail later, but here they are understood as follows. I want.
If the center of gravity vertical trajectory or center of gravity moment is not determined, a temporary value is set and used. For example, the center-of-gravity vertical trajectory and center-of-gravity moment used in the previous control cycle may be used as temporary values. Alternatively, the center-of-gravity vertical trajectory and the center-of-gravity moment may be provisionally set so as not to move from the current state within the prediction interval. Then, after calculating once with the provisional value, the center of gravity vertical trajectory and the center of gravity moment are updated using the obtained results, and the solution is repeated.
In this way, it is assumed that the calculation of the center of gravity vertical trajectory and the moment around the center of gravity proceeds as a known value, and the deviation is corrected in the update calculation.

  Now, in order for Equation (17) to hold at all future sampling points, the following equation needs to hold.

  The equation (10) is substituted into this equation.

Finally, conditions are introduced to keep the contact point stable and in contact.
FIG. 13 shows the coordinate system of the contact point (with the superscript l (el)) and the contact polygon (support polygon of the contact point). Coordinate system of the touch point, the contact point as the origin, and a are defined in accordance with the orientation r _i of the contact surface.
Here, the contact force (six-axis force) θ _i ^l defined in the coordinate system of the contact point is expressed as follows.
^{_{θ i l = [f ix l}} , f iy l, f iz l, τ ix l, τ iy l, τ iz l] T
Then, the contact force (six-axis force) θ _i ^l can be expressed as follows using the posture matrix Φi = rot (r _i ) of the contact surface. Note that rot is a function that converts Euler angles into a posture matrix.

In order for the contact point to maintain stable contact,
(1) The contact point must not be separated,
(2) The contact point does not slip,
(3) The contact point does not peel off,
It is necessary to satisfy these three conditions.
To facilitate understanding of the above three requirements, FIG. 14 illustrates a case where the contact point is unstable.

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

(2) To prevent the contact point from slipping, 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 condition for the contact point not to be peeled off is that the vertex coordinates (x _i1 ^l , y _i1 ^l ) of the contact polygon (x _ih ^l , y _ih ^l )
Is expressed as follows.
(However, the vertices of the contact polygon are given in order counterclockwise).

  The equations (21), (22), and (23) can be summarized as follows.

  Substituting equation (20) into equation (24), an index is added as the j-th sampling point.

Expression (25) is a condition that must be satisfied in order for the i-th contact point at the j-th sampling point to maintain a stable contact, and in order to realize a stable multi-point contact operation, all the contacts at all sampling points are satisfied. It is necessary to hold the equation (25) at the point.
First, the condition for all the contact points to satisfy the equation (25) at the j-th sampling point can be written as follows.

  The condition that this holds for all sampling points can be written as follows.

  The formula (10) is substituted for this.

  Finally, the stable trajectory generation problem at the time of multipoint contact is reduced to the following convex quadratic programming problem by the equations (16), (19), and (28).

  If it can be reduced to a quadratic programming problem (Quadratic Programming: QP), the convex quadratic programming problem (QP) itself is one of the well-known optimization problems and can be solved at high speed. (When the solution is obtained, U [k] is obtained.) After the solution of QP, the gravity center trajectory and the contact force of each contact point are calculated as follows.

In this way, X [k + 1] that satisfies the constraint conditions (1)-(3) and minimizes the evaluation function J in the prediction interval is obtained.
X [k + 1] means that the x-coordinate of the center of gravity, the x-axis direction speed of the center of gravity, the y-coordinate of the center of gravity, the y-axis speed of the center of gravity, It is time series data of contact force (6 axis force) of a contact point.

  Further, as explained in the middle of the development of the formula, the vertical trajectory (z coordinate) of the center of gravity and the moment around the center of gravity are corrected in the update calculation (details will be described later). That is, a trajectory that achieves stable movement within the prediction interval can be obtained.

Add a word to avoid confusion.
As can be seen from the above calculation process and the result obtained by this calculation, the “trajectory” here refers to the contact force (6 axes) to be generated at each contact point in addition to the three-dimensional trajectory of the center of gravity. It is easy to understand when considering the time series data. Note that the “trajectory” for moving the mobile robot is generally a concept including the trajectory of the hand and foot, but in the description of the present embodiment, the “trajectory” refers to the hand and foot. There is no big difference whether or not the orbit is included. From the viewpoint of generating a stable trajectory in the present embodiment, the trajectory of the hand and foot can be determined almost automatically by interpolation from the time series data of the contact point plan, the center of gravity trajectory and the contact force. Therefore, in the description of the present embodiment, when “trajectory” is used, there is no great difference whether or not the trajectory of the hand and foot is included.

(Control of mobile robot)
A movement control method of the mobile robot will be described.
FIG. 15 is an overall flow of the mobile robot movement control method.
The movement control process of the mobile robot is roughly divided into a contact point plan setting (ST100), a trajectory generation (ST200), an operation command (ST300), and an operation control execution (ST400).

  In the setting of the contact point plan (ST100), as outlined above, a multi-point contact movement plan in which order, where and how to reach the hand and foot (FIG. 7). Is input to the mobile robot. The input contact point plan is stored in the contact point plan setting unit 221 of the controller 210.

The trajectory generation step (ST200) will be described.
FIGS. 16, 17, and 18 show detailed flowcharts for explaining the procedure of the trajectory generation step (ST200). The trajectory generation step (ST200) is executed by the trajectory generation unit 222 of the controller 210.
First, in ST201, the trajectory generation unit 222 sets the current state x [k]. Here, the current state (x [k]) should be set as the current x-coordinate of the center of gravity, the x-axis direction speed of the center of gravity, the y-coordinate of the center of gravity, the y-axis speed of the center of gravity, and the contact force at each contact point. (6-axis force). The contact force at each contact point can be obtained from the detection value from the force sensor 25. The x-coordinate of the center of gravity, the x-axis direction speed of the center of gravity, the y-coordinate of the center of gravity, and the y-axis speed of the center of gravity are values obtained by estimation from the current state of the mobile robot.
For example, the sensor value of each encoder may be acquired to grasp the whole body posture of the mobile robot, and the x-coordinate of the center of gravity, the x-axis direction speed of the center of gravity, the y-coordinate of the center of gravity, and the y-axis speed of the center of gravity may be obtained. Alternatively, the position of the center of gravity may be corrected by grasping how much the actual posture of the mobile robot is inclined (deviation) from the assumed posture.

Next, in ST202, weights (Q _i , R _i ) are set.
(Concerning the expansion of the previous equation, confirm that the weight is incorporated in the evaluation function J at the equation (11).)
The weights (Q _i , R _i ) are set and input by the user in advance, and the trajectory generation unit 222 reads and updates as necessary. (If you do not need to update, you need not. Resetting) when a force is distributed equally to the contact point candidate all, and all the Q _i 1, also good as all 1 × 10 ^-6 to R _i You can also set it more finely. By adjusting this weight, it is possible to make adjustments such as how smooth the change in contact force is and how little force is applied to the hand.

  For example, assume that all weights are the same. Then, as a calculation result, a solution is obtained in which force is evenly applied to the landing limb (each contact point). Generally speaking, this is the most stable form (posture) for a mobile robot.

  On the other hand, there is a case where it is desired to change the weight distribution. For example, if the mobile robot is weaker (weaker) than the foot and it is not good to put too much load on the hand, the hand weight is increased. As the weight is increased, the hand θi and the like multiplied by the weight is reduced, so that the distribution of the contact force to the hand is reduced.

  Alternatively, suppose that one hand is put on the table and the other hand is greatly extended forward. In this case, it is impossible to distribute the force evenly to the limbs, and it is necessary to tilt the body forward. In order to tilt the body and shift the center of gravity further forward, it is necessary to slightly increase the distribution of the force applied to one hand. In this case, it is necessary to adjust such that the weight of one hand is reduced, the body can be tilted, and the other hand is extended more forward.

  In a general operation, the weight does not need to be changed, but may be changed every time at the timing of the trajectory update depending on the situation.

Next, in ST203, a prediction interval is set.
As described with reference to FIG. 8, this means that a predetermined time width (for example, 1.6 seconds) from the present is set as the prediction interval.

  Next, in ST204, the contact point position and its posture (pi, ri) within this prediction section are set. This is equivalent to cutting out the contact point plan in the prediction section from the contact point plan (FIG. 7).

Next, in ST205, a gravity center vertical trajectory and a gravity center moment are temporarily set.
The point of this embodiment is to solve the equation (25) (convex quadratic programming problem) in the calculation process (ST206) of the next step, and obtain the xy-direction trajectory of the center of gravity, the contact force at each contact point, and the contact force differential value. There is to get.
However, this solution is based on the premise that the position, posture, center of gravity vertical trajectory, and center of gravity moment are given to each contact point. The position and orientation of each contact point within the prediction interval are given by the contact point plan. On the other hand, the center-of-gravity vertical trajectory and the center-of-gravity moment in this prediction section are unknown.
Therefore, the center-of-gravity vertical trajectory and the center-of-gravity moment are temporarily set once so that equation (25) (convex quadratic programming problem) can be solved in the calculation step (ST206).

  In the completely first control cycle, for example, the center-of-gravity vertical trajectory and the center-of-gravity moment may be temporarily set so as not to move from the current state within the prediction interval. (For example, it may be assumed that the height of the center of gravity does not change.) Alternatively, the center of gravity vertical trajectory and the moment around the center of gravity used in the previous control cycle may be used as temporary values. The prediction interval is a minute time of about 1.6 seconds, and the change in the height of the center of gravity during this stable orbit is also minute. Therefore, as the first tentative calculation, even if the height of the center of gravity does not change, the calculation proceeds even if it is the same as the previous control cycle.

  After this, there is a step (ST209) of correcting the center-of-gravity vertical trajectory and the moment around the center of gravity so that the movement of the mobile robot is stabilized even in the xy-direction trajectory of the center of gravity obtained by provisional calculation. Then, since the loop is rotated so that the corrected center of gravity vertical trajectory and the center-of-gravity moment are calculated again (ST206), it can be understood that a stable trajectory including the center of gravity vertical trajectory is finally obtained appropriately. Like.

Here, the moment around the center of gravity is defined.
If the mobile robot is regarded as a link structure in which links are jointed by joints, the moment around the center of gravity is a total value of moments generated around the center of gravity due to the motion of the mass points distributed on each link.
The mobile robot is represented by a multi-mass model as shown in FIGS. 19 (A) and 19 (B).
_{Mass: m} i,
Position: s _i = [x _i , y _i , z _i ] ^T
Suppose you have the following parameters:
When the center of gravity of the entire mobile robot is G = (G _x , G _y , G _z ) ^T , the moment around the center of gravity is expressed as follows.

Next, a calculation process is performed in ST206.
Specifically, the convex quadratic programming problem of equation (29) is solved.
In the prediction interval, a trajectory is required in which the center of gravity (here, the center of gravity is x and y) stably transitions while realizing the contact force while appropriately distributing the force to each contact point. is there. Specifically, H, F, P, S, V, and W are obtained, and U [k] is obtained. (Recall that this U [k] was a vector of the contact force differential value.) Then, Xout [k + 1] is obtained by the equation (30). (This X _out [k + 1] is time series data of the x-coordinate of the center of gravity, the x-axis direction speed of the center of gravity, the y-coordinate of the center of gravity, the y-axis speed of the center of gravity, and the contact force (six-axis force) of each contact point. is there.)

  Subsequently, in ST208, the trajectory of the hand and foot is interpolated and obtained. In the contact point plan, only the contact point and the posture at that time are set. Therefore, the interpolation trajectories of the hand and the foot are obtained so as to connect the contact points. Then, as shown in FIG. 20, the space between the contact points specified in the contact point plan is filled.

  Note that. ST208 may be executed at any timing before the next ST209. In an extreme case, the interpolation trajectory of the hand and foot may be obtained immediately when the contact point plan is input.

Next, in ST209, an operation for correcting the center-of-gravity vertical trajectory and the center-of-gravity moment is performed.
First, the center of gravity vertical trajectory is corrected. By the steps so far, the trajectory of the center of gravity in the xy direction and the trajectory of the tip of the hand are obtained. (Furthermore, the center of gravity vertical trajectory is also temporarily set.) Based on this, it is possible to estimate the change in the posture of the mobile robot within the prediction interval, but at this time, the knees and arms are fully extended. If there is a factor that does not, adjust the center of gravity vertical trajectory to be stable. For example, when the knee is almost extended, the vertical center of gravity is lowered. When the posture of the mobile robot in the predicted section is determined, the moment around the center of gravity is calculated based on the determined posture.

Next, in ST210, it is determined whether ST206-ST209 has been repeated one or more times. If not repeated, the process returns to ST206 to solve the convex quadratic programming problem.
(Of course, ST208 may be skipped as necessary.)
By this repeated calculation (ST206), Xout [k + 1] is obtained again using the corrected center-of-gravity vertical trajectory and center-of-gravity moment.
In this way, time series data of the center of gravity position, the center of gravity speed, and the contact force that realizes the stable movement of the mobile robot within the prediction section was obtained (ST211).

  In this way, after generating a stable trajectory in a prediction interval having a certain time width, only the first point is used as the current input value. That is, only the first point is output as an operation command to the operation control unit 223 (ST300).

  The period is updated until a predetermined end condition is satisfied (ST213), that is, the current time is advanced by a minute time, the process returns to the head (ST201), and the loop is repeated. In this way, the trajectory generation is repeated while the prediction interval is advanced by a minute time. In this way, the trajectory that stably realizes multipoint contact movement is calculated.

How the motion control unit 223 that receives the motion command controls and moves the mobile robot 100 (whether to drive the joint) is not the point of the present embodiment, but will be described briefly.
In this embodiment, in addition to the center of gravity and the trajectory of the tip of the hand, the contact force at each contact point is also given as an operation command. Therefore, first, a step of finely correcting the position (trajectory) of the hand and foot is required so that the contact force instructed at each contact point is generated (ST410). For example, when a relatively large contact force is instructed while the hand is on the table, the hand trajectory is finely corrected so that the hand is pushed into the table.

  And the motor of each joint is driven and the whole body is operated (ST420). The mobile robot will now operate.

  In the execution of the operation (ST400), contact force control (ST410) is performed, the drive is not performed exactly as instructed by the joint, or it is affected by a disturbance, so there is a gap between the actual machine and the model. Naturally, a deviation occurs. Therefore, in the loop of trajectory generation (ST200), it is necessary to set the current state x every time (ST201).

  As described above, according to the present embodiment, the mobile robot can automatically generate a stable center-of-gravity trajectory and move autonomously based on the set plan of contact points. As can be seen from the meaning of the evaluation function J used to generate the trajectory, the change in force accompanying the switching of the contact point (transition of the contact state) becomes smooth, and stable and smooth movement of the mobile robot is realized. The

(Experimental example)
An experimental example will be described.
An experimental example in which the above embodiment is applied to the case of bipedal walking and quadrupedal walking is shown.
21 to 23 are examples in the case of bipedal walking. A contact point plan as shown in FIG. 21 was given to the mobile robot. (The contact points are only the right and left feet, and the right and left hands do not touch the floor or wall.)

FIG. 22 is a snapshot of the execution result.
Even if there are too many figures, it is redundant, so the middle and the last are thinned out, but if you can associate them with the contact point plan, you will understand the contents.
It can be seen that stable bipedal walking is realized, and a stable center of gravity trajectory is generated as the contact point changes.
23A, 23B, and 23C are graphs showing the position of the center of gravity, the position of the reaction force center, and the vertical load at the contact point when biped walking is performed. It can be seen that the contact point reaction force accompanying the switching of the contact points smoothly transitions.
It can also be confirmed that the center of all reaction forces smoothly transitions between the left and right legs.

Similarly, FIGS. 24 to 26 show a case of walking on four legs.
A contact point plan as shown in FIG. 24 was given to the mobile robot.
FIG. 25 is a snapshot of the execution result. (Similar to the above, it was thinned out.)
It can be seen that a stable quadruped walking is realized, and a stable center of gravity trajectory is generated according to the change of the contact point.
FIGS. 26A, 26B, and 26C are graphs showing the position of the center of gravity, the position of the reaction force center, and the vertical load at the contact point when the quadruped walking is performed.

Similarly, FIG. 27 to FIG. 29 are four-legged walks, in which limbs are randomly placed.
A contact point plan as shown in FIG. 27 was given to the mobile robot.
FIG. 28 is a snapshot of the execution result.
It can be seen that stable walking is realized even with random quadruped walking, and a stable center of gravity trajectory is generated according to the change of the contact point.
FIGS. 29A, 29B, and 29C are graphs showing the position of the center of gravity, the position of the reaction force center, and the vertical load at the contact point when random quadruped walking is performed.

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.
As a hardware configuration as a mobile robot, two-legged walking, four-legged walking, and even six or eight legs may be used, and the number of limbs is not limited.
However, at least one contact point must be grounded somewhere.
(Grounding is to be interpreted, including when you touch the wall.)
In an extreme case, for example, as shown in FIG. 30, the mobile robot 100 may have one leg and one hand and move while alternately putting limbs on the floor and the wall.

  This application claims the priority on the basis of Japanese application Japanese Patent Application No. 2013-254989 for which it applied on December 10, 2013, and takes in those the indications of all here.

DESCRIPTION OF SYMBOLS 1a-28a ... Motor, 1b-28b ... Encoder, 25 ... Contact-force sensor, 100 ... Mobile robot, 210 ... Controller, 221 ... Contact point plan setting part, 222 ... Trajectory generation part, 223 ... Operation control part.

Claims (5)

A movement control method of a mobile robot that moves while alternately grounding two or more moving means,
A contact point plan setting step for setting a contact point plan with time-series data of the position of the contact point for grounding the moving means and the posture of the moving means for grounding;
A trajectory generating step for generating a trajectory for moving while moving the moving means to the contact point as set in the contact point plan setting step,
The trajectory generation step includes
By constructing a prediction model that receives as input a differential value of the contact force when the moving means is grounded, the prediction model represents a change in the state of the mobile robot in a prediction section of a predetermined time width,
In the prediction section, using a predetermined evaluation criterion, to obtain time series data of the differential value of the contact force,
From the time series data of the differential value of the obtained contact force and the prediction model, the center of gravity trajectory of the mobile robot is obtained.
The evaluation criteria include
A standard for distributing the contact force and the differential value of the contact force to each contact point based on the weight set for each contact point is included,
The evaluation criterion is to minimize an evaluation function J including a contact force and a sum of squares of a differential value of the contact force within a prediction interval. The mobile robot movement control method according to claim 1,
In solving the minimization problem of the evaluation function J under constraint conditions,
The constraint equation includes a vertical trajectory and a moment trajectory around the center of gravity of the mobile robot.
The trajectory generation step includes
In the prediction interval, a process of temporarily setting the center of gravity vertical trajectory and the center of gravity moment trajectory of the mobile robot once;
After solving the minimization problem of the evaluation function J under the constraint condition, the center of gravity vertical trajectory and the center of gravity moment trajectory are corrected so that the motion of the mobile robot is stabilized within the prediction interval. And
A movement control method for a mobile robot, wherein the minimization problem of the evaluation function J is recalculated at least once using the corrected center-of-gravity vertical trajectory and the center-of-gravity moment trajectory. In the movement control method of the mobile robot according to claim 1 or 2,
A weight is set for each moving means,
In the evaluation function J, a moving control method for a mobile robot, wherein the contact force and the differential value of the contact force are each multiplied by a weight. In the movement control method of the mobile robot according to any one of claims 1 to 3,
In the trajectory generation step, the trajectory is determined within the prediction interval,
The mobile robot is driven using the data of the first point in this prediction interval as the current input value,
A movement control method for a mobile robot, wherein the trajectory generation step is repeated by advancing a prediction interval for a minute time. A mobile robot that moves while grounding two or more moving means alternately,
A contact point plan setting unit for setting a contact point plan with time-series data of the position of the contact point for grounding the moving means and the posture of the moving means for grounding;
A trajectory generator for generating a trajectory for moving while moving the moving means to the contact point as set in the contact point plan setting unit,
The trajectory generator is
By constructing a prediction model that receives as input a differential value of the contact force when the moving means is grounded, the prediction model represents a change in the state of the mobile robot in a prediction section of a predetermined time width,
In the prediction section, using a predetermined evaluation criterion, to obtain time series data of the differential value of the contact force,
From the time series data of the differential value of the obtained contact force and the prediction model, the center of gravity trajectory of the mobile robot is obtained.
The evaluation criteria include
A standard for distributing the contact force and the differential value of the contact force to each contact point based on the weight set for each contact point is included,
The mobile robot is characterized in that the evaluation criterion is to minimize an evaluation function J including a contact force and a square sum of contact force differential values within a prediction interval.

Images