WO2015087504A1 - 移動ロボットの移動制御方法、および、移動ロボット - Google Patents

移動ロボットの移動制御方法、および、移動ロボット Download PDF

Info

Publication number
WO2015087504A1
WO2015087504A1 PCT/JP2014/005905 JP2014005905W WO2015087504A1 WO 2015087504 A1 WO2015087504 A1 WO 2015087504A1 JP 2014005905 W JP2014005905 W JP 2014005905W WO 2015087504 A1 WO2015087504 A1 WO 2015087504A1
Authority
WO
WIPO (PCT)
Prior art keywords
trajectory
mobile robot
contact
center
contact point
Prior art date
Application number
PCT/JP2014/005905
Other languages
English (en)
French (fr)
Inventor
将弘 土井
Original Assignee
トヨタ自動車株式会社
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by トヨタ自動車株式会社 filed Critical トヨタ自動車株式会社
Priority to JP2015552305A priority Critical patent/JP6168158B2/ja
Publication of WO2015087504A1 publication Critical patent/WO2015087504A1/ja

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B62—LAND VEHICLES FOR TRAVELLING OTHERWISE THAN ON RAILS
    • B62D—MOTOR VEHICLES; TRAILERS
    • B62D57/00—Vehicles characterised by having other propulsion or other ground- engaging means than wheels or endless track, alone or in addition to wheels or endless track
    • B62D57/02—Vehicles characterised by having other propulsion or other ground- engaging means than wheels or endless track, alone or in addition to wheels or endless track with ground-engaging propulsion means, e.g. walking members
    • B62D57/032—Vehicles characterised by having other propulsion or other ground- engaging means than wheels or endless track, alone or in addition to wheels or endless track with ground-engaging propulsion means, e.g. walking members with alternately or sequentially lifted supporting base and legs; with alternately or sequentially lifted feet or skid

Definitions

  • 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 with the ability to move autonomously have been developed, and typically there are legged robots that walk biped.
  • stable bipedal movement is realized by trajectory generation and operation control using ZMP (for example, Patent Document 1).
  • Future robots are expected to play an active role in various environments.
  • service robots are expected to be active in the same living space as people.
  • the robot 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.
  • the service robot needs to have an ability to exercise while making contact with the environment at a plurality of arbitrary points.
  • multi-point contact movement movement (movement) while contacting the environment at a plurality of arbitrary points
  • multi-point contact movement movement (movement) while contacting the environment at a plurality of arbitrary points
  • 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.
  • biped walking technology ZMP is a representative example
  • ZMP is a representative example
  • contact points left foot and right foot in terms of two feet
  • Patent Document 2 A control system developed to support multi-point contact movement is disclosed in Patent Document 2.
  • 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.
  • 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.
  • 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 criterio
  • 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.
  • 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.
  • 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.
  • 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.
  • 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.
  • 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.
  • movement of a robot The figure which shows an example of the machine structure of a mobile robot.
  • 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 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 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 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.
  • 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.
  • joint driving means 1a, 2a,..., 28a (FIG. 3) of each joint can adjust the joint angles ⁇ 1, ⁇ 2,.
  • encoders joint angle detection means 1b, 2b,..., 28b of each joint can measure joint angles ⁇ 1, ⁇ 2,.
  • the mobile robot 100 has a contact force sensor 25 at a foot tip (foot portion) and a hand tip (palm portion).
  • the contact force is a six-axis force.
  • 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 1a to 28a and encoders 1b to 28b of each joint, a contact force sensor 25, and a controller 210.
  • the controller 210 receives sensor detection values from the encoders 1b to 28b and the contact force sensor 25 of each joint. Further, 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, calculation processing, and the like, and a ROM (Read Only Memory) 210b that stores control programs, calculation 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.
  • 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.
  • model predictive control 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.
  • FIG. 6 For example, assume that the robot wants to perform a moving operation as shown in FIG.
  • 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.
  • 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.
  • 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).
  • LH left hand
  • RH right hand
  • LF left foot
  • RF right foot
  • r x , r y, and r z represent roll, pitch, and yaw angle, respectively.
  • the left hand After standing with both feet, swing out the left foot (t 2 ) and land the left foot forward (t 4 ). In the meantime, the left hand is put on the wall (t 3 ).
  • 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 contact point plan will be omitted if it can be understood by comparing FIG. 1, FIG. 6 and FIG. In this way, the contact point plan is created as time-series data by the user.
  • the mobile robot When the contact point plan created in this way is input to the mobile robot, the mobile robot generates a trajectory and moves autonomously so as to realize this 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).
  • 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).
  • 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.
  • 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).
  • 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.
  • 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 quadratic programming problem (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.
  • QP quadratic quadratic programming problem
  • 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.
  • Fig. 11 shows the mobile robot model used for prediction.
  • 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.
  • 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,...
  • 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.
  • equation (4) is discretized 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.
  • I a vector.
  • This x is referred to as a state variable x.
  • u is a vector in which the differential values of the contact force (six-axis force) are arranged.
  • 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.
  • the state variable x in the prediction interval is calculated in order using the equation (8), it is as follows.
  • 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.
  • 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.
  • Qi is all 1
  • Ri is all 1 ⁇ 10 ⁇ 6 .
  • ⁇ 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 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.
  • 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.
  • the contact force (six-axis force) is included in the state variable x, so when the equation (7) is modified, the following is obtained.
  • equation (12) is as follows.
  • evaluation function J can be formulated as follows by using the equation (10).
  • center-of-gravity vertical trajectory Gz [j]
  • center-of-gravity moments Lx (•) [j], Ly (•) [j]
  • 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.
  • the center of gravity vertical trajectory and the center of gravity moment are updated using the obtained results, and the solution is repeated.
  • 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.
  • 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.
  • 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
  • FIG. 14 illustrates a case where the contact point is unstable.
  • the vertical force on the contact surface may be positive. That is, it is necessary to satisfy the following formula.
  • 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 .
  • 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 convex quadratic programming problem 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.
  • 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.
  • 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.
  • 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.
  • 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).
  • 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.
  • 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.
  • the trajectory generation unit 222 sets the current state x [k].
  • 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.
  • 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.
  • 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.
  • 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 there is no need to update, there is no need to reset.)
  • Q i may be all 1 and R i may be 1 ⁇ 10 ⁇ 6. 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.
  • weights do not need to be changed, but may be changed every time at the time of trajectory update depending on the situation.
  • 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.
  • 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.
  • 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.
  • 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).
  • 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.
  • 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.
  • 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.
  • a calculation process is performed in ST206. Specifically, the convex quadratic programming problem of equation (29) is solved.
  • 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.
  • 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.)
  • Xout [k + 1] is obtained by the equation (30).
  • the trajectory of the hand and foot is interpolated and obtained.
  • 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.
  • ST208 may be executed at any timing before the next ST209.
  • the interpolation trajectory of the hand and foot may be obtained immediately when the contact point plan is input.
  • an operation for correcting the center-of-gravity vertical trajectory and the center-of-gravity moment is performed.
  • the center of gravity vertical trajectory is corrected.
  • 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.)
  • the center of gravity vertical trajectory is also temporarily set.
  • 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.)
  • ST206 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).
  • Update the cycle until a predetermined termination condition is satisfied (ST213), that is, advance the current time by a minute time, return to the head (ST201), and repeat the loop.
  • a predetermined termination condition that is, advance the current time by a minute time, return to the head (ST201), and repeat the loop.
  • 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.
  • 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.
  • 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.
  • 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).
  • the mobile robot can automatically generate a stable center-of-gravity trajectory and move autonomously based on the set plan of the contact point.
  • the change in force accompanying the switching of the contact point transition of the contact state
  • stable and smooth movement of the mobile robot is realized.
  • 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.
  • 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.
  • 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.
  • 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.
  • 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.

Abstract

　接触点計画設定工程では、移動手段を接地する接触点の位置と接地するときの移動手段の姿勢とを時系列のデータとした接触点計画を設定する。軌道生成工程では、移動手段を接地するときの接触力の微分値を入力とする予測モデルを構築して、この予測モデルによって所定時間幅の予測区間における移動ロボットの状態変化を表わし、予測区間において、所定の評価基準を用いて接触力の微分値の時系列データを求め、この求まった接触力の微分値の時系列データと前記予測モデルとから、移動ロボットの重心軌道を得る。評価基準には、「各接触点に対応して設定された重みに基づいて各接触点に接触力を配分する」という基準が含まれる。

Description

移動ロボットの移動制御方法、および、移動ロボット
　本発明は、移動ロボットの移動制御方法、および移動ロボットに関する。より具体的には、多点に接触しながら移動する移動ロボットが安定に移動できるような軌道を生成するための軌道生成技術に関する。
　自律的に移動する機能を備えたロボットが開発されており、典型的には、二足歩行する脚式ロボットがある。二足歩行ロボットにあっては、ＺＭＰを用いた軌道生成と動作制御により、安定な二足歩行移動が実現されている（例えば特許文献１）。
特開２００５－１７７８８４号公報 特許５０３４２３５号
　これからのロボットには様々な環境で活躍することが期待されている。例えば、サービスロボットは人と同じ住空間で活動することが期待される。住空間のような環境でロボットが移動するためには、単に脚式の二足歩行ができるだけでは不十分であると考えられる。例えば、図１のような状況が考えられる。図１において、テーブルが壁の近くにあり、そして、テーブルの奥側において壁に近い方にボトルが置かれている。ロボットがこのボトルを掴むためには、単に二足歩行で進むだけでは無理である。ロボットは、壁やテーブルに手を着きながら移動したり、体を傾けて手を伸ばしたりしなければならない。このように、サービスロボットには、複数の任意の点で環境と接触しながら運動を行える能力が必要である。
　今後の説明のために、複数の任意の点で環境と接触しながら運動（移動）することを、本明細書では「多点接触移動」と称することにする。
　従来の人型ロボットの安定化手法は大多数が２足歩行の安定化に関する技術であり、多点接触移動に対応するための技術はこれまであまり開発されてこなかった。２足歩行も単脚状態と両脚状態とが切り替わりながら移動していく動作であるので多点接触移動の一種と言える。しかし、２足歩行の技術と多点接触移動の技術とで大きく違う点がある。２足歩行技術は多くの場合（ＺＭＰがその代表格）、各接触点（２足で言うと左足と右足）が同一平面内に含まれていることを前提にしている。（このことは、例えば、特許文献２の段落００１２にも記載されている。）すなわち、ＺＭＰ制御では、本来的には合計６次元である力・モーメントのうち２次元の情報しか表現できておらず、極めて限定された接触状態の遷移しか扱うことができないのである。
　多点接触移動に対応すべく開発された制御システムが特許文献２に開示されている。しかしながら、特許文献２に開示の技術には根本的問題として次ぎの点がある。すなわち、特許文献２に開示の技術では、接触点の計画だけではなく、目標重心位置や目標運動量を与えないと安定な軌道を作れないという問題がある。ユーザとしてはロボットに接触点の計画だけを与え、あとは、ロボットが設定された接触点の計画に基づいて自動的に安定な重心軌道を生成して自律的に移動してくれることが望ましい。
　そもそも、将来の目標重心位置を予め知ることはできないのであり、制御目標値として未知であるはずの将来の重心位置をユーザが設定するというのは無理がある。（２足歩行の場合で言い換えると、目標ＺＭＰ軌道が最初にあって、この目標ＺＭＰ軌道に基づいて重心軌道を生成するというのが軌道生成の技術であり、目標重心位置を予め知ることはできない。）
　また、特許文献２に開示の技術では、接触点の切替（接触状態の遷移）に伴う力の変化が急峻になってしまい、滑らかな力の遷移が実現できないという問題もある。
　上記のような問題は、住環境で活躍するようなサービスロボットだけではなく、狭い空間や障害物が多い空間などで活躍する移動ロボットにも共通する問題である。
　本発明の目的は、上記問題に鑑み、接触点計画に基づいて自動的に安定な軌道を生成する移動ロボットの移動制御方法を提供することにある。
　本発明の移動ロボットの移動制御方法は、
　二以上の移動手段を交互に接地しながら移動する移動ロボットの移動制御方法であって、
　前記移動手段を接地する接触点の位置と接地するときの前記移動手段の姿勢とを時系列のデータとした接触点計画を設定する接触点計画設定工程と、
　前記接触点計画設定工程で設定したように前記移動手段を接触点に接地しながら移動するための軌道を生成する軌道生成工程と、を備え、
　前記軌道生成工程は、
　前記移動手段を接地するときの接触力の微分値を入力とする予測モデルを構築して、この予測モデルによって所定時間幅の予測区間における移動ロボットの状態変化を表わし、
　前記予測区間において、所定の評価基準を用いて接触力の微分値の時系列データを求め、
　この求まった接触力の微分値の時系列データと前記予測モデルとから、移動ロボットの重心軌道を得るものであり、
　　前記評価基準には、
　　各接触点に対応して設定された重みに基づいて、各接触点に接触力を配分するという基準が含まれる
　ことを特徴とする。
　本発明では、
　前記評価基準には、さらに、
　各接触点に対応して設定された重みに基づいて、各接触点に接触力の微分値を配分するという基準が含まれる
　ことが好ましい。
　本発明では、
　前記評価基準は、接触力および接触力の微分値の二乗和を含む評価関数Jを予測区間内において最小化することである
　ことが好ましい。
　本発明では、
　前記評価関数Jの最小化問題を拘束条件のもとで求解するにあたり、
　前記拘束条件の式には、移動ロボットの重心鉛直軌道と重心回りモーメント軌道とを含んでおり、
　前記軌道生成工程は、
　予測区間内において、移動ロボットの重心鉛直軌道と重心回りモーメント軌道とを一度仮設定する工程と、
　前記評価関数Jの最小化問題を拘束条件のもとで一度求解した後、その結果に応じて、予測区間内において移動ロボットの動作が安定するように重心鉛直軌道と重心回りモーメント軌道とを修正し、
　修正した重心鉛直軌道と重心回りモーメント軌道とを用いて少なくとも一回以上、前記評価関数Jの最小化問題を再計算する
　ことが好ましい。
　本発明では、
　前記移動手段ごとに重みが設定されており、
　前記評価関数Jにおいて、接触力および接触力の微分値にそれぞれ重みを乗算しておく
　ことが好ましい。
　本発明では、
　前記軌道生成工程において予測区間内で軌道を求め、
　この予測区間の最初の一点のデータを現在の入力値として移動ロボットを駆動させ、
　予測区間を微小時間だけ進めて、前記軌道生成工程を繰り返す
　ことが好ましい。
　本発明の移動ロボットは、
　二以上の移動手段を交互に接地しながら移動する移動ロボットであって、
　前記移動手段を接地する接触点の位置と接地するときの前記移動手段の姿勢とを時系列のデータとした接触点計画を設定する接触点計画設定部と、
　前記接触点計画設定部で設定したように前記移動手段を接触点に接地しながら移動するための軌道を生成する軌道生成部と、を備え、
　前記軌道生成部は、
　前記移動手段を接地するときの接触力の微分値を入力とする予測モデルを構築して、この予測モデルによって所定時間幅の予測区間における移動ロボットの状態変化を表わし、
　前記予測区間において、所定の評価基準を用いて接触力の微分値の時系列データを求め、
　この求まった接触力の微分値の時系列データと前記予測モデルとから、移動ロボットの重心軌道を得るものであり、
　　前記評価基準には、
　　各接触点に対応して設定された重みに基づいて、各接触点に接触力を配分するという基準が含まれる
　ことを特徴とする。
　本発明によれば、設定された接触点の計画に基づいて移動ロボットが自動的に安定な重心軌道を生成して自律的に移動できる。このとき、接触点の切替（接触状態の遷移）に伴う力の変化が滑らかになり、移動ロボットの安定かつ滑らかな移動が実現される。
ロボットの動作の一例を示す図。 移動ロボットの機械構成の一例を示す図。 移動ロボットの機能ブロック図。 ６軸力を示す図。 コントローラの機能ブロック図。 接触点計画の概要の一例を示す図。 接触点計画を示す図。 予測区間の例を示す図。 予測区間での動きを表わした図。 予測区間のシフトを説明するための図。 予測に用いる移動ロボットのモデルを示す図。 予測区間の離散化を説明するための図。 接触点の座標系（上添え字ｌ（エル）がついている）と、接触多角形（接触点の支持多角形）と、を示す図。 接触点が不安定化する場合を例示した図。 移動ロボットの移動制御方法の全体フローを示す図。 軌道生成工程の手順を説明するための詳細なフローチャート。 軌道生成工程の手順を説明するための詳細なフローチャート。 軌道生成工程の手順を説明するための詳細なフローチャート。 移動ロボットの多質点モデルを示す図。 移動ロボットの多質点モデルを示す図。 手先、足先の軌道を補間した様子を示す図。 二足歩行の場合の実験例を示す図。 図２２は、実行結果のスナップショットを示す図。 二足歩行実行時の重心位置、反力中心の位置、および、接触点の鉛直方向荷重、を示したグラフ。 二足歩行実行時の重心位置、反力中心の位置、および、接触点の鉛直方向荷重、を示したグラフ。 二足歩行実行時の重心位置、反力中心の位置、および、接触点の鉛直方向荷重、を示したグラフ。 ４足歩行の場合の実験例を示す図。 実行結果のスナップショットである。 ４足歩行実行時の重心位置、反力中心の位置、および、接触点の鉛直方向荷重、を示したグラフ。 ４足歩行実行時の重心位置、反力中心の位置、および、接触点の鉛直方向荷重、を示したグラフ。 ４足歩行実行時の重心位置、反力中心の位置、および、接触点の鉛直方向荷重、を示したグラフ。 ４足歩行で手足の着き方をランダムにした場合の実験例を示す図。 実行結果のスナップショットである。 ランダム４足歩行実行時の重心位置、反力中心の位置、および、接触点の鉛直方向荷重、を示したグラフ。 ランダム４足歩行実行時の重心位置、反力中心の位置、および、接触点の鉛直方向荷重、を示したグラフ。 ランダム４足歩行実行時の重心位置、反力中心の位置、および、接触点の鉛直方向荷重、を示したグラフ。 変形例を示す図。
　本発明の実施形態を図示するとともに図中の各要素に付した符号を参照して説明する。
　（第１実施形態）
　本実施形態は移動ロボットの移動制御方法に特徴があり、具体的には、移動ロボットの移動動作を制御するための軌道生成に特徴を有するのであるが、具体的な制御（軌道生成）を説明する前に、制御対象となる移動ロボットのハードウェア構成について予め説明しておく。
　図２は、移動ロボットの機械構成の一例を示した図である。
　移動ロボット１００は、股関節が３軸、膝関節が１軸、足首関節が２軸。さらに、肩関節が３軸（肩ピッチ、肩ロール、肩ヨー）、肘関節が１軸（肘ピッチ）、および、手首関節が３軸（手首ヨー、手首ピッチ、手首ロール）、で夫々構成されている。（移動ロボットの機械構成はこれに限定されないが、手（腕）の自由度は６以上、足（脚）の自由度も６以上は必要である。）
　移動ロボット１００は、各関節にエンコーダ付きモータ１、２、・・・、２８を有している。各関節のモータ（関節駆動手段）１ａ、２ａ、・・・、２８ａ（図３）は、各関節の関節角度θ１、θ２、・・・、θ２８を調整できる。一方、各関節のエンコーダ（関節角度検出手段）１ｂ、２ｂ、・・・、２８ｂは、各関節の関節角度θ１、θ２・・・、θ２８を計測することができる。
　また、移動ロボット１００は、足先部（足平部）および手先部（手の平部）に接触力センサ２５を有している。ここで接触力とは６軸力であり、図４に示すように、ｘ軸、ｙ軸およびｚ軸方向の力ｆの組（ｆｘ、ｆｙ、ｆｚ）Ｔと、ｘ軸回り、ｙ軸回りおよびｚ軸回りの力τの組（τｘ、τｙ、τｚ）Ｔと、である。
　この移動ロボットは、移動時に、右足、左足、右手および左手のうちの一つ以上を床、壁、あるいはテーブルなどに接触させながら移動する。そこで、本明細書の以下の説明では、右足、左足、右手および左手を接触点候補と称することがある。
　また、手先、足先というのは、すなわち、移動手段のことである。
　図３は、移動ロボット１００の機能ブロック図である。移動ロボット１００は、各関節のモータ１ａ～２８ａ及びエンコーダ１ｂ～２８ｂと、接触力センサ２５と、コントローラ２１０と、を備えている。
　コントローラ２１０には、各関節のエンコーダ１ｂ～２８ｂ及び接触力センサ２５から、センサ検出値が入力される。また、コントローラ２１０は、各関節のモータ１ａ～２８ａに対して駆動信号を出力する。
　コントローラ２１０は、主要なハードウェア構成として、制御処理、演算処理等を行うＣＰＵ（Central Processing Unit）２１０ａと、ＣＰＵ２１０ａによって実行される制御プログラム、演算プログラム等が記憶されたＲＯＭ（Read Only Memory）２１０ｂと、処理データ等を一時的に記憶するＲＡＭ（Random Access Memory）２１０ｃと、を有するマイクロコンピュータにより構成されている。また、これらＣＰＵ２１０ａ、ＲＯＭ２１０ｂ、及びＲＡＭ２１０ｃは、データバス２１０ｄによって相互に接続されている。必要なプログラムを不揮発性記録媒体に記録しておき、必要に応じてインストールするようにしてもよい。
　コントローラ２１０の機能ブロック図を示すと図５のようになる。
　コントローラ２１０は、ユーザから指令される接触点計画を格納する接触点計画設定部２２１と、接触点計画に従った動作を安定に実行できる軌道を生成する軌道生成部２２２と、生成された軌道に従って移動ロボット１００の全身動作を実行させる動作制御部２２３と、を有する。これら機能部の具体的な処理動作については後述する。
　移動ロボットに多点接触移動を安定して行わせるためには、時々刻々と移り変わっていく接触点に応じて接触力を滑らかに適切に分配し、なおかつ、安定な重心軌道を生成する技術が必要である。このために本発明者はモデル予測制御を用いることを試みた。最初にモデル予測制御の概要を説明しておく。
　（概要説明）
　例えば図１に図示したような移動動作をロボットに行わせたいとする。ここでは、２本の腕と２本の脚とを有する人型ロボットに、テーブルの奥側にあるボトルを掴ませるという一連の動作を想定する。この場合、ユーザは、この一連動作（タスク）を実行できるような接触点計画を作成する。つまり、図６のように、手先および足先を、どの順番で、どこに、どのように、着くか、という計画を作成する。図６においては、床、壁およびテーブルにおいて足先および手先を接触させる箇所にマークを付けている。
　この接触点計画は、具体的には図７のようになる。
　接触点計画は、左手（ＬＨ）、右手（ＲＨ）、左足（ＬＦ）および右足（ＲＦ）に関し、どの順番で、どこに、どのように、着いていくか、という時系列のデータである。
　図１、図６および図７の対応関係を簡単に説明する。
　当初（ｔ０）左足１本だけで立ち、遊脚である右足を前に振り出し、そして、右足を着地させる（ｔ１）。この動きの計画を移動ロボットに指令するには、左足が最初に着地している床上の接触点の座標ＰＬＦ１、そのときの左足の姿勢ｒＬＦ１、そして、右足が着地する床上の接触点の座標ＰＲＦ１、そのときの右足の姿勢ｒＲＦ１、を指定することが必要である。ここで、接触点の座標は、空間座標としてＰ＝（Ｐｘ、Ｐｙ、Ｐｚ）の組で表わされる。また、姿勢というのは、接触点に着地したときの足の裏面の向きであり、例えばオイラー角の組としてｒ＝（ｒｘ、ｒｙ、ｒｚ）として表わされる。（すなわち、ｒｘ、ｒｙおよびｒｚは、ロール、ピッチおよびヨー角をそれぞれ表わす。）足に関する接触点の座標およびそのときの姿勢を指令する形式は今後の説明でも同様なので、以後は適宜説明を省略する。
　両足で立った後、左足を振り出し（ｔ２）、左足を前方に着地する（ｔ４）。その間に、左手を壁に着くようにする（ｔ３）。ここで、左手を着く壁上の接触点の座標ＰＬＨ１、および、そのときの左手の姿勢ｒＬＨ１を指定する。この接触点の座標は空間座標としてＰ＝（Ｐｘ、Ｐｙ、Ｐｚ）の組で表わされ、姿勢は接触点に着いたときの手の平の向きとしてオイラー角の組としてｒ＝（ｒｘ、ｒｙ、ｒｚ）として表わされる。
　これ以降の接触点計画は図１、図６および図７を対比して頂ければ自明と思われるので省略する。このようにしてユーザにより接触点計画が時系列のデータとして作成される。
　このように作成された接触点計画が移動ロボットに入力されると、移動ロボットはこの接触点計画を実現するように軌道を生成して自律的に移動する。このとき、軌道生成にあたって、移動ロボットはモデル予測制御を行う。すなわち、ある時間幅を持った予測区間内で移動ロボットが安定移動できる軌道を生成し、予測区間を微小時間（Δｔ）ずつシフトさせながら安定動作を行える軌道を順次更新していくようにする。例えば、図８に予測区間の例を示す。現在から所定時間（例えば１．６秒）先の未来までを予測区間として設定する。
　そして、この予測区間の間で発散しないように安定な軌道を生成する。この予測区間での動きをイメージしたものが図９である。このように、ある時間幅を持つ予測区間で安定な軌道を生成した上で、最初の一点だけを現在の入力値として使用する。
　次の軌道更新周期（Δｔ秒後）には予測区間をシフトさせ、新たな予測区間において同様に安定な軌道を生成する（図１０参照）。
　現在だけ、あるいは、現在から次ぎの制御周期（Δｔ秒）まで、だけを見るのではなく、上記のように、ある程度の未来までを予測区間とし、この予測区間内で発散しない軌道を生成するようにする。これを繰り返すことで移動ロボットは安定に移動することができる。
　さて、ここで問題なのは、ある時間幅を持った予測区間のなかで時々刻々と移り変わっていく接触点に応じて接触力を滑らかに適切に分配し、なおかつ、安定な重心軌道を生成するにはどのようにすればよいか、ということである。
　本発明者らは、ある予測区間における安定軌道の生成問題を凸二次計画問題（Quadratic Programming:QP）に帰着させるという着想を得た。具体的には、各接触点における接触力の二乗和と、前記６軸力の微分値の二乗和と、を含む評価関数Ｊを最小化するという問題を解くことで、多点接触移動の安定軌道を求める。
　そこで、次に、この評価関数Ｊの導出およびその解法（凸二次計画問題への帰着）を説明する。この解法により、ある予測区間内で安定な多点接触移動を実現するための、重心位置、重心速度、接触力および接触力の微分値の時系列データが得られることを示す。
　予測に用いる移動ロボットのモデルを改めて図１１に示す。移動ロボット全体の慣性を一つの重心Ｇで表わす。各接触点には６軸力を定義する。
　この時、重心Ｇの並進運動量をＰ、重心回りの回転運動量（角運動量）をＬ、接触点の数をｎとすると、運動方程式は次のように書ける。
Figure JPOXMLDOC01-appb-M000001
　添え字ｉは接触点のインデックスを表す。例えば接触点の候補が左手、右手、左足、右足の４点であれば、ｎ＝４（左手：ＬＨ＝１、右手：ＲＨ＝２、左足：ＬＦ＝３、右足：ＲＦ＝４）とすればよい。ただし、床や壁に接触していない接触点候補については接触力を０にするように拘束条件を設定しておく。例えば図１１の例であれば次のようにする。
Figure JPOXMLDOC01-appb-M000002
　（１）式の第１式、第２式を微分すると次の式が得られる。（（１）式はベクトルで表現しているが、これをｘ、ｙ、ｚに分解した上で、上から順に第１式、第２式・・・第６式と称する。）
Figure JPOXMLDOC01-appb-M000003
　本実施形態では、この２式をシステムとして用いる。そして、（１）式の第３から第５式を拘束条件として定式化する。
Figure JPOXMLDOC01-appb-M000004
　さらに、予測区間内を図１２のように、Ｎ個の区間に分割し、（３）式、（４）式を離散化する。（３）式を離散化すると次のようになる。
Figure JPOXMLDOC01-appb-M000005
　また、サンプリング点で常に（４）式の拘束が成り立つとすると、（４）式は次のように離散化される。
Figure JPOXMLDOC01-appb-M000006
　ここで、パラメータを次ぎのように置く。
Figure JPOXMLDOC01-appb-M000007
　θｉは、６軸力としての接触力を並べたベクトルである。そして、ｘは、重心Ｇのｘ座標、重心Ｇのｘ軸方向速度、重心Ｇのｙ座標、重心Ｇのｙ軸方向速度、および、各接触点における接触力（６軸力）、を並べたベクトルである。このｘを、状態変数ｘと称する。さらに、ｕは、接触力（６軸力）の微分値を並べたベクトルである。
　このようにパラメータを設定すると、（５）式を次ぎのように記述することができる。
Figure JPOXMLDOC01-appb-M000008
　この（８）式は、（ｊ＋１）のときの状態変数ｘを、その一つ前の状態で記述できることを表わしている。（８）式を用いて予測区間内の状態変数ｘを順に計算していくと次のようになる。
Figure JPOXMLDOC01-appb-M000009
　したがって、時系列的に求められる状態変数ｘを並べて大文字のＸで表わすと、状態変数の時系列データＸを次のように表わすことができる。
Figure JPOXMLDOC01-appb-M000010
　この（１０）式は、接触力の微分値（U［ｋ］）を入力として、ある予測区間内における移動ロボットの状態遷移を表わす予測モデルとなる。
　さて、ここで、本発明者らは、予測区間内において安定な軌道を生成するために次ぎのような評価関数Ｊを導入した。
Figure JPOXMLDOC01-appb-M000011
　なお、Ｑｉ、Ｒｉは、適宜設定した重みである。重みの意味や接触点候補にどのように重みを配分するかなどの考え方については後ほど詳しく説明する。ここでは、例えば、接触点候補すべてに力を均等配分すると考えて、Ｑｉはすべて１とし、Ｒｉはすべて１×１０－６とするようなことをイメージして頂ければよい。
　ここで、θｉは、６軸力としての接触力の成分を並べたベクトルであった。θｉ（・）は、接触力成分の微分値を並べたベクトルである。（後ろにカッコで付けたドットは微分を表わすと解釈されたい。）
　したがって、（１１）式は、「予測区間内で、接触力（６軸力）と接触力（６軸力）微分値との２乗和を最小化する」という意味の式である。（１１）式の第１項は、接触力（６軸力）の２乗和を最小化することを意味する。
　この第１項には、次の作用が含まれている。
（１）各接触点への接触力を均等分配すること。これにより、重心をできる限り安定な位置に動かすという効果がある。
（２）不必要な内力を打ち消すこと。
（３）接触点の接地安定性を高めること。すなわち、接触面内の反力中心点を接触面の中心に設定するという効果がある。
　また、（１１）式の第２項は、接触力（６軸力）微分値（６軸力の時間変化率）の２乗和を最小化することを意味する。
　この第２項には次の作用が含まれている。
（１）重心の発散を抑制すること。
（２）滑らかに接触力を切り替えていくこと。
　これらをＱ、Ｒという重みによって適切に足し合わせることによって、この評価関数Ｊを最小化するということは、
　「高い接触安定性、滑らかな接触力遷移、最低限の内力、といった条件を満たしながら、安定な重心軌道と各接触点の接触力とを出力する」
　ということを意味することとなる。
　（１１）式を離散化すると次の式が得られる。
Figure JPOXMLDOC01-appb-M000012
　ここで、接触力（６軸力）については状態変数ｘに含まれているので、（７）式を変形すると次のようになる。
Figure JPOXMLDOC01-appb-M000013
　したがって、（１２）式の一つめのシグマの計算は次ぎのようになる。
Figure JPOXMLDOC01-appb-M000014
　したがって、（１２）式は次のようになる。
Figure JPOXMLDOC01-appb-M000015
　さらに、（１０）式を用いることで評価関数Ｊを次のように定式化することができる。
Figure JPOXMLDOC01-appb-M000016
　次に拘束条件について考える。
　拘束条件としては、
　（２）式のように非接触の接触点候補に対して６軸力が０という拘束、
　（６）式の鉛直方向の力の釣り合いの拘束、および、
　（６）式のｘｙ軸回りのモーメントの釣り合いの拘束、
　が予測区間の全サンプリング点に渡って成り立つ必要がある。
　ここで、例えば、あるサンプリング点ｊにおいて、ｉ番目とｉ＋２番目の接触点が非接触であったとする。この時、（２）式および（６）式は次のように記述できる。
Figure JPOXMLDOC01-appb-M000017
　ここで、係数行列Ｃ［ｊ］、ｄ［ｊ］の成分はサンプリング点によって異なるが、接触点候補の接触／非接触といった情報や接触点位置は既に与えられている。
　重心鉛直軌道（Ｇｚ［ｊ］）や重心回りモーメント（Ｌｘ（・）［ｊ］、Ｌｙ（・）［ｊ］）については詳しくは後述するが、ここでは、差し当って次ぎのように理解されたい。
　もしも、重心鉛直軌道や重心回りモーメントが定まっていない場合には仮の値を設定して利用する。例えば、一つ前の制御サイクルで用いた重心鉛直軌道、重心回りモーメントを仮の値として利用してもよい。もしくは、重心鉛直軌道、重心回りモーメントは予測区間内においては現時点の状態から動かない、と仮設定してもよい。そして、仮の値で一度計算したあとで、得られた結果を用いて重心鉛直軌道、重心回りモーメントを更新し、求解を繰り返す。
　このようにして、重心鉛直軌道や重心回りモーメントについても既知の値として計算を進行させ、ずれについては更新計算のなかで修正するとする。
　さて、（１７）式が未来の全サンプリング点で成り立つためには、次式が成り立つ必要がある。
Figure JPOXMLDOC01-appb-M000018
　この式に（１０）式を代入する。
Figure JPOXMLDOC01-appb-M000019
　最後に、接触点が安定して接触を保つ為の条件を導入する。
　図１３に接触点の座標系（上添え字ｌ（エル）がついている）と、接触多角形（接触点の支持多角形）と、を示した。接触点の座標系は、接触点を原点とし、かつ、接触面の姿勢ｒｉに合わせて定義されているとする。
　ここで、接触点の座標系で定義される接触力（６軸力）θｉ ｌを次のように表わす。
　　θｉ ｌ＝［ｆｉｘ ｌ、ｆｉｙ ｌ、ｆｉｚ ｌ、τｉｘ ｌ、τｉｙ ｌ、τｉｚ ｌ］Ｔ
　すると、接触力（６軸力）θｉ ｌは、接触面の姿勢行列Φｉ＝ｒｏｔ（ｒｉ）を用いて次ぎのように表現できる。なお、ｒｏｔは、オイラー角を姿勢行列に変換する関数である。
Figure JPOXMLDOC01-appb-M000020
　接触点が安定して接触を保つ為には、
（１）接触点が離れないこと、
（２）接触点が滑らないこと、
（３）接触点が剥がれないこと、
　という３つの条件を満たす必要がある。
　上記３つの要件が理解しやすいように、図１４に、接触点が不安定化する場合を例示した。
（１）接触点が離れない為には、接触面の鉛直力が正であれば良い。
　即ち、次式を満たす必要がある。
Figure JPOXMLDOC01-appb-M000021
（２）接触点が滑らない為には、接触面に平行な２軸力が摩擦力以下であれば良い。
　即ち次式がその条件である。
　ただし接触面の摩擦係数をμｉとする。
Figure JPOXMLDOC01-appb-M000022
（３）接触点が剥がれない為の条件は、接触多角形のｈ個の頂点座標
　（ｘｉ１ ｌ， ｙｉ１ ｌ），・・・・・（ｘｉｈ ｌ， ｙｉｈ ｌ）
を用いて以下のように表される。
　（ただし接触多角形の頂点は反時計回りに順に与えられているとする）。
Figure JPOXMLDOC01-appb-M000023
　以上、（２１）、（２２）、（２３）式をまとめると次のようになる。
Figure JPOXMLDOC01-appb-M000024
　（２４）式に（２０）式を代入し、ｊ番目のサンプリング点としてインデックスを付け加える。
Figure JPOXMLDOC01-appb-M000025
　（２５）式は、ｊ番目のサンプリング点におけるｉ番目の接触点が安定な接触を保つ為に満たすべき条件であり、安定な多点接触動作を実現する為には、全サンプリング点の全接触点において（２５）式が成り立つ必要がある。
　まずｊ番目のサンプリング点において全接触点が（２５）式を満たす為の条件は次のように書ける。
Figure JPOXMLDOC01-appb-M000026
　これが全サンプリング点で成り立つ条件は次のように書ける。
Figure JPOXMLDOC01-appb-M000027
　これに（１０）式を代入する。
Figure JPOXMLDOC01-appb-M000028
　最終的に、（１６）、（１９）、（２８）式により、多点接触時の安定軌道生成問題は次の凸２次計画問題に帰着される。
Figure JPOXMLDOC01-appb-M000029
　凸２次計画問題（Quadratic Programming: QP）にまで帰着できてしまえば、凸２次計画問題（ＱＰ）自体はよく知られた最適化問題の１つであり、高速な求解が可能である。（求解するとＵ［ｋ］が求まる。）ＱＰの求解後は、重心軌道と各接触点の接触力とが次ぎのようにして計算される。
Figure JPOXMLDOC01-appb-M000030
　このようにして、予測区間内において常に拘束条件（１）－（３）を満たし、かつ、評価関数Ｊを最小化するＸ［ｋ＋１］が求まる。
　Ｘ［ｋ＋１］というのは、（７）式と（１０）式とを見てわかる通り、重心のｘ座標、重心のｘ軸方向速度、重心のｙ座標、重心のｙ軸速度、および、各接触点の接触力（６軸力）の時系列データである。
　さらに、式の展開の途中で説明したように、重心の鉛直軌道（ｚ座標）や重心回りモーメントについては更新計算のなかで修正されたものが得られている（詳しくは後述する）。すなわち、これで予測区間内で安定な移動を実現する軌道を得ることができたわけである。
　用語が混乱しないように付言しておく。
　上記の演算過程およびこの演算で求められる結果を見て分かるように、ここでいう「軌道」というのは、重心の三次元的軌道に加えて、各接触点で発生すべき接触力（６軸力）の時系列データを含めて考えると分かりやすいであろう。なお、移動ロボットを移動させるための「軌道」といえば一般的には手先、足先の軌道も含む概念であろうが、本実施形態の説明において「軌道」といった場合には、手先、足先の軌道を含んでいてもいなくても大差無い。本実施形態における安定軌道生成という観点から見ると、手先、足先の軌道は、接触点計画、重心軌道および接触力の時系列データから補間によってほぼ自動的に決定され得るものである。したがって、本実施形態の説明において「軌道」といった場合には、手先、足先の軌道を含んでいてもいなくても大差無い。
　（移動ロボットの制御）
　移動ロボットの移動制御方法を説明する。
　図１５は、移動ロボットの移動制御方法の全体フローである。
　移動ロボットの移動制御の工程には、大きく分けて、接触点計画の設定（ＳＴ１００）と、軌道生成（ＳＴ２００）と、動作指令（ＳＴ３００）と、動作制御の実行（ＳＴ４００）と、がある。
　接触点計画の設定（ＳＴ１００）においては、これまで概略説明してきたように、手先および足先を、どの順番で、どこに、どのように、着くか、という多点接触移動の計画（図７）をユーザが移動ロボットに入力する。入力された接触点計画は、コントローラ２１０の接触点計画設定部２２１に記憶される。
　軌道生成工程（ＳＴ２００）について説明する。
　図１６、図１７、図１８に軌道生成工程（ＳＴ２００）の手順を説明するための詳細なフローチャートを示す。軌道生成工程（ＳＴ２００）はコントローラ２１０の軌道生成部２２２にて実行される。
　まず、ＳＴ２０１において、軌道生成部２２２は現在状態ｘ［ｋ］を設定する。ここで、現在状態（ｘ［ｋ］）として設定すべきは、現在の重心のｘ座標、重心のｘ軸方向速度、重心のｙ座標、重心のｙ軸速度、および、各接触点の接触力（６軸力）である。各接触点における接触力は力センサ２５からの検出値によって得ることができる。重心のｘ座標、重心のｘ軸方向速度、重心のｙ座標および重心のｙ軸速度は、現在の移動ロボットの状態から推定して求める値である。
　例えば、各エンコーダのセンサ値を取得して移動ロボットの全身姿勢を把握した上で、重心のｘ座標、重心のｘ軸方向速度、重心のｙ座標および重心のｙ軸速度を求めてもよい。あるいは、想定している姿勢に対して実際の移動ロボットの姿勢がどの程度傾いているか（ずれているか）を把握することによって、重心位置を修正するようにしてもよい。
　次に、ＳＴ２０２において、重み（Ｑｉ、Ｒｉ）を設定する。
　（先の式の展開でいうと、重みは、（１１）式のところで評価関数Ｊに組み込まれたことを確認されたい。）
　重み（Ｑｉ、Ｒｉ）は、ユーザが予め設定入力しているもので、軌道生成部２２２は、必要に応じて読み出し、更新設定する。（更新する必要がなければ、再設定の必要はない。）接触点候補すべてに力を均等配分する場合には、Ｑｉをすべて１とし、Ｒｉをすべて１×１０－６としてもよいが、もっと細かく設定することもできる。この重みを調整することで、接触力の変化をどれぐらい滑らかにするかや、手先にはあまり力を掛けないようにする、などの調整ができる。
　例えば、重みを全部同じにしたとする。すると、演算結果として、着地している手足（各接触点）に均等に力が掛かるような解が得られる。一般的に考えると、この場合が移動ロボットとしては一番安定した形（姿勢）になる。
　一方、重みの配分を変えたい場合もある。例えば、足に比べて手が弱い（脆弱な）移動ロボットであって手に荷重を掛け過ぎるとよくないという場合には、手の重みを「大きく」する。重みを大きくするほど、重みに乗算されている手のθｉなどが小さくなるので、手には接触力の分配が小さくなる。
　あるいは、テーブルに一方の手を着いて、他方の手を前方に大きく伸ばしたいとする。この場合、手足に均等に力が配分されているだけでは無理であり、体をもっと前に傾ける必要がある。体を傾け、重心をもっと前方にシフトさせるためには、一方の手に掛かる力の分配をやや大きくする必要がある。この場合、一方の手の重みを減らし、体を傾けられるようにし、他方の手をより大きく前方に伸ばすようにする、という調整が必要になる。
　一般的な動作では重みは変更の必要がないが、状況に応じて軌道更新のタイミングで毎回変更してもよい。
　次に、ＳＴ２０３において、予測区間を設定する。
　これは、図８で説明したように、現在から所定の時間幅（例えば１．６秒）を予測区間として設定する、ということである。
　次に、ＳＴ２０４において、この予測区間内の接触点位置およびその姿勢（ｐｉ、ｒｉ）を設定する。これは、接触点計画（図７）のなかから予測区間内の接触点計画を切り出してくることに相当する。
　次に、ＳＴ２０５においては、重心鉛直軌道と重心回りモーメントとを仮設定する。
　本実施形態のポイントは、次ステップの演算工程（ＳＴ２０６）で（２５）式（凸二次計画問題）を解いて、重心のｘｙ方向軌道、各接触点の接触力および接触力微分値、を得ることにある。
　しかし、この求解には、各接触点の位置、姿勢、重心鉛直軌道、および、重心回りモーメントが与えられていることが前提となる。予測区間内における各接触点の位置および姿勢は、接触点計画で与えられる。一方、この予測区間内における重心鉛直軌道および重心回りモーメントは、未知である。
　そこで、演算工程（ＳＴ２０６）で（２５）式（凸二次計画問題）が解けるように重心鉛直軌道および重心回りモーメントを一回仮設定する。
　全くの一回目の制御サイクルでは、例えば、重心鉛直軌道、重心回りモーメントは予測区間内においては現時点の状態から動かない、と仮設定してもよい。（例えば、重心高さは変化しない、と仮定してもよい。）もしくは、一つ前の制御サイクルで用いた重心鉛直軌道、重心回りモーメントを仮の値として利用してもよい。予測区間は１．６秒ほどの微小な時間であり、最終的に求まる安定軌道においてもこの間の重心高さの変化はやはり微小である。したがって、一回目の仮計算としては、重心高さが変化しないとしても、前回の制御周期と同じであるとしても計算は進められる。
　なお、この後、仮の演算で求まった重心のｘｙ方向軌道でも移動ロボットの動きが安定するように重心鉛直軌道および重心回りモーメントを修正する工程（ＳＴ２０９）がある。そして、この修正した重心鉛直軌道および重心回りモーメントを使って再度演算（ＳＴ２０６）するようにループを回すので、最終的には、重心鉛直軌道を含めて安定な軌道が適切に求まることはご理解頂けよう。
　ここで、重心回りモーメントを定義しておく。
　移動ロボットをリンク同士が関節でジョイントされたリンク構造体と見なすと、重心回りモーメントは、各リンクに分布した質点の運動によって重心の回りに発生するモーメントの合計値である。
　移動ロボットが図１９（Ａ）、（Ｂ）に示すような多質点モデルで表されており、Ｍ個の質点がそれぞれ、
　質量：ｍｉ、
　位置：ｓｉ＝［ｘｉ、ｙｉ、ｚｉ］Ｔ
　のパラメータを持っているとする。
　移動ロボット全体の重心をＧ＝（Ｇｘ、Ｇｙ、Ｇｚ）Ｔとすると、重心回りモーメントは次のように表わされる。
Figure JPOXMLDOC01-appb-M000031
　次に、ＳＴ２０６において演算工程を実行する。
　具体的には、（２９）式の凸二次計画問題を解くということである。
　予測区間内において、接触点おのおのに力を適切に分散させつつ、なおかつ、この接触力を実現しながら重心（ここでの重心はｘとｙ）が安定に遷移するような軌道が求められるわけである。具体的には、Ｈ、Ｆ、Ｐ、Ｓ、Ｖ、Ｗが求まり、さらに、Ｕ［ｋ］が求まる。（このＵ［ｋ］は接触力微分値のベクトルであったことを思い出されたい。）そして、（３０）式によって、Ｘｏｕｔ［ｋ＋１］が求まる。（このＸｏｕｔ［ｋ＋１］は、重心のｘ座標、重心のｘ軸方向速度、重心のｙ座標、重心のｙ軸速度、および、各接触点の接触力（６軸力）の時系列データである。）
　続いて、ＳＴ２０８において、手先、足先の軌道を補間して求める。接触点計画においては、接触点とそのときの姿勢だけが設定されている。そこで、各接触点間を結ぶように手先、足先の補間軌道を求める。すると、図２０のように、接触点計画で指示された接触点同士の間が埋まる。
　なお。ＳＴ２０８は、次のＳＴ２０９の前であればどのタイミングで実行されてもよいものである。極端に言えば、接触点計画が入力された時点ですぐに手先足先の補間軌道を求めてしまってもよい。
　さて、次に、ＳＴ２０９において、重心鉛直軌道と重心回りモーメントとを修正する作業を行う。
　まず、重心鉛直軌道を修正する。これまでの工程により、重心のｘｙ方向の軌道と、手先足先の軌道と、が得られている。（さらに、重心鉛直軌道も仮設定している。）これに基づいて、予測区間内における移動ロボットの姿勢の変化を推定することができるが、このとき、膝や腕が伸びきってしまうなど安定しない要因がある場合には安定するように重心鉛直軌道を調整する。例えば、膝が伸びきりそうになったら重心鉛直軌道を下げる、といった調整を行う。予測区間内における移動ロボットの姿勢が決定したら、それに基づいて重心回りモーメントの計算を行う。
　さて、次に、ＳＴ２１０において、ＳＴ２０６－ＳＴ２０９を１回以上繰り返したかどうかを判断し、繰り返していなければＳＴ２０６に戻って凸二次計画問題の求解を行う。
　（ＳＴ２０８については必要に応じてスキップしてもよいことはもちろんである。）
　この繰り返し演算（ＳＴ２０６）によって、修正された重心鉛直軌道および重心回りモーメントを用いて再度Ｘｏｕｔ［ｋ＋１］が求まる。
　このようにして、予測区間内において移動ロボットの安定な移動を実現する、重心位置、重心速度および接触力の時系列データが求まった（ＳＴ２１１）。
　このように、ある時間幅を持つ予測区間で安定な軌道を生成した上で、最初の一点だけを現在の入力値として使用する。すなわち、最初の一点だけを動作指令として動作制御部２２３に出力する（ＳＴ３００）。
　所定の終了条件を満たすまで周期を更新（ＳＴ２１３）すなわち現時点を微小時間進ませて、先頭（ＳＴ２０１）に戻り、ループを繰り返す。このように、予測区間を微小時間だけ先に進ませながら軌道生成を繰り返す。このようにして多点接触移動を安定に実現する軌道の算出が行われる。
　動作指令を受けた動作制御部２２３が移動ロボット１００をどのように制御して動かすか（関節を駆動させるか）は本実施形態のポイントではないが、簡単に説明しておく。
　本実施形態においては、重心および手先足先の軌道に加えて、各接触点における接触力も動作指令として与えられる。そこで、まず、各接触点で指示された接触力が発生するように手先足先の位置（軌道）を微修正するという工程が必要になる（ＳＴ４１０）。例えば、手先をテーブルに着いているときに、比較的大きな接触力が指示された場合には、手先をテーブルに押し込むように手先の軌道を微修正する。
　そして、各関節のモータを駆動して全身を動作させる（ＳＴ４２０）。これで移動ロボットが動作することになる。
　なお、動作の実行（ＳＴ４００）において、接触力制御（ＳＴ４１０）を行ったり、関節が指示した通りに正確に駆動しなかったり、外乱の影響を受けたりして、実機とモデルとの間には当然にずれが生じる。したがって、軌道生成（ＳＴ２００）のループにあっては、毎回、現在状態ｘの設定を行う必要がある（ＳＴ２０１）。
　以上、本実施形態によれば、設定された接触点の計画に基づいて移動ロボットが自動的に安定な重心軌道を生成して自律的に移動できる。そして、軌道生成のために用いた評価関数Jの意味から分かるように、接触点の切替（接触状態の遷移）に伴う力の変化が滑らかになり、移動ロボットの安定かつ滑らかな移動が実現される。
　（実験例）
　実験例を説明する。
　上記実施形態を２足歩行と４足歩行の場合に適用した実験例を示す。
　図２１から図２３は二足歩行の場合の例である。図２１のような接触点計画を移動ロボットに与えた。（接触点は右足と左足だけであり、右手、左手は床や壁に接触しないものとした。）
　図２２は、実行結果のスナップショットである。
　図が多くなりすぎても冗長であるので、途中および最後を間引いたが、接触点計画と付き合わせていただければ内容はご理解頂けるであろう。
　安定した２足歩行が実現されており、そして、接触点の移り変わりに従って安定な重心軌道が生成されていることがわかる。
　図２３（A）、（B）、（C）は、二足歩行実行時の重心位置、反力中心の位置、および、接触点の鉛直方向荷重、を示したグラフである。接触点の切り替えに伴う接触点反力が滑らかに遷移していることがわかる。
　全反力中心が左右脚間で滑らかに遷移していることも確認できる。
　同様に、図２４から図２６は４足歩行の場合である。
　図２４のような接触点計画を移動ロボットに与えた。
　図２５が、実行結果のスナップショットである。（前記と同様に途中を間引いた。）
　やはり、安定した４足歩行が実現されており、接触点の移り変わりに従って安定な重心軌道が生成されていることがわかる。
　図２６（A）、（B）、（C）は、４足歩行実行時の重心位置、反力中心の位置、および、接触点の鉛直方向荷重、を示したグラフである。
　同様に、図２７から図２９は４足歩行であって、手足の着き方をランダムにしたものである。
　図２７のような接触点計画を移動ロボットに与えた。
　図２８が、実行結果のスナップショットである。
　やはり、ランダム４足歩行でも安定した歩行が実現されており、接触点の移り変わりに従って安定な重心軌道が生成されていることがわかる。
　図２９（A）、（B）、（C）は、ランダム４足歩行実行時の重心位置、反力中心の位置、および、接触点の鉛直方向荷重、を示したグラフである。
　なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。
　移動ロボットとしてのハードウェア構成としては、二足歩行、４足歩行、さらには、６足でも８足でもよいのであって、手足の数に制限があるわけではない。
　ただし、少なくとも一つの接触点がどこかに接地している必要がある。
　（接地、というが、壁に手を着く場合も含めて解釈されたい。）
　極端なケースであるが、例えば、図３０のように、足が１本、手が１本で、床と壁に手足を交互に着けながら移動するような移動ロボット１００であってもよい。
　この出願は、２０１３年１２月１０日に出願された日本出願特願２０１３－２５４９８９を基礎とする優先権を主張し、その開示の全てをここに取り込む。
１ａ-２８ａ…モータ、１ｂ-２８ｂ…エンコーダ、２５…接触力センサ、１００…移動ロボット、２１０…コントローラ、２２１…接触点計画設定部、２２２…軌道生成部、２２３…動作制御部。

Claims (5)

  1. 　二以上の移動手段を交互に接地しながら移動する移動ロボットの移動制御方法であって、
    　前記移動手段を接地する接触点の位置と接地するときの前記移動手段の姿勢とを時系列のデータとした接触点計画を設定する接触点計画設定工程と、
    　前記接触点計画設定工程で設定したように前記移動手段を接触点に接地しながら移動するための軌道を生成する軌道生成工程と、を備え、
    　前記軌道生成工程は、
    　前記移動手段を接地するときの接触力の微分値を入力とする予測モデルを構築して、この予測モデルによって所定時間幅の予測区間における移動ロボットの状態変化を表わし、
    　前記予測区間において、所定の評価基準を用いて接触力の微分値の時系列データを求め、
    　この求まった接触力の微分値の時系列データと前記予測モデルとから、移動ロボットの重心軌道を得るものであり、
    　　前記評価基準には、
    　　各接触点に対応して設定された重みに基づいて各接触点に接触力と、接触力の微分値とを配分するという基準が含まれ、
    　前記評価基準は、接触力および接触力の微分値の二乗和を含む評価関数Jを予測区間内において最小化することである
    　ことを特徴とする移動ロボットの移動制御方法。
  2. 　請求項１に記載の移動ロボットの移動制御方法において、
    　前記評価関数Jの最小化問題を拘束条件のもとで求解するにあたり、
    　前記拘束条件の式には、移動ロボットの重心鉛直軌道と重心回りモーメント軌道とを含んでおり、
    　前記軌道生成工程は、
    　予測区間内において、移動ロボットの重心鉛直軌道と重心回りモーメント軌道とを一度仮設定する工程と、
    　前記評価関数Jの最小化問題を拘束条件のもとで一度求解した後、その結果に応じて、予測区間内において移動ロボットの動作が安定するように重心鉛直軌道と重心回りモーメント軌道とを修正し、
    　修正した重心鉛直軌道と重心回りモーメント軌道とを用いて少なくとも一回以上、前記評価関数Jの最小化問題を再計算する
    　ことを特徴とする移動ロボットの移動制御方法。
  3. 　請求項１または請求項２に記載の移動ロボットの移動制御方法において、
    　前記移動手段ごとに重みが設定されており、
    　前記評価関数Jにおいて、接触力および接触力の微分値にそれぞれ重みを乗算しておく
    　　ことを特徴とする移動ロボットの移動制御方法。
  4. 　請求項１から請求項３のいずれかに記載の移動ロボットの移動制御方法において、
    　前記軌道生成工程において予測区間内で軌道を求め、
    　この予測区間の最初の一点のデータを現在の入力値として移動ロボットを駆動させ、
    　予測区間を微小時間だけ進めて、前記軌道生成工程を繰り返す
    　ことを特徴とする移動ロボットの移動制御方法。
  5. 　二以上の移動手段を交互に接地しながら移動する移動ロボットであって、
    　前記移動手段を接地する接触点の位置と接地するときの前記移動手段の姿勢とを時系列のデータとした接触点計画を設定する接触点計画設定部と、
    　前記接触点計画設定部で設定したように前記移動手段を接触点に接地しながら移動するための軌道を生成する軌道生成部と、を備え、
    　前記軌道生成部は、
    　前記移動手段を接地するときの接触力の微分値を入力とする予測モデルを構築して、この予測モデルによって所定時間幅の予測区間における移動ロボットの状態変化を表わし、
    　前記予測区間において、所定の評価基準を用いて接触力の微分値の時系列データを求め、
    　この求まった接触力の微分値の時系列データと前記予測モデルとから、移動ロボットの重心軌道を得るものであり、
    　　前記評価基準には、
    　　各接触点に対応して設定された重みに基づいて各接触点に接触力と、接触力の微分値とを配分するという基準が含まれ、
    　前記評価基準は、接触力および接触力の微分値の二乗和を含む評価関数Jを予測区間内において最小化することである
    　ことを特徴とする移動ロボット。
PCT/JP2014/005905 2013-12-10 2014-11-26 移動ロボットの移動制御方法、および、移動ロボット WO2015087504A1 (ja)

Priority Applications (1)

Application Number Priority Date Filing Date Title
JP2015552305A JP6168158B2 (ja) 2013-12-10 2014-11-26 移動ロボットの移動制御方法、および、移動ロボット

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
JP2013-254989 2013-12-10
JP2013254989 2013-12-10

Publications (1)

Publication Number Publication Date
WO2015087504A1 true WO2015087504A1 (ja) 2015-06-18

Family

ID=53370835

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/JP2014/005905 WO2015087504A1 (ja) 2013-12-10 2014-11-26 移動ロボットの移動制御方法、および、移動ロボット

Country Status (2)

Country Link
JP (1) JP6168158B2 (ja)
WO (1) WO2015087504A1 (ja)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9327399B2 (en) 2014-02-19 2016-05-03 Toyota Jidosha Kabushiki Kaisha Movement control method for mobile robot
US10633045B2 (en) 2017-03-29 2020-04-28 Honda Motor Co., Ltd. Robot and control device of the robot
EP4043991A4 (en) * 2020-04-14 2023-02-08 Tencent Technology (Shenzhen) Company Limited METHOD AND APPARATUS FOR DETERMINING A CENTER OF GRAVITY, LEGGED ROBOT AND STORAGE MEDIA

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2010137289A (ja) * 2008-12-09 2010-06-24 Sony Corp 情報処理装置及び情報処理方法、並びにコンピューター・プログラム
JP2011177839A (ja) * 2010-03-01 2011-09-15 Honda Motor Co Ltd 脚式移動ロボットの運動状態評価装置
JP2013107191A (ja) * 2011-11-22 2013-06-06 Disney Enterprises Inc 力制御関節を有する有脚ロボットのための運動学的および動力学的キャリブレーション方法
JP2013116529A (ja) * 2011-12-02 2013-06-13 Honda Motor Co Ltd 脚式移動ロボットの歩容生成装置及びロボットの動作目標生成装置
JP2013184232A (ja) * 2012-03-06 2013-09-19 Toyota Motor Corp 脚式歩行ロボットおよびその重心軌道生成方法
JP2013184248A (ja) * 2012-03-07 2013-09-19 Toyota Motor Corp 重心軌道生成装置、その生成方法及びプログラム

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2010137289A (ja) * 2008-12-09 2010-06-24 Sony Corp 情報処理装置及び情報処理方法、並びにコンピューター・プログラム
JP2011177839A (ja) * 2010-03-01 2011-09-15 Honda Motor Co Ltd 脚式移動ロボットの運動状態評価装置
JP2013107191A (ja) * 2011-11-22 2013-06-06 Disney Enterprises Inc 力制御関節を有する有脚ロボットのための運動学的および動力学的キャリブレーション方法
JP2013116529A (ja) * 2011-12-02 2013-06-13 Honda Motor Co Ltd 脚式移動ロボットの歩容生成装置及びロボットの動作目標生成装置
JP2013184232A (ja) * 2012-03-06 2013-09-19 Toyota Motor Corp 脚式歩行ロボットおよびその重心軌道生成方法
JP2013184248A (ja) * 2012-03-07 2013-09-19 Toyota Motor Corp 重心軌道生成装置、その生成方法及びプログラム

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
HYON, S.; ET AL.: "Gravity Compensation and Full-Body Balancing for Humanoid Robots", HUMANOID ROBOTS, 2006 6TH IEEE -RAS INTERNATIONAL CONFERENCE, pages 214 - 221 *

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9327399B2 (en) 2014-02-19 2016-05-03 Toyota Jidosha Kabushiki Kaisha Movement control method for mobile robot
US10633045B2 (en) 2017-03-29 2020-04-28 Honda Motor Co., Ltd. Robot and control device of the robot
EP4043991A4 (en) * 2020-04-14 2023-02-08 Tencent Technology (Shenzhen) Company Limited METHOD AND APPARATUS FOR DETERMINING A CENTER OF GRAVITY, LEGGED ROBOT AND STORAGE MEDIA

Also Published As

Publication number Publication date
JPWO2015087504A1 (ja) 2017-03-16
JP6168158B2 (ja) 2017-07-26

Similar Documents

Publication Publication Date Title
JP5910647B2 (ja) 移動ロボットの移動制御方法
Feng et al. 3D walking based on online optimization
US7053577B2 (en) Robot and motion control method of robot
Feng et al. Optimization‐based full body control for the darpa robotics challenge
JP3972854B2 (ja) ロボットの運動制御装置
JP3599244B2 (ja) ロボット装置、ロボット装置の運動制御装置並びに運動制御方法
US9120227B2 (en) Human motion tracking control with strict contact force constraints for floating-base humanoid robots
JP4548135B2 (ja) 脚式ロボットとその制御方法
US8868240B2 (en) Walking robot and pose control method thereof
US10836040B2 (en) Desired ZMP trajectory generating device
JP6498597B2 (ja) 移動ロボットの制御装置
WO2002040224A1 (fr) Dispositif generateur d'un modele de demarche pour robot mobile pourvu de jambes
JP7324932B2 (ja) 動的計画コントローラ
JP2007185756A (ja) 制御システム及び制御方法、並びにコンピュータ・プログラム
US20120158183A1 (en) Walking robot and control method thereof
WO2006067904A1 (ja) 脚式移動ロボットの歩容生成装置
WO2002040222A1 (fr) Dispositif et procede de commande des mouvements d'un robot marcheur mobile et procede de generation d'unites de mouvement destinees a un robot marcheur mobile
JP6421683B2 (ja) 最適制御装置、最適制御方法及び最適制御プログラム
Vladareanu et al. Modeling and hybrid position-force control of walking modular robots
Shkolnik et al. Inverse kinematics for a point-foot quadruped robot with dynamic redundancy resolution
US20220314446A1 (en) Controlling a robot using predictive decision making
Agrawal et al. Vision-aided dynamic quadrupedal locomotion on discrete terrain using motion libraries
JP6168158B2 (ja) 移動ロボットの移動制御方法、および、移動ロボット
You et al. Semi-autonomous bilateral teleoperation of hexapod robot based on haptic force feedback
Liljebäck et al. A 3D motion planning framework for snake robots

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 14868821

Country of ref document: EP

Kind code of ref document: A1

ENP Entry into the national phase

Ref document number: 2015552305

Country of ref document: JP

Kind code of ref document: A

NENP Non-entry into the national phase

Ref country code: DE

122 Ep: pct application non-entry in european phase

Ref document number: 14868821

Country of ref document: EP

Kind code of ref document: A1