JP2007007803A - Robot and control method thereof - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To provide technology for returning to foot sole disposition course described by gait data by natural action in a robot walking while correcting gait data based upon deviation between the actual foot sole disposition and the foot sole disposition course described by the gait data by measuring the foot sole disposition of the actual support leg. <P>SOLUTION: This control method includes: a process of measuring the actual foot sole disposition; a process of specifying the virtual foot sole disposition corresponding to the actual foot sole disposition; a process of calculating deviation between the actual foot sole disposition and the virtual foot sole disposition; a process of deciding the number of steps until return to the foot sole disposition course described by gait data based upon the deviation; a process of calculating the foot sole disposition course in walking based upon the gait data from the actual foot sole disposition at least up to the number of steps; a process of calculating the correction amount for each one step so that the foot sole disposition when walking up to the number of steps is performed from the actual foot sole disposition coincides with the foot sole disposition when walking up to the number of steps is performed from the virtual foot sole disposition; and a process of correcting gait data based upon the correction amount. <P>COPYRIGHT: (C)2007,JPO&INPIT

Description

  The present invention relates to a technique for controlling a robot that moves autonomously. Specifically, when walking by controlling a machine (legged robot mechanical system) in which the leg link is swingably connected to the trunk (torso), the target walking path is deviated during walking. The present invention relates to a robot that returns to a target walking path by natural movement in the event of a failure and a control method thereof.

A legged robot that walks by changing the relative postures of the left foot link, trunk, and right foot link has been developed to imitate human walking.
In order for a legged robot to walk, data indicating the motion of the left foot tip, trunk and right foot tip is required. Among them, the position and posture of the trunk need to be appropriate values for the position and posture of the toes, and if the values are not appropriate, the robot will fall.
In order to obtain the position and posture of the trunk where the legged robot does not fall down, complex calculations that take into account the dynamics of the robot are required. The calculation process is roughly as follows.
(1) Designate temporal data indicating the position and posture of the left and right toes of the robot.
(2) Designate the position where the ZMP of the robot must exist in consideration of the position and posture of the toes. ZMP (zero moment point) refers to a point on the floor where the moment of the resultant force of gravity, floor reaction force and inertial force acting on the robot becomes zero. If the ZMP is within the foot of the grounded leg, the robot will not tip over. Conversely, in order for the robot not to fall, the ZMP must be in the foot of the grounding leg. Therefore, a target ZMP that satisfies the following relationship is designated in consideration of the position and posture of the foot of the grounding leg. That is, while one leg link (for example, the left leg) is a free leg, it exists in the foot of the ground leg (right leg), and the one leg (left leg) is grounded and both legs are grounded. At the time of movement, it starts to move toward the foot of the newly grounded leg (left leg), and before the leg (right leg) that was grounded before becomes a free leg, Specify the ZMP to finish moving in the foot. The ZMP designated in this way is called a target ZMP. If the actual ZMP moves as the target ZMP, the robot continues to walk without falling.
(3) When a change in the position and posture of the toe and a target ZMP that changes following the change are specified, the dynamics of the robot is calculated assuming a change in the position and posture of the trunk over time. At the time of calculation, since the temporal changes in the position and posture of the toes are specified, assuming the position and posture of the trunk of the robot, the posture of the whole body of the robot is determined. When the posture of the whole body of the robot is determined, the position of the ZMP in that posture can be calculated. In order to calculate the position of the ZMP, in addition to static elements, the influence of inertial force acting on the robot must be taken into account. By including temporal changes in the assumed position and posture of the trunk, it is possible to calculate the position of the ZMP in consideration of the dynamics of the robot. Since it is possible to calculate the position of the ZMP assuming the temporal change of the position and posture of the trunk, it is possible to search for the temporal change of the position and posture of the trunk that realizes the ZMP that matches the target ZMP.
The data indicating the temporal changes in the position and posture of the trunk sought in the above is referred to as trunk gait data, and the data indicating the temporal changes in the position and posture of the foot that was originally specified is referred to as the foot gait. It is called data, and both are collectively called gait data. If the robot walks according to the gait data, the actual ZMP matches the target ZMP, and the robot can continue to walk without falling.

  Gait data is given by changes in position and posture over time. Since position, velocity, and acceleration are related, and other quantities can be calculated from one of these quantities, changes over time in speed or acceleration may be handled instead of position. Attitude is expressed in terms of the rotation angle with respect to the coordinate axis, but the angle, angular velocity, and angular acceleration are related, and the other amount can be calculated from one of them, so the angular velocity or angular acceleration instead of the angle You may handle changes over time. In this specification, since the gait data is described by any one of position / velocity / acceleration and any of angle / angular velocity / angular acceleration, it is referred to as data describing a motion.

  The technique for calculating the trunk movement that brings about the ZMP that matches the target ZMP is a technique for calculating the change over time of the target trunk movement that allows the robot to continue walking following the change in the target toe movement. This is a typical example and is not limited thereto. Generally speaking, a legged robot has a leg link that is swingably connected to the trunk, and describes a change in foot movement over time described by gait data. When the foot gait data is instructed, the body gait data describing the change over time of the target trunk motion that allows the user to continue walking following the change in the target foot motion is calculated and indicated It walks using the calculated toe gait data and the calculated trunk gait data.

  When the joint group is driven based on the gait data, the robot realizes a walking motion. If the realized walking motion matches the walking motion expressed by the assumed gait data, the realized ZMP will be maintained in the foot of the support leg, so the robot will be stable without falling down. Walking motion can be continued.

While the robot is walking, an unexpected disturbance force acts on the robot, and a deviation may occur between the motion described by the gait data and the actual motion. An unexpected disturbance force may act due to an unexpected unevenness of the road surface, and an external force that pushes or pulls the robot from the outside may be applied. If it deviates from the gait data due to the structural deflection of the robot, the backlash of the robot joints, the response delay of the robot, etc., it is the same as an unexpected disturbance force.
If the actual movement of the robot deviates from the movement described by the gait data, there will be a difference between the actual robot's ZMP and the target ZMP, the ZMP will not be maintained in the foot of the support leg, and the robot will fall there is a possibility. In order to prevent such a situation, a technique for detecting a state in which the robot is actually operating and feeding back to the operation described by the gait data has been developed.
In the technique described in Patent Document 1, the floor reaction force or the floor reaction force moment acting on the robot is measured to actually measure the ZMP position. The measured ZMP position is compared with the target ZMP position, and the deviation is fed back to the target positions and postures of both feet to stabilize the walking.
JP-A-5-305579

  Many conventional techniques for controlling the walking of legged robots measure the force and acceleration acting on the robot and provide feedback to the planned motion. According to such a control technique, it is possible to ensure the dynamic stability of the robot, and it is possible to realize a robot that continues to walk stably. According to the above control technique, the robot does not necessarily ground the foot of the leg link in a desired arrangement, but at least a stable walking can be realized.

However, there is a case where it is desired to control the foot of the robot leg link to be grounded in a desired arrangement. For example, when a robot walks in a room where obstacles are scattered on the floor, it is desired to control the arrangement of the grounding feet so as not to step on the obstacles. In the case of an entertainment robot that performs, it is desired to control the robot so as to walk along a predetermined walking path.
As used herein, “foot placement” refers to information that describes the area on the floor that abuts the foot when the foot of the leg link contacts the floor. Such information may be information indicating the position of a point on the floor that is in contact with a reference point of the foot in a state where the foot is in contact with the floor, for example. In addition, when the foot is in contact with the floor, the position of the point on the floor that is in contact with the foot reference point and the floor surface that matches the foot reference direction (for example, the direction from the heel to the toe) It may be information indicating both of the directions.

In order to control the arrangement of the foot when the robot is in contact with the ground, it is desirable to measure the arrangement of the foot of the supporting leg that is actually in contact with the ground and reflect it in the operation of the robot. The desired foot placement path is described in the gait data, and if the foot placement of the supporting leg that is actually touching the ground can be measured, the foot placement path described by the gait data Can be calculated and fed back to subsequent movements to return to the foot placement path described by the gait data.
When the robot is returned to the foot arrangement path described by the gait data, if the robot operation is suddenly changed, the operation becomes awkward and unnatural to the eye. In the case of a robot used for entertainment, it is desirable to return to the foot placement path described by the gait data by a smooth and natural motion, similar to walking performed by humans. A technique that can return the robot to the foot placement path described by the gait data while maintaining natural motion is awaited.

The present invention solves the above problems. The present invention is a legged robot that measures the actual foot placement of the supporting legs and walks while correcting the gait data based on the deviation of the foot placement route described by the measured foot placement and the gait data. Provides a technique capable of returning to the foot placement route described by the gait data with natural motion.
The principle of the present invention can be applied not only to walking robots but also to robots that move autonomously based on motion plans. The present invention describes an action plan in a robot that measures the actual position and direction of the robot and moves while correcting the action plan based on the deviation of the movement path described by the measured position and direction and the action plan. Provided is a technique capable of returning to a moving path with natural movement.

  The method of the present invention is a method for controlling a walking robot based on gait data. The method includes the steps of measuring “the actual foot placement of the supporting legs” and the “virtual placement foot described by the gait data” corresponding to the “actual foot placement of the supporting legs”. A step of identifying the “foot placement of the support legs”, a step of calculating a deviation between the “foot placement of the actual support legs” and the “foot placement of the virtual support legs”, and the “actual The step of determining the number of steps from returning to the “foot placement path described by the gait data” from the “placement of the foot of the supporting leg” and the “gait data from the“ foot placement of the actual support leg ” Step of calculating at least the number of steps described above, and “actual foot placement route when walking based on gait data from“ actual foot placement of supporting legs ”. Foot placement of the support legs when walking from the number of steps to To match the foot placement of the supporting legs when walking from the “virtual supporting leg foot placement” in the foot placement path described by the gait data to the number of steps, the “actual supporting leg foot” A step of calculating a correction amount for each step of “foot placement route when walking based on gait data” from “flat layout”, and a step of correcting gait data based on the correction amount.

In the above control method, the actual foot placement of the supporting legs that are in contact with the ground is measured while the robot is walking. If it is possible to measure the foot arrangement in which the support legs are actually in contact with the ground, it is possible to know how much the actual walking path of the robot is deviated from the target walking path described by the gait data. In the above control method, in the target “foot placement path described by gait data”, “virtual support leg foot placement” corresponding to “actual support leg foot placement” is specified. , The deviation between “actual foot placement of support legs” and “virtual foot support placement” is calculated. The gait data is corrected according to the calculated deviation, and the subsequent walking route is corrected.
In the above control method, the number of steps until returning to the “foot placement route described by the gait data” is determined based on the calculated deviation. If the calculated deviation is large, the number of steps until returning to the “foot placement route described by the gait data” is set to a large value, and if the calculated deviation is small, the “footprint described by the gait data” is set. The number of steps until returning to the “arrangement route” is set to a small value.
The upper limit of the stride and turning angle of each step of the robot is determined according to the movable range of the leg link of the robot. As described above, by determining the number of steps to return to the “foot placement path described by the gait data” based on the calculated deviation, the robot can perform the “gait without taking excessive action. It is possible to return to the “foot placement path described by the data” with natural operations.
In the above control method, when the number of steps until returning to the “foot placement path described by the gait data” is determined, the number of steps is determined based on the gait data from the “actual foot placement of the supporting legs”. The foot placement route when walking is calculated. Then, in the “foot placement path when walking based on gait data from“ actual support leg foot placement ”, the support when walking from“ actual support leg foot placement ”to the number of steps. The foot placement of the legs matches the foot placement of the supporting legs when walking from the "virtual supporting leg footing arrangement" described in the gait data described in the gait data to the number of steps. , “A foot placement path when walking based on gait data from“ actual foot placement of supporting legs ”is calculated for each step. Here, the correction amount is obtained for each number of steps. For example, if there is a deviation of only 20 cm, and if the number of steps is determined to eliminate the deviation in 4 steps if it is 20 cm, 5 cm is corrected at the first step, 10 cm is corrected at the second step, and 15 cm is corrected at the third step. Then, by correcting 20 cm at the fourth step, a correction amount for eliminating the shift at the fourth step is calculated.
In the above control method, the gait data is corrected based on the calculated correction amount. The robot walks based on the corrected gait data. After the number of steps determined based on the deviation, the robot can return to the foot placement route described by the gait data.

  The control method further includes a step of measuring the walking speed of the robot, and the step of determining the number of steps returns to the “foot placement path described by the gait data” based on the deviation and the walking speed. It is preferable to determine the number of steps until.

When the robot is walking at high speed, it is preferable not to change the operation greatly because the balance is easily lost if the operation is changed rapidly. Conversely, when the robot is walking at low speed, even if the movement is changed greatly, the balance will not be lost so much, so the movement will be changed greatly and return to the foot placement route described by the gait data immediately. Is preferred.
According to the control method described above, it is possible to determine the number of steps to return to the foot placement path described by the gait data according to the walking speed of the robot, while ensuring the stability of the robot's walking. It is possible to return to the foot placement route described by the gait data more quickly.

  The present invention is also embodied as a robot. The robot of the present invention is a robot that walks based on gait data. The robot measures “virtual foot placement of support legs” and “virtual foot placement path described by gait data” corresponding to “actual foot placement of support legs”. Means for identifying the “foot placement of the support legs”, means for calculating the deviation between the “foot placement of the actual support legs” and “the foot placement of the virtual support legs”, and “actually based on the deviations” Based on the gait data from the "actual foot placement of the supporting legs" and the means for determining the number of steps from the "foot placement of the supporting legs" to the "foot placement path described by the gait data" ”Foot placement path when walking on foot” and “actual foot placement path when walking based on gait data from“ actual foot placement of supporting legs ”to at least the number of steps” Foot placement of the support legs when walking from the "leg placement of the support legs" to the above steps To match the foot placement of the supporting legs when walking from the “virtual supporting foot foot placement” in the “foot placement path described by the gait data” to the number of steps described above. Means for calculating a correction amount for each step of a foot placement path when walking based on gait data from “foot placement” and means for correcting gait data based on the correction amount. Yes.

The principle of the present invention is not limited to walking robots, but measures the actual position and direction of the robot, and moves while correcting the motion plan based on the measured position and direction and the deviation of the movement path described by the motion plan. It can also be applied to autonomous mobile robots.
Another method of the present invention is a method of controlling a robot that moves based on an action plan. The method measures the “actual position and direction” of the robot, and identifies the “virtual position and direction” in the “movement path described by the motion plan” corresponding to the “actual position and direction”. A process, a process of calculating a deviation between "actual position and direction" and "virtual position and direction", and from "actual position and direction" to "movement path described by the motion plan" based on the deviation A step of determining a time until returning, a step of calculating “a movement path when moving based on an operation plan from“ actual position and direction ”” until at least the time, and “an“ actual position and direction ” The "virtual position" in the "movement path described by the action plan" is the position and direction of the robot when moving from the "actual position and direction" in the "movement path when moving based on the action plan" And directions Calculating a correction amount of “movement path when moving based on the motion plan from“ actual position and direction ”” so as to coincide with the position and direction of the robot when moving only for the above time, and And a step of correcting the operation plan based on the correction amount.

  In the above control method, the “actual position and direction” of the robot is measured, and the “virtual position and direction” corresponding to the “actual position and direction” in the “movement path described by the motion plan” is specified. . Then, the deviation between “actual position and direction” and “virtual position and direction” is calculated, and based on the deviation between the two, “actual position and direction” is returned to “movement path described by the motion plan”. The time required for the movement is determined, and a movement path that returns from the “actual position and direction” to the “movement path described by the motion plan” at the determined time is calculated. This travel route is calculated by calculating the travel route when moving from the “actual position and direction” based on the motion plan until the determined time, and the time when the calculated travel route is determined. This is done by solving the optimization problem so that it returns to the movement path described in the action plan. In the above control method, the motion plan is updated based on the calculated correction amount, and the robot can return to the movement path described by the motion plan.

  The present invention is also embodied as a robot that moves autonomously based on an operation plan. Another robot of the present invention is a robot that moves based on an operation plan. The robot measures the “actual position and direction” of the robot, and specifies the “virtual position and direction” in the “movement path described by the motion plan” corresponding to the “actual position and direction”. Means, a means for calculating a deviation between "actual position and direction" and "virtual position and direction", and from the "actual position and direction" to the "movement path described by the motion plan" based on the deviation Means for determining the time until return, means for calculating “movement path when moving based on the motion plan from“ actual position and direction ”” up to at least the time, and “actual position and direction” The "virtual position" in the "movement path described by the action plan" is the position and direction of the robot when moving from the "actual position and direction" in the "movement path when moving based on the action plan" And direction " Means for calculating a correction amount of “movement path when moving based on the motion plan from“ actual position and direction ”” so as to coincide with the position and direction of the robot when moving for the above time. Means for correcting the operation plan based on the correction amount.

According to the control method and the robot of the present invention, the actual foot placement of the support legs is measured, and the gait data is corrected based on the deviation of the foot placement route described by the measured foot placement and the gait data. When walking while walking, it is possible to return to the foot placement path described by the gait data with a natural motion.
According to another control method and robot of the present invention, the position and direction of the robot are actually measured, and the motion plan is corrected based on the measured position and direction and the deviation of the movement path described by the motion plan. When moving, it is possible to return to the movement path described by the action plan with natural movement.

  Hereinafter, embodiments embodying the present invention will be described with reference to the drawings.

Example 1
The robot 6 shown in FIG. 1 includes a head 18, a trunk 12, a right leg 20, and a left leg 22. The robot 6 moves the left leg 22 as a free leg like the track 7 while the right leg 20 is in contact with the ground, and when the left leg 22 touches, this time moves like the track 8 using the right leg 20 as a free leg. In the same manner, the left leg 22 is moved as a free leg as in the track 9, and then the right leg 20 is moved as a free leg as in the path 10 to continue walking.

  In order to control walking of the robot 6, a controller 14 is mounted on the trunk 12 of the robot 6. The controller 14 has a CPU, ROM, RAM, hard disk, and the like. The hardware configuration of the controller 14 is the same as that of a general-purpose computer, and a description thereof will be omitted. The controller 14 stores gait data, and controls the joint group of the robot 6 based on the gait data.

In order for the robot 6 to continue walking, gait data including foot gait data describing the motion of the toes and trunk gait data describing the motion of the trunk 12 is required.
The toe gait data is designated in advance by the operator. The trunk gait data is set so that the ZMP trajectory calculated using the dynamic model of the robot matches the target ZMP trajectory.

  In this embodiment, as shown in FIG. 1, the coordinate system fixed to the floor outside the robot 6 is (x, y, z), and the coordinate system fixed to the reference point of the support leg of the robot 6 is ( x ', y', z '). The coordinate system (x ′, y ′, z ′) is fixed to the reference point R when the robot 6 uses the right leg 20 as a supporting leg, and when the robot 6 uses the left leg 22 as a supporting leg, Fixed at reference point L. In the coordinate system (x ', y', z '), the direction extending from the heel to the toe along the foot is the x' axis, the direction perpendicular to the x 'axis is the y' axis, The direction perpendicular to the plane is the z 'axis. Usually, the x ′ axis corresponds to the direction in which the robot walks, the y ′ axis corresponds to the body side direction of the robot, and the z ′ axis corresponds to the height direction of the robot.

The toe gait data represents changes in the position and posture of the toes of the robot 6 over time. For example, with respect to the right leg 20, the foot gait data includes the temporal change in the position x _R , y _R , z _R of the foot reference point R and the vector R _r extending from the reference point R in the normal direction of the foot. Eulerian angles α _R , β _R , γ _R are provided over time. Similarly for the left leg 22, the toe gait data includes a temporal change in the position x _L , y _L , z _L of the foot reference point L and a vector L extending from the reference point L in the normal direction of the foot. The Euler angles α _L , β _L , and γ _L of _r are provided over time.
The trunk gait data represents changes in the position and posture of the trunk over time. The trunk gait data consists of changes in the position x _W , y _W , z _W of the trunk reference point W over time, and the Euler angle α of the vector W _r extending from the reference point W along the trunk toward the head _W , β _W , and γ _W have changes over time.

A head position detection device 220 and a head direction detection device 222 are mounted on the head 18 of the robot 6. The head position detection device 220 detects the positions x _h , y _h , and z _h of the reference point H of the head 18 viewed from the coordinate system fixed outside the robot 6, and transmits the detected positions to the controller 14. The head direction detection device 222 detects the direction of the head 18 viewed from the coordinate system fixed outside the robot 6 and transmits the detected direction to the controller 14. The direction of the head 18 is expressed by Euler angles α _h , β _h , γ _h of a vector H _r extending from the reference point H toward the front of the head 18.

  FIG. 2 is a block diagram showing the functions of the controller 14. Of the elements shown in FIG. 2, parts excluding the robot mechanical system 216, the encoder 218, the head position detection device 220, and the head direction detection device 222 are components of the controller 14. The controller 14 may be physically included in one device as a whole, or may be accommodated separately for each physically separated device.

The gait data storage unit 210 stores foot gait data designated by the operator.
The toe gait data is corrected by the adder 230 and input to the joint angle group calculation unit 212. The corrected toe gait data is also used by the trunk gait calculator 232 to calculate trunk gait data.
In addition, the gait data storage unit 210 includes a foot contact position X ′ _k , Y ′ _k (k = 1, 2,...) And a ground contact as a foot placement path described by the gait data. The direction Γ ′ _k (k = 1, 2,...) Is stored for each step. The contact position X ′ _k , Y ′ _k (k = 1, 2,...) Of the foot of the support leg is determined by the foot of the support leg for each step when the joint group is driven according to the foot gait data. The ground contact position is shown, and the position of the reference point of the foot tip in a state where the support leg is grounded is expressed by a coordinate system fixed outside the robot 6. Also, the ground contact direction Γ ' _k (k = 1, 2, ...) of the foot of the support leg indicates the ground contact direction of the foot of the support leg when the joint group is driven according to the foot gait data. The x 'axis of the coordinate system (x', y ', z') fixed to the reference point of the toe in the state where the support leg is in contact with the coordinate system (x, y, z) fixed outside It represents the angle formed with the x axis. The ground contact position X ′ _k , Y ′ _k (k = 1, 2,...) And the ground contact direction Γ ′ _k (k = 1, 2,. And the foot placement comparison unit 238.
Further, the gait data storage unit 210 stores the relative contact position ΔX ′ _k , ΔY ′ _k (k = 1, 2,...) And the relative contact direction ΔΓ ′ _k (k = 1, 2, ...) is memorized for each step. The relative contact position ΔX ' _k , ΔY' _k (k = 1, 2, ...) of the support leg is the relative position of the support leg as viewed from the contact position of the support leg one step before. Show. The relative grounding direction ΔΓ ′ _k (k = 1, 2,...) Of the support leg is a relative direction when the grounding direction of the support leg is viewed from the grounding direction of the support leg one step before. The relative contact position ΔX ′ _k , ΔY ′ _k (k = 1, 2,...) And the relative contact direction ΔΓ ′ _k (k = 1, 2,. Input to the calculation unit 226.

The trunk gait generator 232 generates trunk gait data based on the corrected toe gait data input from the adder 230. The trunk gait generator 232 identifies the target ZMP to be maintained in the foot of the supporting leg based on the input foot gait data, and the trunk such that the realized ZMP matches the target ZMP. Identify changes in position and posture over time.
The generated trunk gait data is input to the joint angle group calculation unit 212 together with the toe gait data corrected by the adder 230.

The joint angle group calculation unit 212 solves so-called inverse kinematics based on the input toe gait data and trunk gait data, so that each joint angle θ ′ _i (t) (i = 1) , 2, ..., n). The calculated joint angle group data is input to the actuator control unit 214.

  The actuator control unit 214 controls the actuator group that rotates the joint group of the robot 6. The actuator group exists in the mechanical system 216 of the robot. By controlling the actuator group, the joint group of the robot 6 can be adjusted to the calculated joint angle. When the actuator control unit 214 drives the actuator group, the robot 6 walks according to the gait data.

An encoder 218 is provided at each joint of the robot 6. The encoder 218 measures each joint angle θ _i (t) (i = 1, 2,..., N) of the robot 6, and uses the measured joint angle to calculate the support leg foot placement calculation unit 224 and the center of gravity speed. The data is output to the calculation unit 236.

The head position detection unit 220 measures the positions x _h (t), y _h (t), and z _h (t) of the head 18 in a coordinate system fixed outside the robot 6, and the measured head The position 18 is output to the support leg foot position calculation unit 224 and the center-of-gravity velocity calculation unit 236. As such a position measuring device, for example, a GPS receiver can be used. Moreover, the apparatus which measures distance using an ultrasonic wave may be sufficient, the apparatus which measures distance using a laser beam may be sufficient, and the apparatus which measures distance using electromagnetic waves may be sufficient. Alternatively, an external situation may be photographed using a camera or the like, and the position of the head may be specified based on the photographed data.
The head position detection device 220 is mounted on the top of the head 18. Thereby, it is possible to prevent the signal used for position measurement from being shielded by the body of the robot 6. Whatever posture the robot 6 takes, the position of the head 18 can be measured stably and accurately.
In addition to the above, the head position detection device 220 may be a combination of an acceleration sensor and an integrator. In this case, the head position detection device 220 measures acceleration with an acceleration sensor, measures the speed by integrating the measured acceleration, and measures the position by integrating the measured speed.
In addition to the above, the head position detection device 220 can measure the position by various position measurement techniques that are conventionally known.

The head direction detection device 222 measures the directions α _h (t), β _h (t), and γ _h (t) of the head 18 in a coordinate system fixed outside the robot 6, and the measured head The directions α _h (t), β _h (t), and γ _h (t) of the unit 18 are output to the supporting leg foot position calculating unit 224 and the center-of-gravity velocity measuring unit 236.
The head direction detection device 222 described above may be a combination of two position detection devices. In this case, the head direction detection device 222 calculates a position deviation vector from a position measured by one position detection device to a position measured by the other position detection device, and the calculated position deviation vector, The direction of the head 18 can be calculated from the mounting position of each position detection device on the head 18.
In addition to the above, the head direction detection device 222 may be a combination of a gyro sensor and an integrator. In this case, the head direction detection device 222 measures the angular velocity with the gyro sensor, and integrates the measured angular velocity to measure the direction angle. Further, as the head direction detecting device 222 described above, for example, a geomagnetic sensor can be used.
In addition to the above, the head direction detecting device 222 can measure the direction by various known direction measuring techniques.

Based on the inputs from the encoder 218, the head position detection device 220, and the head direction detection device 222, the support leg foot placement calculation unit 224 calculates the position X _k (t), Y _k (t) and the grounding direction Γ _k (t) are calculated.
Based on the joint angles θ _i (t) (i = 1, 2,..., N) of the respective joints input from the encoder 218, the support leg foot placement calculation unit 224 calculates the reference point H of the head 18. From the above, the relative position of the reference point L or R of the support leg and the relative direction of the foot of the support leg with respect to the vector Hr of the head 18 are calculated. Then, the calculated relative position and direction, the position x _h (t), y _h (t), z _h (t) input from the head position detection device 220, and the head direction detection device 222 From the input directions α _h (t), β _h (t), γ _h (t), the ground contact position X _k (t), Y _k (t) and the ground contact direction Γ _k (t) Calculate

If the measurement by the encoder 218, the head position detection device 220, and the head direction detection device 222 is ideally performed, the ground contact position X _k (t) of the foot of the support leg until the support leg is switched. , Y _k (t) and grounding direction Γ _k (t) should not change. However, due to measurement errors, the ground contact positions X _k (t), Y _k (t) and the ground contact direction Γ _k (t) calculated above may vary with time. In the robot 6 of the present embodiment, each of the ground contact positions X _k (t), Y _k (t) and the ground direction Γ _k (t) calculated as described above in the period in which the support legs are not switched. Is calculated, and the contact positions X _k and Y _k of the foot of the support leg and the contact direction Γ _k are specified.
The support leg foot position calculation unit 224 calculates the contact position X _k (t), Y _k (t) and the contact direction Γ _k (t) of the foot of the support leg, and monitors the switching of the support legs. is doing. The support leg foot placement calculation unit 224, when the support leg is switched, the foot contact position X _k (t), Y _k (t) and the contact direction Γ _k (t) calculated so far. Are integrated with respect to time to calculate an average value, and the calculated average value is set as the ground contact position X _k , Y _k and the ground contact direction Γ _k of the foot of the specified support leg, and the foot placement comparison unit 238 and the landing The data is output to the arrangement calculation unit 226.

The foot placement comparison unit 238 receives the foot contact positions X _k and Y _k and the contact direction Γ _k of the foot of the support leg in the kth step input from the support leg foot placement calculation unit 224, and the gait data storage unit 210. Are compared with the target ground contact position X ′ _k , Y ′ _k of the foot of the supporting leg in the kth step input from the target ground direction Γ ′ _k, and it is determined whether correction of gait data is necessary. The target ground contact position X ′ _k , Y ′ _k and the target ground contact direction Γ ′ _k of the foot of the support leg at the kth step input from the gait data storage unit 210 are the feet that the robot 6 targets at the kth step. This corresponds to the foot placement of the support legs when the foot placement of the support legs in the foot placement path described by the gait data is measured. If it is determined that the gait data needs to be corrected, the footing gait data is corrected by the landing arrangement calculation unit 226, the landing arrangement correction unit 234, the correction amount calculation unit 228, and the adder 230. The foot placement comparison unit 238 also includes the foot contact position X _k , Y _k and the contact direction Γ _k of the foot of the k-th support leg, and the target contact position X ′ _k , The deviation between Y ′ _k and the target grounding direction Γ ′ _k is calculated and output to the landing arrangement calculation unit 226.

The center-of-gravity speed calculation unit 236 calculates the center-of-gravity speed V (t) of the robot 6 based on outputs from the encoder 218, the head position detection device 220, and the head direction detection device 222. The calculation of the center-of-gravity velocity is based on the joint angle θ _i (t) (i = 1, 2,..., N) of each joint input from the encoder 218, for example, and the reference point of the head 18 viewed from the center of gravity. The relative position of H and the relative direction of the vector Hr of the head 18 viewed from the center of gravity are calculated. Then, the calculated relative position and direction and the position x _h (t), y _h (t), z _h (t) of the reference point H of the head 18 input from the head position detection device 220 Then, the position of the center of gravity is calculated from the directions α _h (t), β _h (t), and γ _h (t) of the vector Hr of the head 18 inputted from the head direction detecting device 222, and the center of gravity is calculated from the change over time. Calculate the velocity V (t). The calculated center-of-gravity velocity V (t) is output to the landing arrangement calculation unit 226.

The landing placement calculation unit 226 sets the number of steps m until returning to the foot placement route described by the gait data. The landing placement calculation unit 226 also calculates the measured foot placement X _k , Y _k , Γ _{k of} the supporting leg and the relative foot placement path ΔX _{k + 1} , scheduled after the k + 1 step. Based on ΔY _{k + 1} , ΔΓ _{k + 1} , ΔX _{k + 2} , ΔY _{k + 2} , ΔΓ _{k + 2} , ..., planned feet from k + 1 to k + m steps Flat arrangement paths X ^* _{k + 1} , Y ^* _{k + 1} , Γ ^* _{k + 1} ,..., X ^* _{k + m} , Y ^* _{k + m} , Γ ^* _{k + m} are calculated. Scheduled foot placement path from step k + 1 to step k + m X ^* _{k + 1} , Y ^* _{k + 1} , Γ ^* _{k + 1} , ..., X ^* _{k + m} , Y ^* _{k + m} and Γ ^* _{k + m} correspond to the foot placement route from the measured foot placement to the determined number of steps when walking based on the gait data. The calculated planned foot placement X ^* _{k + 1} , Y ^* _{k + 1} , Γ ^* _{k + 1} , ..., X ^* _{k + m} , Y ^* _{k + m} , Γ ^* _{k + m} is the landing The data is output to the arrangement correction unit 234.
The number of steps taken to return to the foot placement path described by the gait data described above is input from the center of gravity speed V (t) of the robot 6 input from the center of gravity speed calculation unit 236 and the foot placement comparison unit 238. It is set according to the deviation.

The landing arrangement correcting unit 234 inputs the foot arrangements X ^* _{k + 1} , Y ^* _{k + 1} , Γ ^* _{k + 1} that the leg link is expected to contact after the (k + 1) th step inputted from the landing arrangement calculating unit 226. ,..., X ^* _{k + m} , Y ^* _{k + m} , Γ ^* _{k + m,} and the desired foot arrangement X ′ _{k + 1} after the (k + 1) th step inputted from the gait data storage unit 210 _{, Y 'k + 1, Γ} ' k + 1, ···, X 'k + m, Y' k + m, on the basis of the Γ _{'k + m,} the correction amount ΔX of the foot placement path ⁺ _{k + 2} , ΔY ⁺ _{k + 2} , ΔΓ ⁺ _{k + 2} ,..., ΔX ⁺ _{k + m} , ΔY ⁺ _{k + m} , ΔΓ ⁺ _{k + m} . Note that the foot placement of the k + 1 step is not corrected because the foot is switched to the k + 1 step when the foot placement of the kth support leg is specified. . Correction amount ΔX ⁺ _{k + 2} , ΔY ⁺ _{k + 2} , ΔΓ ⁺ _{k + 2} , ..., ΔX ⁺ _{k + m} , ΔY ⁺ _{k + m} , ΔΓ ⁺ _{k + m} for foot placement path is determined The foot placement route calculated for the number of steps corresponds to the correction amount for returning to the foot placement route described by the gait data.

Hereinafter, calculation of the foot placement route correction amounts ΔX ⁺ _{k + 2} , ΔY ⁺ _{k + 2} , ΔΓ ⁺ _{k + 2} ,... By the landing placement correcting unit 234 will be described. The landing arrangement correcting unit 234 uses the evaluation function J to calculate a correction amount of the foot arrangement for realizing the optimum foot arrangement path.
The evaluation function J used here is given below.

Here, J _Γ , J _X , and J _Y are evaluation functions, and w ₁ , w ₂ , and w ₃ are weight functions that weight each evaluation function.
J _Γ is an evaluation function related to the deviation between the foot contact direction described by the gait data and the foot contact direction realized. The J _gamma is defined by the following equation, to minimize J _gamma means that to minimize the accumulated value of the time rate of change of the ground direction of the deviation of the foot.

J _X and J _Y are evaluation functions related to deviations in the X direction and Y direction between the foot contact position described by the gait data and the foot contact position realized. J _X and J _Y are defined by the following equations, and minimizing J _X and J _Y means minimizing the integrated value of the square of the deviation of the foot contact position in the X and Y directions.

The values of the weight functions w ₁ , w ₂ , and w ₃ define the priorities of the above-described conditions for the deviation in the ground contact direction and the conditions for the deviation in the ground contact position. I can leave. For w ₁ , w ₂ , and w ₃ , any one may be a predetermined value and the others may be zero.

The landing arrangement correcting unit 234 calculates a foot arrangement path that minimizes the evaluation function J described above while satisfying the following constraint conditions.
In the foot placement for each step of the robot 6 of this embodiment, the foot position and direction must be set within a range specified based on the foot placement one step before. The above range is defined in consideration of the movable range of the joint when swinging out the leg link and the interference between the leg links.
In addition, the correction amount is 0 for the foot placement at the k + 1 step, and the foot placement at the k + m step set as the number of steps until returning to the foot placement route described by the gait data is Must match the target foot placement.
The setting of the walking path that minimizes the evaluation function J under the above-described constraints can be performed using various optimization methods known in the art. In the present specification, detailed description is omitted.
In the above description, an example in which the foot placement path that minimizes the evaluation function J is set has been described. Instead of using the evaluation function, for example, an upper limit value of the deviation in the ground contact direction of the foot is set in advance. The foot placement path may be calculated so that the deviation in the flat contact direction does not always exceed the upper limit value.

The correction amount calculation unit 228 includes toe gait data x _{k + 2} (t), y _{k + 2} (t), z _{k + 2} (t), output from the gait data storage unit 210 to the adder 230. Correction amounts corresponding to α _{k + 2} (t), β _{k + 2} (t), γ _{k + 2} (t) Δx _{k + 2} (t), Δy _{k + 2} (t), Δz _{k + 2} (t ), Δα _{k + 2} (t), Δβ _{k + 2} (t), Δγ _{k + 2} (t). The correction amount calculation unit 228 generates a foot gait corresponding to the foot placement correction amounts ΔX ⁺ _{k + 2} , ΔY ⁺ _{k + 2} , ΔΓ ⁺ _{k + 2} ,... Input from the landing placement correction unit 234. Data correction amount Δx _{k + 2} (t), Δy _{k + 2} (t), Δz _{k + 2} (t), Δα _{k + 2} (t), Δβ _{k + 2} (t), Δγ _{k + 2} (t ), ... is calculated. Ashisaki gait data correction amount _{Δx k + 2 (t),} Δy k + 2 (t), Δz k + 2 (t), Δα k + 2 (t), Δβ k + 2 (t), Δγ k ₊₂ (t), ... is the amount of correction for the toe gait data that describes the position / posture of the toes of the free leg, and the toes' position and posture of the leg link that becomes a free leg for each step Is expressed over time.
Correction amount of the above foot gait data Δx _{k + 2} (t), Δy _{k + 2} (t), Δz _{k + 2} (t), Δα _{k + 2} (t), Δβ _{k + 2} (t), Δγ _{k + 2} (t),... Is calculated by multiplying the correction amounts ΔX ⁺ _{k + 2} , ΔY ⁺ _{k + 2} , ΔΓ ⁺ _{k + 2} of the foot placement by a gain that changes with time. This gain is defined to be 0 when the support leg is switched, gradually increases, and is maximized when the leg link that has been the support leg is grounded next time. FIG. 5 shows how the foot gait data is corrected in accordance with the correction amount of the foot contact position and the correction amount in the contact direction. By correcting the toe gait data in this way, the movement until the leg link that has been the supporting leg floats to become a free leg and then touches the ground without rapidly changing the movement of the robot. It is possible to perform correction so as to contact the corrected foot placement. The foot arrangement when the leg link contacts the ground can be brought close to the foot arrangement described by the gait data.
The correction amount calculation unit 228 calculates the correction amount of the foot gait data for each step until returning to the foot placement route described by the gait data, and the correction amount of the calculated foot gait data Among these, the correction amount at the number of steps (k + 2 step in FIG. 2) corresponding to the foot gait data output to the adder 230 is output to the adder 230.
Correction amounts Δx _{k + 2} (t), Δy _{k + 2} (t), Δz _{k + 2} (t), Δα _{k + 2} (t), Δβ _{k + 2} (t) output from the correction amount calculation unit 228 , Δγ _{k + 2} (t) are added to the foot gait data x _{k + 2} (t), y _{k + 2} (t), z _{k + 2} (t), α _{k + 2} (t) by the adder 230. , β _{k + 2} (t), γ _{k + 2} (t).

  The robot 6 reads the toe gait data from the gait data storage unit 210, corrects the toe gait data if necessary, calculates the trunk gait data, calculates the joint angle group, To control. Each joint rotates by driving the actuator, and the robot 6 realizes walking.

  While the robot 6 is performing the operation, the controller 14 performs the processing shown in the flowchart of FIG. 3 in parallel. Hereinafter, the process shown in FIG. 3 will be described.

In step S302, the current step count k is specified. The number of steps k is a number indicating how many steps the supporting leg currently in contact with corresponds to after the robot 6 starts walking.
In step S304, the positions x _h (t), y _h (t), and z _h (t) of the head 18 viewed from the coordinate system fixed outside the robot 6 are measured. The positions x _h (t), y _h (t), and z _h (t) of the head 18 are measured by the head position detection device 220 mounted on the head 18.
In step S306, directions α _h (t), β _h (t), and γ _h (t) of the head 18 viewed from the coordinate system fixed outside the robot 6 are measured. The directions α _h (t), β _h (t), and γ _h (t) of the head 18 are measured by a head direction detection device 222 mounted on the head 18.
In step S308, the joint angle θ _i (i = 1, 2,..., N) of the joint group of the robot 6 is measured. The joint angle θ _i (i = 1, 2,..., N) of the joint group is measured by the encoder 218.
In step S310, the foot placement X _k (t), Y _k (t), Γ _k (t) of the k-th supporting leg as the supporting leg at that time is calculated. The foot placement X _k (t), Y _k (t), Γ _k (t) of the support leg currently in contact with the ground is the position x _h (t), y _h ( t), z _h (t), the direction α _h (t), β _h (t), γ _h (t) of the head 18 measured in step S306, and the robot joint group measured in step S308 Is calculated using a Jacobian matrix based on the joint angle θ _i (i = 1, 2,..., N).
In step S312, the center of gravity velocity V (t) of the robot 6 at that time is calculated. The center-of-gravity velocity V (t) depends on the position x _h (t), y _h (t), z _h (t) of the head 18 measured in step S304 and the direction α of the head 18 measured in step S306. _{Based on h} (t), β _h (t), γ _h (t) and the joint angle θ _i (i = 1, 2,..., n) of the robot joint group measured in step S308, Calculated using the centroid Jacobian matrix.
In step S314, it is determined whether or not the support leg has been switched. If the support leg is switched (YES in step S314), the process proceeds to step S316. If the support legs have not yet been switched (NO in step S314), the process proceeds to step S304, and the processes from step S304 to step S314 are repeatedly performed.
In step S316, average values X _k , Y _k , Γ _k of the foot placements X _k (t), Y _k (t), Γ _k (t) of the supporting leg in the kth step are calculated. The average value X _k , Y _k , Γ _k of the foot placement of the support legs is the foot placement of the support legs X _k (t), Y _k (t), Γ, which is calculated every moment in step S310. Obtained by calculating the time average of _k (t). The above average value calculation is performed for a period from the time when the support leg is switched to the kth step to the time when the support leg is switched to the k + 1th step. At the time of performing the process of step S316, the k + 1 step support legs are in contact with the ground, and the foot placement of the _kth support legs, which has been in contact with the previous step by the process of step S316, Xk, Y _k and Γ _k are specified.

FIG. 4 shows the transition of the foot placement when the robot 6 of this embodiment walks. For the foot placement paths 402, 404, 406, 408, 410, 412, and 414 described by the gait data, the robot 6 grounds the left leg 22 to the foot placement 416 at the k-1th step, and takes k steps. The right leg 20 is grounded to the foot arrangement 418 with eyes. While the robot 6 floats the left leg 22 as a free leg and swings the left leg 22 toward the foot placement 420 at the (k + 1) th step, the controller 14 repeatedly performs the processing from step S302 to step S314. Then, at the kth step, the ground contact positions X _k (t), Y _k (t) and the ground contact direction Γ _k (t) of the foot arrangement 418 of the right leg 20 as the support leg are repeatedly measured. When the left leg 22 which is a free leg contacts the foot arrangement 420 of the (k + 1) th step, the supporting leg is switched from the right leg 20 to the left leg 22, and the foot arrangement 418 of the k-th supporting leg in step S316. The ground contact positions X _k and Y _k and the ground direction Γ _k are specified.

Returning to FIG 3, in step S318, the foot arrangement comparator 238, the foot arrangement X _k of the identified k-th step of the support legs, Y _k, and gamma _k, k-th step of the support legs of desired foot flat arranged _{X 'k, Y' k,} Γ 'k deviation δX _k, δY _k, calculates the [Delta] [gamma] _k.
In the example shown in FIG. 4, the foot placement 418 (X _k , Y _k , Γ _k ) of the identified support legs at the kth step and the target foot placement 404 (X ′ _k , Y ′ _k , Γ ′ _k ) (δX _k , δY _k , δΓ _k ) are calculated.

Returning to FIG. 3, in step S320, the foot placement comparison unit 238 compares the calculated deviations δX _k , δY _k , and δΓ _k with thresholds indicating the respective allowable ranges. If any one of the calculated deviations δX _k , δY _k , δΓ _k exceeds the threshold value (YES in step S320), the process proceeds to step S322 to correct the subsequent operation pattern. Proceed to If all of calculated deviations δX _k , δY _k , and δΓ _k are equal to or less than the threshold value (NO in step S320), the process proceeds to step S328 without correcting the subsequent operation pattern.

In step S322, the landing placement calculation unit 238 sets the number of steps m from the current foot placement until returning to the foot placement route described by the gait data.
The number of steps m until return is set according to the foot placement deviations δX _k , δY _k , δΓ _k and the speed V (t) of the robot 6.
When the foot placement deviations ΔX _k , ΔY _k , and ΔΓ _k are large, a large number of steps m to return is set. When the foot placement deviations ΔX _k , ΔY _k , and ΔΓ _k are small, the number of steps m to return is set to be small. In this way, by setting the number of steps m to return according to the foot placement deviations δX _k , δY _k , and δΓ _k , the robot 6 can describe gait data with natural motion without taking an unreasonable posture. It is possible to return to the foot placement route.
If the speed V (t) is fast, set a large number of steps m to return. If the speed V (t) is slow, set a smaller number of steps m to return. In this way, by setting the number of steps m to return in accordance with the speed V (t), it is possible to suppress the movement change when the robot 6 moves at a high speed, which is likely to lose the balance, and the robot 6 can be prevented from walking. Stability can be ensured. Further, when the robot 6 can move stably at a low speed, it is possible to quickly return to the foot placement path described by the gait data, and to follow the foot placement path described by the gait data. Improves.
In the example shown in FIG. 4, the number m of steps from the measured foot placement 418 of the supporting leg at the kth step until returning to the foot placement route described by the gait data is set to 5 steps, and k + An example of returning to the foot arrangement 414 described by the gait data at the fifth step is shown.

In step S324, an optimum foot placement route that can return to the foot placement route described by the gait data with the number of steps set in step S322 is calculated.
First, the landing placement calculation unit 226 calculates the planned foot placement routes X ^* _{k + 1} , Y ^* _{k + 1} , Γ ^* _{k + 1} ,... From the (k + 1) th step to the (k + 5) th step.・, X ^* _{k + 5} , Y ^* _{k + 5} , Γ ^* _{k + 5} are calculated. The landing arrangement calculation unit 226 receives the foot arrangements X _k , Y _k , Γ _{k of the k-th} supporting leg input from the support leg foot arrangement calculation unit 224 and k input from the gait data storage unit 219. Relative foot placement paths scheduled after +1 step ΔX _{k + 1} , ΔY _{k + 1} , ΔΓ _{k + 1} , ..., ΔX _{k + 5} , ΔY _{k + 5} , ΔΓ _{k + 5} Based on, planned foot placement paths up to k + 5 steps X ^* _{k + 1} , Y ^* _{k + 1} , Γ ^* _{k + 1} , ..., X ^* _{k + 5} , Y ^* Calculate _{k + 5} and Γ ^* _{k + 5} . The calculated planned foot placement paths X ^* _{k + 1} , Y ^* _{k + 1} , Γ ^* _{k + 1} , ..., X ^* _{k + 5} , Y ^* _{k + 5} , Γ ^* _{k + 5} are It is output to the landing arrangement correcting unit 234.

Next, the landing arrangement correcting unit 234 corrects the amount of correction of the foot arrangement from the k + 2 step to the k + 5 step, ΔX ⁺ _{k + 2} , ΔY ⁺ _{k + 2} , ΔΓ ⁺ _{k + 2} ,. ΔX ⁺ _{k + 5} , ΔY ⁺ _{k + 5} , ΔΓ ⁺ _{k + 5} are calculated. Here, at the time when the process of step S324 is executed, the k + 1 step support legs are already in contact with the ground, and the k + 1 step foot placement X ^* _{k + 1} , Y ^* _{k + 1} , Γ ^*. Note that _{k + 1} is not corrected.
The correction amounts Δ ⁺ X _{k + 2} , ΔY ⁺ _{k + 2} , ΔΓ ⁺ _{k + 2} , ・ ・ ・, ΔX ⁺ _{k + 5} , ΔY ⁺ _{k + 5} , ΔΓ ⁺ _{k + 5} It is calculated as a solution to an optimization problem with constraints.
The constraints used here are that the foot placement at each step can be grounded only within the range specified from the foot placement of the previous step, and that the foot placement at the k + 1 step is not corrected. This corresponds to the foot arrangement described by the gait data at the k + 5th step.
Under the above-mentioned constraints, the correction amount of the foot placement ΔX ⁺ _{k + 2} , ΔY ⁺ _{k + 2} , ΔΓ ⁺ _{k + 2} ,. ΔX ⁺ _{k + 5} , ΔY ⁺ _{k + 5} , ΔΓ ⁺ _{k + 5} are calculated. The calculation of the optimal foot placement correction amount can be performed using various conventionally known optimization methods, and detailed description thereof is omitted in this specification.
In the example shown in FIG. 4, the planned foot arrangement 422 (X ^* _{k + 2} , Y ^* _{k + 2} , Γ ^* _{k + 2} ) of the k + ₂ step, the planned foot arrangement 424 of the k + 3 step ( X ^* _{k + 3} , Y ^* _{k + 3} , Γ ^* _{k + 3} ), planned foot placement 426 at the k + 4th step (X ^* _{k + 4} , Y ^* _{k + 4} , Γ ^* _{k + 4} ), Correction amounts are calculated for the planned foot placement 428 (X ^* _{k + 5} , Y ^* _{k + 5} , Γ ^* _{k + 5} ) of the _{k + 5th step} . As a result of such correction, the k + 2 step is to the corrected planned foot arrangement 430, the k + 3 step is to the corrected planned foot arrangement 432, and the k + 4 step is the corrected planned foot arrangement 430. Correction is made so as to contact the flat arrangement 434, and the k + 5th step is corrected so as to contact the corrected planned foot arrangement that matches the foot arrangement 414 described by the gait data.
ΔX ⁺ _{k + 2} , ΔY ⁺ _{k + 2} , ΔΓ ⁺ _{k + 2} , ..., ΔX ⁺ _{k + 5} , ΔY ⁺ _{k + 5} , ΔΓ ⁺ _{k + 5} It is output to the correction amount calculation unit 228.

Returning to FIG. 3, in step S326, the correction amount calculation unit 228 calculates the correction amount of the toe gait data. The correction amount calculation unit 228 includes foot placement correction amounts ΔX ⁺ _{k + 2} , ΔY ⁺ _{k + 2} , ΔΓ ⁺ _{k + 2} ,..., ΔX ⁺ _{k + 5} , which are input from the landing placement correction unit 234. Based on ΔY ⁺ _{k + 5} and ΔΓ ⁺ _{k + 5} , correction amount of foot gait data Δx _{k + 2} (t), Δy _{k + 2} (t), Δz _{k + 2} (t), Δα _{k +} Calculate ₂ (t), Δβ _{k + 2} (t), Δγ _{k + 2} (t). The correction amount of the above-mentioned foot gait data is calculated for each step based on the correction amount of the foot placement for each step.
For example, correction amount of foot gait data of leg link touching the ground at k + 2 step Δx _{k + 2} (t), Δy _{k + 2} (t), Δz _{k + 2} (t), Δα _{k + 2} (t ), Δβ _{k + 2} (t), Δγ _{k + 2} (t) is the correction amount of the ground contact position of the foot of k + 2 step ΔX ⁺ _{k + 2} , ΔY ⁺ _{k + 2} and the correction amount of the ground contact direction It is calculated by multiplying ΔΓ ⁺ _{k + 2} by a gain that changes with time. As shown in FIG. 5, this gain is defined in the period from the time when the leg link, which was the supporting leg at the k-th step, floats to become a free leg until the time when it reaches the next foot placement, and is initially zero. It is set to increase with time.
The correction amount calculation unit 228 calculates and stores the correction amount of the above-mentioned footstep gait data for each step. The correction amount calculation unit 228 monitors the switching of the support legs of the robot 6 and outputs the correction amount of the toe gait data at the corresponding number of steps to the adder 230 every time the support legs are switched.
In FIG. 2, the correction amount of the toe gait data of the leg link touching on the k + 2 step Δx _{k + 2} (t), Δy _{k + 2} (t), Δz _{k + 2} (t), Δα _{k + 2} (t), Δβ _{k + 2} (t), Δγ _{k + 2} (t) are converted into foot gait data x _{k + 2} (t), y _{k + 2} (t), z _{k +} by adder 230. _This shows the case where ₂ (t), α _{k + 2} (t), β _{k + 2} (t), and γ _{k + 2} (t) are added. Based on the corrected toe gait data, the trunk gait data is calculated, the joint angle group is calculated, the actuator is driven, and the robot 6 continues walking.
Thereafter, the process proceeds to step S328, and waits until the robot 6 advances walking by a predetermined number of steps. After the supporting legs of the robot 6 are switched a predetermined number of times and the robot 6 has walked a predetermined number of steps, the process proceeds to step S302, and the series of processes described above is repeated again.

  The above-described legged robot 6 measures the actual foot placement of the supporting legs, and walks while correcting the gait data based on the deviation of the foot placement route described by the measured foot placement and the gait data. To do. The above legged robot 6 determines the number of steps to return to the foot placement path described by the gait data based on the measured foot placement and the deviation of the foot placement path described by the gait data. Therefore, it is possible to return to the foot placement path described by the gait data with a natural motion without taking an unreasonable posture.

(Second embodiment)
Another robot 610 according to the present invention will be described with reference to the drawings. FIG. 6 is a plan view of how the robot 610 travels. The robot 610 plays the drum 660 while traveling through the venue 602 (beats the drum 660). A podium 700 is installed at the venue 602. The conductor 702 conducts the command by swinging the command stick 730 on the command platform 700. The robot 610 performs a performance while visually confirming that the conductor 702 swings the conductor rod 730. In FIG. 6, the conductor 702 is shown in a very simplified manner.
The robot 610 has a head 640 imitating a human head (face), a torso part 670 imitating a human torso, wheels 622a and 622b, and the like. Holding. A detailed configuration of the robot 610 will be described later. The robot 610 plays the drum 660 while traveling through the set substantially arc-shaped moving route (position A → position B → position C →...).

First, the configuration of the robot 610 will be described in detail. FIG. 7 shows a front view of the robot 610. It can be said that the robot 610 is a humanoid robot imitating a human.
The robot 610 advances itself by rotating the wheels 622a and 622b. The robot 610 plays the drum 660 while moving so that a human plays (beats) the drum while marching.

The body part 670 has a substantially rectangular parallelepiped shape. The body 670 is directed to a controller 690 that comprehensively controls the operation of the robot 610, a motor 652 that rotates the head 640, a position sensor 712 that measures the position of the robot 610, and the body 670. A direction sensor 714 for measuring the direction and the like are accommodated.
Arms (arms) 630 a and 630 b are connected to the body portion 670. Hereinafter, the arm 630a is referred to as a right arm, and the arm 630b is referred to as a left arm. The right arm 630a has an upper arm portion 632a and a lower arm portion 634a. One end of the upper arm portion 632a is swingably attached to the body portion 670. The lower arm 634a is connected to the other end of the upper arm 632a by a joint 638a. The lower arm 634a can swing around the joint 638a. This joint 638a corresponds to a human elbow joint. A repellent 636a for hitting the drum 660 is fixed to one end of the lower arm 634a. An actuator 635a (for example, a solenoid or a motor) that swings the lower arm 634a is provided inside the upper arm 632a. The actuator 635a is driven based on a control signal output from the control unit 690. The left arm 630b has the same configuration as the right arm 630a. That is, the left arm 630b has an upper arm portion 632b and a lower arm portion 634b. The lower arm 634b can swing around the elbow joint 638b. A repellent 636b is fixed to one end of the lower arm 634b. An actuator 635b that swings the lower arm 634b is provided in the upper arm 632b.
The position sensor 712 is fixed near the center of the body portion 670. The position sensor 712 includes a DGPS receiver (not shown), and outputs the position of the robot 610 inside the venue 602 using a coordinate system (X, Y) shown in FIG. The position sensor 712 is connected to the control unit 690 and outputs the measured position to the control unit 690.
The direction sensor 714 is fixed near the center of the body portion 670. The direction sensor 714 measures the direction of the line of magnetic force of the geomagnetism, and outputs the direction in which the body portion 670 is directed at a tilt angle θ from a straight line (for example, the X axis of the coordinate system shown in FIG. To do. The direction sensor 714 is connected to the control unit 690 and outputs the measured direction to the control unit 690.

The head 640 has a substantially rectangular parallelepiped shape. The head 640 is provided with two holes 640a and 640b which are made to imitate human eyes. The hole 640a corresponds to the left eye, and the hole 640b corresponds to the right eye. The lens of the video camera 642 that continues to photograph continuously faces the hole 640a.
A neck portion 650 is provided between the head portion 640 and the body portion 670. The neck 650 has a substantially cylindrical shape. The neck portion 650 is supported by the body portion 670 so that the neck portion 650 can rotate (the rotation axis is indicated by a symbol C1). The rotation of the neck 650 causes the head 640 of the robot 610 to turn sideways so that a human shakes his head and turns sideways. The motor 652 that rotates the neck 650 is driven based on a control signal output from the control unit 690. Depending on the rotation direction of the motor 652, the head 640 turns to the right or to the left. During operation, the robot 610 rotates the neck 650 so that the head 640 faces the conductor 702 and the video camera 642 keeps shooting the conductor 702 at all times.

The lower end of the body portion 670 is fixed to the moving mechanism 620. The moving mechanism 620 rotatably holds the wheels 622a and 622b. The wheels 622a and 622b are coupled to the actuators 624a and 624b, respectively, and can rotate at different rotational speeds. Actuators 624a and 624b rotate wheels 622a and 622b in accordance with instructions from control unit 690.
The moving mechanism 620 includes an encoder 710 that detects the rotation angles of the wheels 622a and 622b. The encoder 710 detects the rotation angles of the wheels 622a and 622b at a predetermined time interval and outputs them to the control unit 690.

  FIG. 8 shows a schematic configuration of the control unit 690 of the robot 610. The control unit 690 includes an operation pattern storage unit 802, a path correction unit 804, an operation pattern update unit 806, an actuator control unit 808, an instruction recognition unit 812, a position / direction comparison unit 814, and a path estimation unit 816. A correction amount calculation unit 818 and a speed calculation unit 820 are provided.

  The movement pattern storage unit 802 stores the movement path (path described by the movement plan) targeted by the robot 610, the position (coordinates) of the podium 700, and the movement plan of the robot 610. The operation plan of the robot 610 includes an operation pattern related to raising and lowering the right arm 630a and the left arm 630b, an operation pattern related to rotation of the wheels 622a and 622b for moving along the target movement path of the robot 610, and a conductor 702. An operation pattern relating to the rotation of the neck 650 for continuing to capture the image is stored. The operation pattern related to raising and lowering of the right arm 630a and the left arm 630b describes a change with time of the relative position of the tip of the repellent 636a, 636b with respect to the body portion 670. The operation pattern related to the rotation of the wheels 622a and 622b describes a change with time of the rotation angle of the wheels 622a and 622b. The operation pattern related to the rotation of the neck 650 describes the change over time of the rotation angle of the neck 650 with respect to the body 670.

  The route correction unit 804 uses the correction amount input from the correction amount calculation unit 818 to correct the operation pattern related to rotation of the wheels 622a and 622b input from the operation pattern storage unit 802. The corrected motion pattern related to the rotation of the wheels 622a and 622b is input to the motion pattern update unit 806. Further, the path correction unit 804 corrects the operation pattern related to the rotation of the neck 650 based on the corrected operation pattern related to the rotation of the wheels 622a and 622b so that the head 640 continues to photograph the conductor 702. The data is output to the pattern update unit 806.

  The operation pattern update unit 806 includes an operation pattern related to raising and lowering of the right arm 630a and the left arm 630b input from the operation pattern storage unit 802, an operation pattern related to the wheels 622a and 622b input from the path correction unit 804, and a conductor 702. The operation pattern related to the rotation of the neck portion 650 for continuously capturing the image is updated based on the instruction content input from the instruction recognition unit 812. When the conductor 702 swings the conductor rod 730 quickly (that is, when the tempo is fast), the moving mechanism 620 is controlled so that the robot 610 moves at high speed. Conversely, when the conductor 702 swings the conductor rod 730 slowly (that is, when the tempo is slow), the moving mechanism 620 is controlled so that the robot 610 proceeds at a low speed.

  The actuator control unit 808 calculates the change over time of the target rotation angle of each actuator based on each operation pattern input from the operation pattern update unit 806, and realizes the calculated rotation angle. The group 810 is driven. The actuator group 810 includes actuators 624a, 624b, 635a, 635b, a motor 652 and the like.

  The instruction recognition unit 812 recognizes the instruction content from the conductor 702 based on the image data input from the video camera 642. The recognized instruction content is input to the operation pattern update unit 806.

  The speed calculation unit 820 calculates the moving speed V of the robot 610 based on the rotation angle of the wheels 622a and 622b input from the encoder 710, the diameter of the wheels 622a and 622b, and the distance between the wheels 622a and 622b.

  The position / direction comparing unit 814 includes the actual position and direction of the body 670 input from the position sensor 712 and the direction sensor 714 and the path of the body 670 in the path described by the operation plan input from the operation pattern storage unit 802. The position and direction are compared, and the deviation is calculated for each position and direction. The position / direction comparison unit 814 determines whether to correct the operation pattern of the robot 610 based on the calculated deviation. The position / direction comparison unit 814 inputs the calculated deviation and the determination result regarding the necessity of correction to the route estimation unit 816.

  When the route estimation unit 816 receives the determination result that the motion pattern needs to be corrected from the position / direction comparison unit 814, the route estimation unit 816 sets the time from the actual position and direction until returning to the route described by the motion plan. . The time until return is set based on the position / direction deviation input from the position / direction comparison unit 814 and the speed V input from the speed calculation unit 820. The larger the input deviation is, the longer the time until return is set, and the smaller the input deviation is, the shorter the time until return is set. The higher the input speed V, the longer the time until return is set, and the lower the input speed V, the shorter the time until return. Thereafter, the path estimation unit 816 determines the movement path of the robot 610 when operating based on the operation plan stored in the operation pattern storage unit 802 from the actual position and direction measured by the position sensor 712 and the direction sensor 714. Calculate up to the time set as the time to return. The calculated movement route is input to the correction amount calculation unit 818.

  The correction amount calculation unit 818 returns to the travel route described by the motion plan input from the motion pattern storage unit 802, the calculated travel route input from the route estimation unit 816, and the travel route described by the motion plan. The correction amount of the calculated movement route is calculated based on the time until. The calculated amount of correction of the moving path takes into consideration the integrated value of the absolute value of the time change rate of the direction deviation of the robot 610 and the integrated value of the square of the positional deviation of the robot 610 as an evaluation function, and uses these evaluation functions. It is calculated so that the movement path to be minimized is realized. When the correction amount of the movement route is calculated, the correction amount calculation unit 818 calculates the correction amount of the operation pattern related to the rotation of the wheels 622a and 622b. The calculated correction amount of the operation pattern is input to the path correction unit 804.

The robot 610 reads the motion plan from the motion pattern storage unit 802, updates the motion plan by the motion pattern update unit 806 according to the instruction content from the conductor 702 photographed by the video camera 642, and creates the updated motion plan. Based on this, the actuator group 810 is driven to operate. The control unit 690 sequentially executes the processing shown in the flowchart of FIG. 9 while the robot 610 is operating, and reflects the position and direction input from the position sensor 712 and the direction sensor 714 in the operation plan.
Hereinafter, the operation plan correction process executed by the control unit 690 of the robot 610 will be described with reference to FIG.

  In step S <b> 902, the position of the robot 610 is measured using the position sensor 712. The position of the robot 610 is described in the coordinate system (X, Y) shown in FIG.

  In step S904, the direction of the robot 610 is measured using the direction sensor 714. The direction of the robot 610 is described by the inclination angle θ from the X axis of the coordinate system shown in FIG.

  In step S906, the speed of the robot 610 is measured using the input from the encoder 710. The speed of the robot 610 is calculated from the change over time of the rotation angle of the wheels 622a and 622b, the diameter of the wheels 622a and 622b, and the distance between the wheels 622a and 622b.

  In step S908, the position measured in step S902, the direction measured in step S904, and the position and direction at the corresponding point in the movement path described by the motion plan are compared, and a deviation is calculated for each of the position and direction. To do.

  In step S910, the calculated deviation is compared with a threshold value indicating an allowable range. If one or both of the calculated position and direction deviations exceed the respective threshold values (YES in step S910), the process proceeds to step 912 to correct the operation plan of the robot 610. If both the calculated position and direction deviations are equal to or less than the respective threshold values (NO in step S910), it is determined that there is no need to correct the operation plan of robot 610, and the process proceeds to step S918. move on.

In step S912, a time until returning to the movement route described by the operation plan is determined. The time until return is determined based on the deviation calculated in step S908 and the moving speed V of the robot 610 measured in step S906.
The time until returning is set to a larger value as the deviation calculated in step S908 is larger, and is set to a smaller value as the deviation calculated in step S908 is smaller. By setting the time until returning to the movement path in this way, the robot 610 can return to the target movement path with natural movement without suddenly changing the movement.
The time until the return is set to a larger value as the moving speed calculated in step S906 is larger, and is set to a smaller value as the moving speed calculated in step S906 is smaller. When the robot 610 is moving at a high speed, if the movement path is suddenly changed, the actuators 624a and 624b driving the wheels 622a and 622b need to suddenly change the output torque, and the actuators 624a and 624b It puts a big burden. By setting the time until returning to the target movement path in this way, the robot 610 can return to the target movement path with natural movement without placing a heavy burden on the actuators 624a and 624b. .

  In step S914, a route for moving based on the motion plan is calculated from the measured position and direction. In step S914, a correction amount for correcting the calculated route to an optimum route is calculated. The optimum path is set so as to minimize the above-described evaluation function. By calculating the difference between the optimal route and the calculated route, the route correction amount is calculated.

  In step S916, based on the route correction amount calculated in step S914, the operation pattern correction amount related to the rotation of the wheels 622a and 622b is calculated. The motion plan input to the motion pattern update unit 806 is corrected based on the calculated correction amount.

In step S918, the process waits until a predetermined time has elapsed. After waiting for a predetermined time, the process proceeds to step S902, and the above-described process is repeated.
By repeatedly performing the above-described series of processing, the robot 610 can return to the target route with natural movement.

The manner in which the robot 610 returns to the path described in the operation plan by the above-described processing will be described with reference to FIG.
The robot 610 of this embodiment measures the actual position 1004 in step S902, measures the actual direction 1006 in step S904, and in step S906, the position and direction in the path 1002 described in the action plan, and the measured position. The deviation between 1004 and direction 1006 is calculated. If it is determined in step S910 that the motion plan needs to be corrected, a time until returning to the path 1002 described in the motion plan is determined in step S912, and according to the motion plan from the position 1004 and the direction 1006 measured in step S914. An optimum route 1010 from the route 1008 when moving to the return to the route 1002 described in the operation plan is calculated, and a route correction amount 1012 is calculated so as to realize the calculated optimum route. Based on the calculated route correction amount 1012, the operation pattern correction amount relating to the rotation of the wheels 622a and 622b is calculated to correct the operation plan.

As mentioned above, although embodiment of this invention was described in detail, these are only illustrations and do not limit a claim. For example, in the gait data, an example was described in which changes over time were described in terms of time and position. However, time and speed, or the relationship between time and acceleration, time-dependent movements of feet, arms, and trunk Changes may be described. The technology described in the claims includes various modifications and changes of the specific examples illustrated above.
The technical elements described in this specification or the drawings exhibit technical usefulness alone or in various combinations, and are not limited to the combinations described in the claims at the time of filing. In addition, the technology exemplified in the present specification or the drawings achieves a plurality of objects at the same time, and has technical usefulness by achieving one of them.

FIG. 1 is a diagram showing an outline of walking of the legged robot 6. (First embodiment) FIG. 2 is a block diagram showing the functions of the controller 14. (First embodiment) FIG. 3 is a flowchart for explaining processing performed by the controller 14. (First embodiment) FIG. 4 is a diagram showing the transition of the foot placement of the supporting legs of the legged robot 6. (First embodiment) FIG. 5 is a view showing the trajectory of the free leg foot. (First embodiment) FIG. 6 is a diagram showing an outline of the operation of the robot 610. (Second embodiment) FIG. 7 is a view of the outline of the robot 610 as seen from the front. (Second embodiment) FIG. 8 is a block diagram showing the functions of the control unit 690. (Second embodiment) FIG. 9 is a flowchart for explaining processing performed by the control unit 690. (Second embodiment) FIG. 10 is a diagram illustrating how the operation plan of the robot 610 is corrected. (Second embodiment)

Explanation of symbols

6 ... legged robots 7, 8, 9, 10 ... foot trajectory 12 ... trunk 14 ... controller 18 ... head 20 ... right leg 22 ... left leg 210 ... Gait data storage unit 212 ... Joint angle group calculation unit 214 ... Actuator control unit 216 ... Robot mechanical system 218 ... Encoder 220 ... Head position detection device 222 ... Head direction detecting device 224... Supporting leg foot arrangement calculating unit 226 .. landing arrangement calculating unit 228... Correction amount calculating unit 230... Adder 232 .. trunk gait calculating unit 234. .. Landing arrangement correction unit 236... Gravity center speed calculation unit 238... Foot arrangement comparison unit 402, 404, 406, 408, 410, 412, 414... Foot arrangement 416 described by gait data 418, 420 ... foot arrangement 422, 424, 4 6,428 ... scheduled foot arrangement 430, 432, 434 ... after correction of foot placement

602 ... Venue 610 ... Robot 620 ... Moving mechanisms 622a, 622b ... Wheels 624a, 624b ... Actuators 630a, 630b ... Arms 632a, 632b ... Upper arms 634a, 634b ... Lower arm 635a, 635b ... Actuator 636a, 636b ... repellent 638a, 638b ... joint 640 ... head 640a, 640b ... hole 642 ... video camera 650 ... neck 652 ... Motor 660 ... Taiko 670 ... Body part 690 ... Control part 700 ... Podium 702 ... Conductor 710 ... Encoder 712 ... Position sensor 714 ... Direction sensor 730 ... Commander 802 ... Operation pattern storage unit 804 ... Path correction unit 806 ... Operation pattern update unit 808 Actuator control section 810 ... Actuator group 812 ... Instruction recognition section 814 ... Position / direction comparison section 816 ... Path estimation section 818 ... Correction amount calculation section 820 ... Speed calculation section 1002 ..Movement path 1004 described by the action plan ... Measured position 1006 ... Measured direction 1008 ... Movement path 1010 when operating based on the action plan ... Movement path 1012 after correction ... Movement path correction amount

Claims (5)

A method for controlling a walking robot based on gait data,
Measuring the "actual foot placement of the support legs";
A step of identifying “virtual support leg foot placement” in “foot placement path described by gait data” corresponding to “actual support leg foot placement”;
Calculating a deviation between “actual foot placement of support legs” and “virtual foot support placement”;
Based on the deviation, a step of determining the number of steps until returning to the “foot placement path described by the gait data” from “the actual foot placement of the supporting legs”;
A step of calculating “foot placement path when walking based on gait data from“ actual foot placement of supporting legs ”to at least the number of steps;
“Foot placement path when walking based on gait data from“ actual support leg foot placement ”to“ the actual support leg foot placement ”to the above-mentioned number of steps "" So that the flat layout matches the foot layout of the support legs when walking from the "virtual support leg foot layout" in the "foot placement path described by the gait data" to the number of steps. A step of calculating a correction amount for each step from the "actual foot placement of supporting legs" to the foot placement route when walking based on gait data;
Correcting gait data based on the correction amount;
A control method comprising: The method further includes a step of measuring the walking speed of the robot,
2. The control according to claim 1, wherein in the step of determining the number of steps, the number of steps until returning to the “foot placement route described by the gait data” is determined based on the deviation and the walking speed. Method. A robot that walks based on gait data,
Means for measuring "actual foot placement of support legs";
Means for specifying “virtual support leg foot placement” in “foot placement route described by gait data” corresponding to “actual support leg foot placement”;
Means for calculating a deviation between “actual foot placement of support legs” and “virtual foot support placement”;
Based on the deviation, means for determining the number of steps from the “actual foot placement of the supporting legs” to the return to the “foot placement route described by the gait data”;
Means for calculating "foot placement path when walking based on gait data from" actual support leg foot placement "to at least the number of steps;
“Foot placement path when walking based on gait data from“ actual support leg foot placement ”to“ the actual support leg foot placement ”to the above-mentioned number of steps "" So that the flat layout matches the foot layout of the support legs when walking from the "virtual support leg foot layout" in the "foot placement path described by the gait data" to the number of steps. Means for calculating a correction amount for each step from the actual foot placement of the supporting legs to the foot placement route when walking based on gait data;
Means for correcting gait data based on the correction amount;
A robot characterized by comprising: A method for controlling a moving robot based on an operation plan,
Measuring the "actual position and direction" of the robot;
Identifying "virtual position and direction" in "movement path described by the motion plan" corresponding to "actual position and direction";
Calculating a deviation between "actual position and direction" and "virtual position and direction";
Based on the deviation, a step of determining a time from the “actual position and direction” to returning to the “movement path described by the motion plan”;
Calculating a “movement path when moving based on an operation plan from“ actual position and direction ”” until at least the time;
The position and direction of the robot when moving from the “actual position and direction” in the “movement path when moving based on the action plan from the“ actual position and direction ”” for the time described above is described in “the action plan describes. Movement when moving from "Actual position and direction" based on the motion plan so that it matches the position and direction of the robot when moving for the time from "Virtual position and direction" Calculating a correction amount of “route”;
Correcting the motion plan based on the calculated correction amount;
A control method comprising: A robot that moves based on an action plan,
Means for measuring the "actual position and direction" of the robot;
Means for specifying "virtual position and direction" in "movement path described by the motion plan" corresponding to "actual position and direction";
Means for calculating the deviation between "actual position and direction" and "virtual position and direction";
Based on the deviation, means for determining a time until returning from the “actual position and direction” to the “movement path described by the motion plan”;
Means for calculating “a movement path when moving based on an operation plan from“ actual position and direction ”” at least until the time;
The position and direction of the robot when moving from the “actual position and direction” in the “movement path when moving based on the action plan from the“ actual position and direction ”” for the time described above is described in “the action plan describes. Movement when moving from "Actual position and direction" based on the motion plan so that it matches the position and direction of the robot when moving for the time from "Virtual position and direction" A means for calculating the correction amount of the "route",
Means for correcting the motion plan based on the calculated correction amount;
A robot characterized by comprising:

Images