JP7179187B2 - mobile control system - Google Patents

Description

The present invention relates to a control system for a vehicle with two legs.

2. Description of the Related Art Techniques for controlling the movement of a mobile body having two legs (legged mobile body) such as a humanoid robot are conventionally known, for example, in Patent Documents 1 to 5 and the like. In these techniques, a dynamics model of a mobile body is used to generate a desired gait of the mobile body so as to satisfy dynamical required conditions such as ZMP (Zero Moment Point). A movement operation of the moving object is performed.

As a technology for remotely controlling a legged mobile body such as a humanoid robot, conventionally, a control device as seen in Patent Documents 6 and 7, for example, is known. This control device has foot support mechanisms attached to the left and right feet of the operator sitting on the saddle. In the operating device, the operator moves the foot support mechanism so as to perform a walking motion, thereby moving both legs of the moving body by bilateral control. Thereby, the walking motion of the moving object is performed. Also, conventionally, there has been generally known a technique in which a mobile object is moved by an operator operating a remote controller, for example.

JP-A-10-277969 Japanese Patent No. 4246638 U.S. Pat. No. 6,969,965 Japanese Patent No. 4126061 U.S. Pat. No. 8,005,573 JP-A-10-217159 U.S. Pat. No. 5,841,258

When moving a mobile body having two legs, such as a humanoid robot, in various environments by remote control, the mobile body cannot predict the floor surface even if the control as seen in Patent Documents 1 to 5 is performed. Due to the influence of various disturbances such as unevenness and contact with obstacles, it is easy for the robot to lose its posture.

Therefore, in a situation where the attitude of the moving object has collapsed, the occurrence of the situation (hereinafter sometimes referred to as an attitude collapse situation) is required so that the operator can take appropriate countermeasures. It is desired that the operator can quickly recognize the position in real time.

Here, in the control devices disclosed in Patent Documents 6 and 7, force corresponding to the floor reaction force acting on each foot of the moving body acts on the feet of the operator through bilateral control, Also, the saddle tilts according to the tilt of the upper body of the moving body.

However, as in the steering devices disclosed in Patent Documents 6 and 7, when the operator is seated on the saddle, the above force acts on each leg of the operator, or the saddle tilts. However, it is generally difficult for the operator to distinguish and quickly recognize whether or not the forces acting on the feet and the inclination of the saddle are caused by the occurrence of the posture collapse of the moving body. be.

In addition, the control devices disclosed in Patent Documents 6 and 7 require a mechanism for spatially moving the foot support mechanism attached to the operator's foot and a large number of actuators, so the configuration of the device is complicated. It tends to be large and expensive.

SUMMARY OF THE INVENTION The present invention has been made in view of the above background, and an object of the present invention is to provide a control system that enables the operator to quickly and appropriately recognize when the posture of a mobile body collapses. do. Another object of the present invention is to realize such a control system with a simple configuration.

In order to achieve the above objects, the moving object control system of the present invention performs maneuvers to move a slave device which is a legged moving object having an upper body and two legs extending from the upper body. a steering system to obtain
an upper body support attached to the operator's upper body so as to move with the operator as the operator moves; an upper body support drive mechanism applicably attached to the support; and an upper body support drive mechanism;
a slave-side motion target including a target slave leg motion that is a target motion of each leg of the slave device and a target slave body motion that is a target motion of the upper body of the slave device; and upper body support driving of the master device. an operation target determination unit that determines a master-side operation target that is the operation target of the mechanism;
a master-side control unit that controls the operation of the upper body support unit drive mechanism according to the determined master-side operation target;
a slave-side control unit that controls the operation of the slave device according to the determined slave-side operation target,
The lateral position of any one of the upper body support, the operator's upper body, and the operator's center of gravity is defined as the master-side reference part lateral position, and the slave device's upper body and center of gravity of the slave device. When the lateral position of any one of them is defined as the lateral position of the slave side reference part,
The motion target determination unit
determining the target slave leg motion using observations of the state of motion of at least each leg of the operator, such that as the operator moves through the leg motion, the slave device is moved by the leg motion; A first processing unit that determines;
The relationship between the actual lateral position of the master-side reference part and the actual lateral position of the slave-side reference part approaches a state that satisfies a predetermined first target correspondence, and is defined by the slave-side operation target. The master-side reference portion lateral position and the slave-side reference portion lateral position are adjusted so that the target value of the slave-side reference portion lateral direction position to match or approach the actual slave side reference portion lateral position. and a second processing unit that determines the target slave upper body motion and the master motion target using the respective observed values (first invention).

In this specification, the "observed value" of any state quantity such as the motion or force of any object means the value of the state quantity detected by an appropriate detector or sensor, or a constant correlation with the state quantity. An estimated value estimated based on the correlation from the detected value or estimated value of one or more other state quantities having a relationship, or a pseudo value that can be considered to match or nearly match the actual value of the state quantity means a reasonable estimate.
In addition, "vertical direction" means a vertical direction or a substantially vertical direction, and "horizontal direction" means a horizontal direction or a substantially horizontal direction.

According to the first aspect of the invention, when the operator moves on the floor by movement of his/her legs (for example, walking motion) while wearing the upper body support part on his/her upper body, , a target slave leg motion of the slave side motion targets is determined such that the slave device is moved by the motion of its leg.

Further, the relationship between the actual lateral position of the master-side reference part and the actual lateral position of the slave-side reference part approaches a state that satisfies a predetermined first target correspondence relationship, and is defined by the slave-side operation target. Of the slave side motion targets, the desired slave upper body motion and the master side motion target are determined so that the target value of the slave side reference portion lateral position matches or approaches the actual slave side reference portion lateral position. be.

As the first target correspondence relationship, for example, the master-side reference portion lateral position and the slave-side reference portion lateral position match each other, or one is proportional to the other, or one is proportional to the other. A relationship of being represented by the other linear function or the like can be adopted.

Then, the operation control of the slave device is performed according to the slave side motion target including the target slave leg motion and the target slave body motion determined as described above, and according to the master side motion target determined as described above. Operation control of the master device is performed.

In this case, the slave device basically moves in such a manner as to follow the movement of the operator. However, even if the operator is moving in a normal posture without losing his posture, the impact of disturbances such as unevenness on the floor surface of the slave device's movement environment, contact between the slave device and obstacles, etc. , the slave device may lose its posture. As a result, the actual lateral position of the reference portion on the slave side may deviate from the state that satisfies the first target correspondence relationship with the actual lateral position of the reference portion on the master side.

At this time, the target slave body motion is set so that the target value of the lateral position of the slave side reference part is the actual lateral position of the slave side reference part regardless of the lateral position of the operator's upper body or the upper body support part. is determined to match or approximate to . On the other hand, the master-side operation target is determined so that the relationship between the actual master-side reference portion lateral position and the actual slave-side reference portion lateral position approaches the first target correspondence relationship.

Therefore, when the posture of the slave device is disturbed, the actual lateral position of the master-side reference portion is set in the first target relationship with respect to the actual lateral position of the slave-side reference portion of the slave device whose posture is disturbed. The operation of the master device is controlled such that a lateral translational force is exerted on the operator from the upper body support to displace the operator toward a position where .DELTA. For example, when the slave device loses its posture in the forward direction, the operation of the master device is controlled so that a forward translational force acts on the operator's upper body from the upper body support portion.

Therefore, if the posture of the slave device is disturbed while the slave device is moving along with the movement of the operator, the operator must apply a lateral translational force to the upper body of the slave device to break the posture of the slave device. from the upper body support. As a result, the operator can appropriately and quickly recognize that the slave device has lost its posture and in which direction the slave device has lost its posture. Therefore, according to the first invention, when the posture of the slave device (moving body) is disturbed, the operator can quickly and appropriately recognize it.

Supplementally, as the processing of the first processing unit for determining the target slave leg motion, for example, the position of the tip (foot) of each leg of the operator and each leg of the slave device (corresponding to the leg of the operator) are determined. and the position of the tip of the slave leg) satisfies the same target correspondence as the first target correspondence between the lateral position of the reference part on the master side and the lateral position of the reference part on the slave side. A process may be employed to determine a target position for each leg tip of the device as a function of observations of the operator's leg tip position.

Regarding the posture of the tip of each leg of the slave device in the target slave leg motion, for example, the posture of the tip of each leg (foot) of the operator and each leg of the slave device (leg of the operator) The target pose of each leg tip of the slave device is set to the pose of the tip of the leg of the operator so that the relationship between the pose of the tip of the leg and the pose of the leg tip of the operator satisfies a predetermined target correspondence relationship. A process of making decisions according to observations may be employed.

In the first aspect, the master-side motion target determined by the motion target determination unit includes a target lateral position of the upper body support unit, and the second processing unit controls the target lateral position of the upper body support unit. so that the relationship between the lateral position of the master-side reference portion defined by the position and the observed value or target value of the lateral position of the slave-side reference portion satisfies the first target correspondence relationship. a 201st processing unit that determines a target lateral position of the upper body support using an observed value or a target value of the lateral position of the reference portion on the slave side; It is possible to adopt a mode in which the operation control of the upper body support portion drive mechanism is performed so that the actual lateral position of the support portion follows the determined target lateral position (the second aspect of the present invention). ).

According to this, the lateral position of the upper body supporting portion is controlled so as to follow the target lateral position determined as described above with respect to the observed value or the target value of the lateral position of the slave side reference portion. Therefore, it is possible to appropriately apply a lateral translational force from the upper body support section to the operator's upper body in accordance with the collapse of the posture of the slave device.

In the first aspect or the second aspect, the second processing unit causes the target slave to match the target value of the lateral position of the reference portion on the slave side with the observed value of the lateral position of the reference portion on the slave side. A mode is adopted in which a processing unit 202 is configured to determine a target lateral position of the upper body of the slave device in body motion using an observed value of the lateral position of the slave-side reference part. obtain (third invention).

According to this, the target lateral position of the upper body of the slave device is not dependent on the actual lateral position of the reference part of the master side machine, and the lateral position of the slave side reference part defined by the target slave side operation target. A target value is determined to match the actual position. For this reason, if the relationship between the actual lateral position of the reference part on the slave side and the lateral position of the reference part on the master side deviates from the first target correspondence relationship due to the deformation of the posture of the slave device, the deformation of the slave device will be prevented. In a manner that the operator can perceive, the application of lateral translational force from the upper body support to the operator's upper body can be realized quickly.

In the first invention or the second invention, the motion target determination unit defines the target slave floor reaction force, which is the floor reaction force to be applied to the slave device, as the floor reaction force acting on each leg of the operator. a third processing unit that determines using an observed value of the operator's floor reaction force; and a fourth processing that determines a first virtual external force acting on the slave device in a first dynamics model representing dynamics of the slave device. and
The second processing unit determines that a resultant force of an inertial force generated by the slave device due to a desired motion of the slave device defined by the slave-side motion target and a gravitational force acting on the slave device is equal to the determined target slave device. determining a target lateral translational acceleration of the upper body of the slave device so as to balance the resultant force of the floor reaction force and the determined first virtual external force on the first dynamic model; a target lateral position of the slave device's body in the target slave body motion by integrating
deviation between a target value of the slave-side reference lateral position defined by the slave-side motion target and an observed value of the slave-side reference lateral position, and the slave device defined by the slave-side motion target When any one of the deviations between the target value of the inclination of the upper body and the observed value of the inclination is defined as the first deviation, the fourth processing unit causes the first deviation to approach zero In addition, it is also possible to employ a mode in which the first imaginary external force is determined using the calculated value of the first deviation (fourth invention).

According to this, the first virtual external force is determined such that the first deviation approaches zero. Also, the target horizontal translational acceleration of the upper body of the slave device is the resultant force of the inertial force generated by the slave device due to the target motion of the slave device defined by the slave-side motion target and the gravitational force acting on the slave device. , is determined to balance the resultant force of the desired slave floor reaction force and the first virtual external force on the first dynamic model, and the desired lateral position of the upper body of the slave device by integrating this desired translational acceleration. is determined.

Therefore, if the first deviation occurs due to the occurrence of a collapse of the posture of the slave device, etc., it is possible to prevent the actual movement of the upper body of the slave device from deviating greatly from the target body motion of the slave device. In addition, the slave side operation target can be determined. As a result, the slave side operation target can be determined so as to suppress abrupt changes in the operation of the slave device. As a result, the movement of the slave device can be performed smoothly.

Further, the desired lateral position of the upper body of the slave device is such that the desired motion of the slave device including the desired lateral position acts on the first dynamic model in addition to the desired slave floor reaction force and the first imaginary external force. It is determined so that it can be dynamically established in the slave device that has been set. Therefore, even if the slave side movement target cannot be determined in accordance with the actual motion state of the operator due to, for example, a communication failure between the master device and the slave device, the slave device can still be used to some extent. Slave-side action goals can be determined so that appropriate action can be taken.

Supplementally, in the process of the third processing unit for determining the target slave floor reaction force, the target slave floor reaction force to be determined is the target value of some of the constituent elements of the floor reaction force acting on the slave device. good too. For example, only the target position of the total floor reaction force center point (COP) as the total floor reaction force center point (COP) acting on the slave device may be determined in the third process. This also applies to the invention described later.

Further, when determining the target value of the floor reaction force (both or one of the translational force and the moment) acting on each leg of the slave device as the target slave floor reaction force, for example, one of the two legs of the slave device The ratio of the floor reaction force acting on the first leg of the operator to the floor reaction force acting on the first leg of the two legs of the operator (the leg corresponding to the first leg of the slave device), and the The ratio of the floor reaction force acting on the second leg of the two legs to the floor reaction force acting on the second leg of the operator's two legs (the leg corresponding to the second leg of the slave device) is , are predetermined values, among the target slave floor reaction forces, the target values of the floor reaction forces acting on the legs of the slave device are determined using the observed values of the operator floor reaction force. obtain. In this case, for example, the ratio between the total mass of the slave device and the total mass of the operator can be used as the predetermined value for the ratio.

In the first aspect, the second processing section is configured such that the relationship between the target value of the lateral position of the slave-side reference portion and the actual lateral position of the master-side reference portion satisfies the first target correspondence relationship. a processing unit 204 for determining a target lateral position of the slave device's body out of the target slave body motion to achieve a state using the observed master side reference lateral position; so that the degree of deviation of the relationship between the observed value of the master-side reference portion lateral position and the observed value or target value of the slave-side reference portion lateral position from the first target correspondence relationship approaches zero, and a 205th processing unit that determines the master-side operation target using the calculated value of the degree of deviation (a fifth aspect of the invention).

The "degree of deviation" includes, for example, the value of the lateral position of the slave-side reference portion that satisfies the first target correspondence with respect to the observed value of the lateral position of the master-side reference portion, and the value of the lateral position of the slave-side reference portion. deviation from the observed value of the direction position, or the value of the lateral position of the master-side reference part that satisfies the first target correspondence relationship with the observed value of the lateral position of the slave-side reference part, and the lateral direction of the master-side reference part Deviations from position observations, or monotonically varying function values with respect to these deviations, etc. may be used.

According to the fifth aspect, the relationship between the actual lateral position of the master-side reference portion and the actual lateral position of the slave-side reference portion is adjusted to the first target correspondence relationship so that the degree of deviation approaches zero. ), the master side motion goal is determined. For this reason, when the degree of displacement increases due to the occurrence of a collapse of the posture of the slave device, the master is arranged so that a lateral translational force in a direction to reduce the degree of displacement acts on the upper body of the operator from the upper body support portion. Operation control of the device is performed. As a result, the operator can recognize that the posture of the slave device has collapsed.

Further, the above operation control of the master device results in the actual lateral position of the master-side reference portion approaching the position that satisfies the first target correspondence relationship with the actual lateral position of the slave-side reference portion. operated by Then, the target lateral position of the upper body of the slave device is determined as described above using the observed value of the lateral position of the master side datum. Therefore, the desired slave body motion is determined so that the target value of the slave-side reference portion lateral position is brought closer to the actual slave-side reference portion lateral position.

In the fifth aspect, the 205th processing unit sets a desired upper body support lateral reaction force, which is a target value of lateral reaction force received by the operator from the upper body support part, as the master side action target. The master-side control unit causes the actual lateral reaction force that the operator receives from the upper body support part to follow the determined desired upper body support part lateral reaction force. It is possible to employ a mode in which the operation control of the upper body support portion driving mechanism is performed as described above (sixth invention).

According to this, it is possible to determine the desired upper body support reaction force so as to reduce the degree of deviation, and to control the operation of the master device so as to generate the desired upper body support reaction force. As a result, it is possible to appropriately realize that a horizontal translational force capable of reducing the degree of displacement is applied to the operator's upper body from the upper body support portion.

In the above first to sixth inventions, the second processing unit sets the target vertical position of the body of the slave device in the target slave body motion to the body support unit or the operator's body. The observed value of the upper body support or the operator's upper body vertical position is used to satisfy a predetermined second target correspondence with the actual vertical position of the upper body of the slave device. A mode may be employed in which a 206th processing unit is included that determines a target vertical position or determines a target vertical position of the upper body of the slave device to be a predetermined value. 7 invention).

According to this, the actual vertical position of the upper body of the slave device satisfies the predetermined second target correspondence relationship with the upper body support section of the master device or the actual vertical position of the operator's upper body. can be controlled to

The second target correspondence relationship may be, for example, a relationship in which the vertical position of the upper body support section or the operator's upper body and the vertical position of the upper body of the slave device match each other, or one is the other. , or the relationship that one is represented by a linear function of the other, or the like can be adopted.

In the above first to seventh inventions, the second processing unit sets the target posture of the body of the slave device among the target slave body movements to the actual posture of the upper body of the operator. A 207th processing unit that determines a target body posture of the slave device by using an observed value of the body posture of the operator so as to satisfy the third target correspondence relationship of Aspects can be adopted (eighth invention).

According to this, the actual posture of the upper body of the slave device is controlled so as to satisfy a predetermined third target correspondence relationship with the upper body support portion of the master device or the actual posture of the operator's upper body. can be done.

In the eighth invention, the posture of the upper body of the slave device is defined as the posture in the roll direction of the slave device (the direction around the longitudinal axis of the slave device) and the posture in the pitch direction (the direction around the axis in the lateral direction of the slave device). ) and the yaw direction (vertical direction around the axis), or only one or two of these postures. . This also applies to the posture of the operator's upper body. This also applies to other inventions described later.

Further, the third target correspondence relationship may be, for example, a relationship in which the posture of the body of the operator and the posture of the body of the slave device match each other, a relationship in which one is proportional to the other, or a relationship in which one is proportional to the other. is represented by the linear function of the other.

In the above first to seventh inventions, the upper body support section drive mechanism applies a translational force for laterally moving the upper body support section and a rotational force for rotating the upper body support section to the upper body support section. In addition, the upper body support section is configured to apply a rotational force applied from the upper body support section drive mechanism to the upper body of the operator in a state of being attached to the upper body of the operator. It may be configured to be transmitted to the operator's upper body as a force that changes body posture.

In this case, the motion target determination unit observes the operator floor reaction force, which is the floor reaction force acting on each leg of the operator, to determine the desired slave floor reaction force, which is the floor reaction force to be applied to the slave device. configured to include a third processing unit that determines using the value;
The second processing unit determines that a resultant force of an inertia force generated by the slave device due to a desired motion of the slave device defined by the slave-side operation target and a gravitational force acting on the slave device is equal to the determined target slave device. Determining a target angular acceleration of the upper body of the slave device so as to balance the floor reaction force on a second dynamic model representing the dynamics of the slave device, and integrating the target angular acceleration A 208th processing unit for determining a target posture of the body of the slave device among the target slave body motions, an actual posture of the body of the slave device, and an actual posture of the body of the operator. The upper body support portion rotational movement target, which is the operation target of the upper body support portion drive mechanism related to the rotational movement of the upper body support portion, is set so that the relationship between a 210th processing unit that determines using the observed value of the body posture of the slave device or the determined target posture,
It is possible to employ a mode in which the master-side control section is configured to generate a rotational force for rotating the upper body support section in accordance with the determined upper body support section rotational movement target (the ninth aspect of the present invention). ).

According to this, the target angular acceleration of the upper body of the slave device is the resultant force of the inertial force generated by the slave device due to the target motion of the slave device defined by the slave-side motion target and the gravitational force acting on the slave device. is determined to match the target slave ground reaction force determined using the observed operator ground reaction force on a second dynamics model representing the dynamics of the slave device. By integrating this target angular acceleration, the target posture of the body of the slave device is determined. That is, the desired posture of the body of the slave device is dynamically established in the slave device where the desired motion of the sleigh device including the desired posture causes the desired slave floor reaction force to act on the second dynamic model. determined as possible.

Then, the upper body support portion rotational movement target is set so that the relationship between the actual posture of the upper body of the slave device and the actual posture of the upper body of the operator satisfies a predetermined third target correspondence relationship. is determined using the observed or desired body pose of the slave device. Further, the operation of the master device is controlled so as to generate a rotational force for rotating the upper body support section in accordance with the upper body support section rotation target.

As a result, a rotational force acts on the operator's upper body so as to follow the change in the posture of the slave device's upper body and change the posture of the operator's upper body. Therefore, the operator can perceptually recognize the change in the posture of the upper body of the slave device.

Supplementally, when combining the ninth invention with the fourth invention, even if the first dynamics model in the fourth invention and the second dynamics model in the ninth invention are the same dynamics model good. Furthermore, the 203rd processing unit in the fourth invention and the 208th processing unit in the ninth invention are integrated, and by the integrated processing unit, the target angular acceleration of the upper body of the slave device and the target lateral translational acceleration are calculated. Both may be determined using the same kinetic model.

In addition, in the first to seventh inventions, when the upper body support portion drive mechanism is configured in the same manner as in the ninth invention, the motion target determining portion determines the floor reaction force to be applied to the slave device. represents the dynamics of the slave device, and a third processing unit that determines the target slave ground reaction force, which is the floor reaction force acting on each leg of the operator, using observed values of the operator floor reaction force a fifth processing unit that determines a second virtual external force acting on the slave device in a third dynamic model;
The second processing unit determines that a resultant force of an inertial force generated by the slave device due to a desired motion of the slave device defined by the slave-side operation target and a gravitational force acting on the slave device is equal to the determined target slave device. determining a target angular acceleration of the upper body of the slave device so as to balance the resultant force of the floor reaction force and the determined second virtual external force on the third dynamic model; and integrating the target angular acceleration. A 209th processing unit for determining a target posture of the body of the slave device out of the target slave body motion by performing a combination of the posture of the body of the slave device and the posture of the operator's body The upper body support part rotational movement target, which is the movement target of the upper body support part drive mechanism related to the rotational movement of the upper body support part, is set to the slave so that the relationship between the a 210th processing unit for determining using the observed value of the actual posture of the upper body of the device or the determined target posture,
The master-side control section is configured to generate a rotational force for rotating the upper body support section in accordance with the determined upper body support section rotational movement target,
deviation between a target value of the slave-side reference lateral position defined by the slave-side motion target and an observed value of the slave-side reference lateral position, and the slave device defined by the slave-side motion target When any one of the deviations between the target value of the inclination of the upper body and the observed value of the inclination is defined as the first deviation, the fifth processing unit causes the first deviation to approach zero. In addition, it is possible to employ a mode in which the calculated value of the first deviation is used to determine the second virtual external force (a tenth invention).

According to this, the target angular acceleration of the upper body of the slave device is the resultant force of the inertial force generated by the slave device due to the target motion of the slave device defined by the slave-side motion target and the gravitational force acting on the slave device. is the resultant force of the desired slave floor reaction force determined using the observed value of the pilot floor reaction force and the second virtual external force determined using the calculated value of the first deviation, the dynamics of the slave device is determined to be balanced on a third kinetic model representing By integrating this target angular acceleration, the target posture of the body of the slave device is determined. That is, the desired posture of the body of the slave device is the desired motion of the slave device including the desired posture. is determined to be kinetically feasible.

Then, similarly to the ninth invention, the target of the rotation of the upper body support is determined, and the master device is operated so as to generate a rotational force for rotating the upper body support in accordance with the target of the rotation of the upper body support. control is performed.

As a result, a rotational force acts on the operator's upper body so as to follow the change in the posture of the slave device's upper body and change the posture of the operator's upper body. Therefore, the operator can perceptually recognize the change in the posture of the upper body of the slave device.

In addition, since the second virtual external force is determined so as to converge the first deviation to zero, it is possible to prevent a large deviation between the actual movement of the body of the slave device and the target body movement of the slave device. As such, the slave side motion goals can be determined. As a result, it is possible to determine the slave-side motion target so as to suppress abrupt changes in the motion of the slave device, and to smoothly perform the motion of the slave device.

Supplementally, when combining the tenth invention with the fourth invention, even if the first dynamics model in the fourth invention and the third dynamics model in the tenth invention are the same dynamics model good. Further, the 203rd processing unit in the fourth invention and the 209th processing unit in the tenth invention are integrated, and the integrated processing unit calculates the target angular acceleration of the upper body of the slave device and the target lateral translational acceleration. Both may be determined using the same kinetic model.

In the above first to tenth inventions, the motion target determining unit sets the target slave floor reaction force, which is a floor reaction force to be applied to the slave device, to the operator's legs. configured to include a third processing unit that determines using the observed value of the floor reaction force,
The first processing unit determines a basic desired slave leg position/posture, which is a basic desired value of the position and posture of the tip of each leg of the slave device, from the observed values of the motion state of each leg of the operator. A processing unit 101 and a floor reaction force acting on each leg of the slave device for adjusting the basic desired slave leg position/orientation so that the floor reaction force actually acting on the slave device approaches the determined desired slave floor reaction force. a 102nd processing unit that corrects using the observed force values, and calculates a desired slave leg position/orientation, which is a target value of the position and orientation of the tip of each leg of the slave device after correction by the 102nd processing unit. It is also possible to adopt a mode in which it is configured to be determined as the target slave leg motion (the eleventh invention).

According to this, each leg of the slave device is caused to move according to the actual motion state of each leg of the operator, and the floor reaction force actually acting on the slave device does not deviate from the target slave floor reaction force. can determine the target slave leg motion. As a result, the stability of the posture of the slave device can be enhanced.

In addition, in the first to tenth inventions, the motion target determination unit determines the desired slave floor reaction force, which is the floor reaction force to be applied to the slave device, by the floor reaction force acting on each leg of the operator. a third processing unit that determines using a certain observed value of the operator floor reaction force; and so that at least one of the degree of deviation from the first target correspondence relationship and the lateral translational force of the reaction force of the upper body support portion, which is the reaction force that the operator receives from the upper body support portion, approaches zero, A compensating floor reaction force, which is a floor reaction force to be added to the determined desired slave floor reaction force, is calculated from the calculated value of the deviation degree and the observed value of the lateral translational force of the upper body support reaction force. A sixth processing unit that determines using at least one,
The first processing unit determines a basic target slave leg position/posture, which is a basic target value of the position and posture of the tip of each leg of the slave device, from observed values of the actual motion state of each leg of the operator. and a 101st processing unit that adjusts the basic desired slave leg position/orientation so that the floor reaction force actually acting on the slave device approaches a desired value obtained by adding the compensating floor reaction force to the desired slave floor reaction force. and a 103rd processing unit that corrects using the observed value of the floor reaction force acting on each leg of the slave device, wherein the positions and postures of the tips of the legs of the slave device after correction by the 103rd processing unit. A desired slave leg position/orientation, which is a desired value of , is determined as the desired slave leg motion (a twelfth invention).

According to this, each leg of the slave device is caused to move according to the actual motion state of each leg of the operator, and the target slave floor reaction force determined according to the observed value of the operator's floor reaction force is compensated. The desired slave leg position/orientation can be determined so that the floor reaction force added with the floor reaction force can act on the slave device.

In this case, the compensating floor reaction force is determined so that at least one of the degree of deviation and the lateral translational force of the reaction force of the upper body supporting portion approaches zero. Here, when the compensating floor reaction force is determined so that the degree of deviation approaches zero, the relationship between the actual lateral position of the master-side reference portion and the actual lateral position of the slave-side reference portion is the first A desired slave leg position/posture is determined so as to minimize deviation from the target relationship (in other words, to avoid as much as possible the collapse of the posture of the slave device that causes the deviation). The leg of the slave device can be moved according to the leg position and posture.

Further, when the compensating floor reaction force is determined so that the lateral translational force of the body support reaction force approaches zero, the position and posture of the tip of each leg of the slave device are adjusted so that the slave device In a situation where the collapse of the posture can be avoided, it is possible to suppress the application of a force to the operator's upper body from the upper body support portion, which may cause the operator to recognize the collapse of the posture of the slave device.
Therefore, it is possible to prevent a force from acting on the upper body of the operator more frequently than necessary, which may cause the operator to recognize that the posture of the slave device has collapsed.

In the above first to tenth inventions, the motion target determination unit determines the desired slave floor reaction force, which is the floor reaction force to be applied to the slave device, to the desired slave floor reaction force, which is the floor reaction force acting on each leg of the operator. a third processing unit that determines using an observed value of a person's floor reaction force; a target value of the lateral position of the reference part on the slave side defined by the target of movement on the slave side; and an observed value of the lateral position of the reference part on the slave side. and the deviation between the target value of the inclination of the upper body of the slave device defined by the slave side operation target and the observed value of the inclination is defined as the first deviation. and the horizontal translational force of the upper body supporting portion reaction force, which is the reaction force received by the operator from the upper body supporting portion, to approach zero. A compensating floor reaction force, which is a floor reaction force to be added to the desired slave floor reaction force, is at least one of the calculated value of the first deviation and the observed value of the lateral translational force of the upper body support reaction force. and a seventh processing unit that determines using
The first processing unit determines a basic target slave leg position/posture, which is a basic target value of the position and posture of the tip of each leg of the slave device, from observed values of the actual motion state of each leg of the operator. and a 101st processing unit that adjusts the basic desired slave leg position/orientation so that the floor reaction force actually acting on the slave device approaches a desired value obtained by adding the compensating floor reaction force to the desired slave floor reaction force. and a 103rd processing unit that corrects using the observed value of the floor reaction force acting on each leg of the slave device, wherein the positions and postures of the tips of the legs of the slave device after correction by the 103rd processing unit. A desired slave leg position/orientation, which is a desired value of , is determined as the desired slave leg motion (a thirteenth invention).

According to this, each leg of the slave device is caused to move according to the actual motion state of each leg of the operator, and the target slave floor reaction force determined according to the observed value of the operator's floor reaction force is compensated. The desired slave leg position/orientation can be determined so that the floor reaction force added with the floor reaction force can act on the slave device.

In this case, the compensating floor reaction force is determined such that at least one of the first deviation and the lateral translational force of the upper body support reaction force approaches zero. Here, when the compensating floor reaction force is determined so that the first deviation approaches zero, the actual lateral position of the reference portion on the slave side or the actual inclination of the upper body of the slave device deviates from the target value. is determined as much as possible (in other words, as much as possible to avoid collapse of the attitude of the slave device that causes the divergence), and according to the desired slave leg position/orientation, can be used to move the legs of the slave device.

In addition, when the compensating floor reaction force is determined so that the lateral translational force of the body support reaction force approaches zero, the position and posture of the tip of each leg of the slave device are adjusted so that the slave device In a situation where the collapse of the posture can be avoided, it is possible to suppress the force from acting on the operator's upper body from the upper body support portion, which would cause the operator to recognize the collapse of the posture of the slave device.

Therefore, as in the twelfth invention, it is possible to prevent a force from acting more frequently than necessary on the operator's upper body, which would cause the operator to recognize that the posture of the slave device has collapsed.

In the above first to thirteenth inventions, the upper body support drive mechanism includes a moving mechanism configured to move on the floor surface on which the operator moves, and a movement drive of the moving mechanism with respect to the floor surface. a first actuator capable of generating a force, wherein the upper body support section is mounted on the movement mechanism so as to move together with the movement mechanism, and the master side control section controls the movement of the master side. It is possible to employ a mode in which the moving operation of the moving mechanism is controlled by controlling the operation of the first actuator according to a target ( 14th invention).

According to this, since the upper body support part drive mechanism can be moved along with the movement of the operator, the configuration of the upper body support part drive mechanism can be made small even if the operator's movement environment is wide. becomes possible. Further, by controlling the first actuator, it is possible to appropriately control the operation of the body support drive mechanism for controlling the lateral translational force applied to the body support.

Further, in the ninth or tenth invention (an invention in which the upper body support part drive mechanism is configured to apply a rotational force to the upper body support part to rotate the upper body support part), the upper body support part The driving mechanism includes a moving mechanism configured to move on a floor on which the operator moves, a first actuator capable of generating a driving force for moving the moving mechanism with respect to the floor, and the upper body support. and a second actuator capable of generating a rotational force for rotating the upper body support section. It is possible to employ a mode in which the operation of the second actuator is controlled in accordance with the target of rotational movement ( 15th invention).

According to this, as in the fourteenth invention, the upper body support drive mechanism can be moved along with the movement of the operator. It is possible to make the configuration small. Further, by controlling the first actuator and the second actuator, it is possible to appropriately control the operation of the body support driving mechanism for controlling the lateral translational force and the rotational force applied to the body support. It becomes possible. The fifteenth invention may be combined with the eleventh to thirteenth inventions.

In the fourteenth or fifteenth invention, the upper body support drive mechanism includes an elevating mechanism that supports the upper body support so that the upper body support can be raised and lowered with respect to the moving mechanism; and a third actuator capable of generating a driving force for raising and lowering the mechanism, and the master-side control unit further controls the vertical translational force received by the operator from the upper body support unit to be a predetermined value. ( 16th aspect of the invention) . As the "predetermined value" for the translational force in the vertical direction, for example, zero or a value offset from zero so that the translational force is directed upward can be adopted.

According to this, it is possible to appropriately control the vertical translational force received by the operator from the upper body support portion so that the vertical translational force is equal to a predetermined value by controlling the third actuator. .

In the above first to fifteenth inventions, it is also possible to employ a mode in which the upper body support section is attached to the upper body support section drive mechanism so as to be vertically movable ( 17th invention).

According to this, by attaching the upper body support section to the upper body of the operator, the vertical movement of the upper body of the operator can be followed without requiring an actuator for driving the upper body support section in the vertical direction. to move the upper body support portion up and down.

In the first to fifteenth inventions, it is also possible to employ a mode in which the upper body support is attached to the upper body support drive mechanism so as to be able to move elastically in the vertical direction ( 18th invention). invention).

It should be noted that "capable of elastically moving in the vertical direction" means that the upper body supporting portion is provided with a direction to return the upper body supporting portion to the reference position in accordance with the displacement of the upper body supporting portion from the reference position in the vertical direction. It means that a force can be applied to the upper body support.

According to this, it is possible to reduce or eliminate the weight of the upper body support portion acting on the operator's upper body. Alternatively, it is possible to reduce the burden on the operator's legs against the force of gravity acting on the operator.

In the first to eighth inventions, the upper body support portion is rotatable in the roll direction of the operator, in the pitch direction of the operator, or in the roll direction and the pitch direction. It is also possible to employ a mode in which it is attached to the upper body support portion drive mechanism ( 19th invention). The 19th invention can be combined with the 11th to 14th inventions and the 16th to 18th inventions.

According to this, the operator can smoothly tilt the upper body in the roll direction, in the pitch direction, or in the roll and pitch directions during walking without feeling resistance. becomes possible. As a result, the operator can move smoothly by walking.

In addition, a moving body control system of the present invention is a control system capable of performing a control to move a slave device, which is a legged moving body having an upper body and two legs extending from the upper body,
an upper body support attached to the operator's upper body so as to move with the operator as the operator moves; an upper body support drive mechanism applicably attached to the support; and an upper body support drive mechanism;
a motion target determination unit that determines a slave-side motion target that is the motion target of the slave device and a master-side motion target that is the motion target of the upper body support portion driving mechanism of the master device;
a master-side control unit that controls the operation of the upper body support unit drive mechanism according to the determined master-side operation target;
a slave-side control unit that controls the operation of the slave device according to the determined slave-side operation target,
The lateral position of any one of the upper body support, the operator's upper body, and the operator's center of gravity is defined as the master-side reference part lateral position, and the slave device's upper body and center of gravity of the slave device. When the lateral position of any one of them is defined as the lateral position of the slave side reference part,
The motion goal determination unit
The slave side motion target is determined so as to move the slave device along with the movement of the operator, and the actual lateral position of the master side reference portion and the actual lateral position of the slave side reference portion are determined. determining the master-side motion goal to cause the upper body support to exert a lateral translational force on the operator in a direction that reduces the deviation of the relationship between It is possible to adopt the aspect of being configured (the twentieth invention).

According to the twentieth invention, The relationship between the actual lateral position of the master-side reference portion and the actual lateral position of the slave-side reference portion deviates from the predetermined first target correspondence relationship due to the collapse of the posture of the slave device. Lateral translational forces directed to reduce it are then acting on the operator from the upper body support. Therefore, the operator can appropriately and quickly recognize that the slave device has lost its posture and in which direction the slave device has lost its posture. Therefore, according to the twentieth aspect, when the posture of the slave device (moving body) has collapsed, the operator can quickly and appropriately recognize that fact.

FIG. 2 is a perspective view showing the configuration of a slave device as a moving object according to the embodiment of the present invention; FIG. 2 is a block diagram showing a configuration related to operation control of a slave device in the first embodiment; FIG. FIG. 2 is a perspective view showing the configuration of a master device according to first to sixth embodiments; FIG. 4 is a perspective view showing a master device and an operator in first to sixth embodiments; FIG. 2 is a block diagram showing a configuration related to operation control of the master device in the first embodiment; FIG. 6 is a flowchart showing processing of a master control unit shown in FIG. 5; FIG. 6 is an explanatory diagram relating to processing of the master control unit shown in FIG. 5; FIG. 3 is a flowchart showing processing of a slave operation target determination unit shown in FIG. 2; FIG. FIG. 11 is a block diagram showing a configuration related to operation control of the master device in the second embodiment; FIG. 11 is a block diagram showing a configuration related to operation control of a slave device in the second embodiment; FIG. 11 is a block diagram showing a configuration related to operation control of a master device according to the third embodiment; FIG. 11 is a block diagram showing a configuration related to operation control of a slave device according to the third embodiment; FIG. 13 is a flowchart showing processing of a slave operation target determination unit shown in FIG. 12; FIG. FIG. 14 is an explanatory diagram of a dynamics model used in STEP 18a of FIG. 13; FIG. 11 is a block diagram showing a configuration related to operation control of a slave device in the fourth embodiment; Block diagram showing the configuration related to the operation control of the master device in the fifth embodiment 17 is a flow chart showing the processing of the master-control unit shown in FIG. 16; FIG. 11 is a block diagram showing a configuration related to operation control of a slave device according to the fifth embodiment; FIG. 19 is a flowchart showing processing of a slave operation target determination unit shown in FIG. 18; FIG. FIG. 12 is a block diagram showing a configuration related to operation control of a slave device in the sixth embodiment; FIG. 21A is a plan view of the configuration of main parts of the master device in seventh to twelfth embodiments, and FIG. 21B is a side view of the configuration of the main parts. FIG. 12 is a block diagram showing the configuration of a main part relating to operation control of the master device in seventh to twelfth embodiments; FIG. 12 is a block diagram showing a configuration related to operation control of a slave device in the seventh embodiment; FIG. 24 is a flowchart showing processing of a slave operation target determination unit shown in FIG. 23; FIG. FIG. 25 is an explanatory diagram of a dynamics model used in STEP19 of FIG. 24; FIG. 12 is a block diagram showing a configuration related to operation control of a slave device in the eighth embodiment; FIG. 21 is a block diagram showing a configuration related to operation control of a slave device in the ninth embodiment; FIG. 28 is a flowchart showing processing of a slave operation target determination unit shown in FIG. 27; FIG. FIG. 21 is a block diagram showing a configuration related to operation control of a slave device according to the tenth embodiment; FIG. 21 is a block diagram showing a configuration related to operation control of a slave device in the eleventh embodiment; FIG. 31 is a flowchart showing processing of a slave operation target determination unit shown in FIG. 30; FIG. FIG. 21 is a block diagram showing a configuration related to operation control of a slave device in the twelfth embodiment; The figure which shows the structure of the principal part of the raising/lowering mechanism in 14th Embodiment. The figure which shows the other structural example of a master device. The figure which shows the other structural example of a master device.

[First embodiment]
A first embodiment of the present invention will be described below with reference to FIGS. 1 to 8. FIG. In this embodiment, a control system in which the slave device 1 illustrated in FIG. 1 is controlled by the master device 51 illustrated in FIGS. 3 and 4 will be described as an example.

[Slave device configuration]
Referring to FIG. 1, a slave device 1 of the present embodiment is, for example, a humanoid legged mobile object, and includes an upper body 2 as a base and a pair of left and right (2 a pair of left and right (two) arms 10, 10 extending from the upper part of the body 2; and a head 17 attached to the upper end of the body 2. Note that in FIG. 1, the direction perpendicular to the paper surface is the front-rear direction of the slave device 1 .

Each leg 3 has a thigh portion 4, a lower leg portion 5 and a foot portion 6 as its component links, and the thigh portion 4, the lower leg portion 5 and the foot portion 6 are sequentially connected from the upper body 2 side. , through a hip joint mechanism 7 , a knee joint mechanism 8 , and an ankle joint mechanism 9 . Each of the joint mechanisms 7, 8, 9 of each leg 3 is composed of one or more joints (not shown).

For example, as the joint, a joint having a known structure having a rotational degree of freedom of one axis (a joint composed of two members connected so as to be relatively rotatable about one rotational axis) can be used. The hip joint mechanism 7, the knee joint mechanism 8, and the ankle joint mechanism 9 of each leg 3 are composed of, for example, three joints, one joint, and two joints, respectively. Thereby, each leg 3 is configured such that its foot 6 has six degrees of freedom of movement with respect to the upper body 2 .

Each arm 10 has an upper arm portion 11, a forearm portion 12 and a hand portion 13 as links of its constituent elements. The joint mechanism 14 , the elbow joint mechanism 15 , and the wrist joint mechanism 16 are connected to each other. Each of the joint mechanisms 14, 15, 16 of each arm 10 is composed of one or more joints (not shown).

For example, the shoulder joint mechanism 14, the elbow joint mechanism 15, and the wrist joint mechanism 16 of each arm 10, like each leg 3, use joints with one axis of rotational freedom, each having three joints, It can consist of one joint or two joints. Thereby, each arm 10 is configured such that its hand portion 13 has 6 degrees of freedom of motion with respect to the upper body 2 .

Also, the hand portion 13 of each arm 10 is configured to be able to perform a desired task such as gripping an object. For example, each hand unit 13 may be configured by a clamp mechanism, a plurality of finger mechanisms capable of performing operations similar to those of human fingers, tools, or the like.

The head 17 is attached to the upper end of the upper body 2 via a neck joint mechanism 18 . The neck joint mechanism 18 may be configured with one or more joints to have, for example, one, two, or three rotational degrees of freedom. Alternatively, head 17 may be fixed to the upper end of upper body 2 .

The slave device 1 configured as described above moves each leg 3 so that the foot portion 6 of each leg 3 alternately moves in the air and lands on the floor (grounding) in the same way as a human being walks. By moving it, it can move (walk) on the floor. In this specification, the "floor surface" is not limited to the floor surface in the usual sense, and may be the ground surface, the road surface, or the like. Further, the "floor surface" is not limited to a flat surface, and may be a surface having irregularities, steps, or the like.

Supplementally, each of the legs 3 and arms 10 of the slave device 1 may be configured to have, for example, seven or more degrees of freedom of motion, instead of being limited to six degrees of freedom of motion. Moreover, each of the leg 3 and the arm 10 may include a direct-acting joint instead of a rotary joint. Also, the slave device 1 is not limited to a mobile body having two arms 10, 10, but may be a mobile body having one or three or more arms, or a mobile body having no arms. Also, the slave device 1 may be a moving body without the head 17 . Also, the upper body 2 of the slave device 1 may be configured, for example, to have one or more joints between the upper and lower parts so that the upper and lower parts can be displaced relative to each other. .

Referring to FIG. 2, the slave device 1 further includes a joint actuator 21 that drives each joint. The slave device 1 also includes a joint displacement detector 22 for detecting the displacement (rotation angle in this embodiment) of each joint as detectors for detecting the operating state of the slave device 1, and the upper body 2 and a floor reaction force detector 25 for detecting the floor reaction force received from the floor on which each leg 6 touches the ground.

A joint actuator 21 and a joint displacement detector 22 are provided for each joint of the slave device 1 , and a floor reaction force detector 25 is provided for each leg 3 . However, in FIG. 2, only one of each of the joint actuator 21, the joint displacement detector 22, and the floor reaction force detector 25 is shown representatively.

In the following description, the detected value or estimated value of any state quantity such as position, orientation, force, etc. may be generically referred to as observed value. In addition, the name of any state quantity prefixed with "actual" means the actual value of the state quantity or its observed value, and the name of any state quantity prefixed with "target". What is attached means the target value of the state quantity.

Each joint actuator 21 is configured by, for example, an electric motor, and is connected to a joint to be driven so as to drive the joint via an appropriate power transmission mechanism such as a speed reducer (not shown). Note that each joint actuator 21 is not limited to an electric motor, and may be configured by, for example, a hydraulic actuator. Further, each joint actuator 21 is not limited to a rotary actuator, and may be a linear actuator.

Each joint displacement detector 22 is composed of, for example, a rotary encoder, a resolver, a potentiometer, or the like, and detects the joint (or is connected to a rotating member that rotates in conjunction with the displacement of the

The body posture detector 23 includes, for example, an acceleration sensor 23a capable of detecting triaxial acceleration (three-dimensional translational acceleration vector) and an angular velocity sensor 23b capable of detecting triaxial angular velocity (three-dimensional angular velocity vector). and is mounted on the body 2 so as to detect acceleration and angular velocity occurring in the body 2 .

In this embodiment, the body posture detector 23 detects the actual body tilt, which is the tilt of the actual posture of the body 2, from the acceleration and angular velocity detected by the acceleration sensor 23a and the angular velocity sensor 23b. It includes a posture estimation unit 23c that executes estimation processing. The posture estimator 23c is composed of an electronic circuit unit including, for example, a microcomputer or processor, memory, an interface circuit, and the like. Then, the posture estimator 23c estimates the actual body inclination from the observed values of the acceleration and angular velocity by a known technique such as the strapdown method, and outputs the estimated value (observed value).

Here, supplementing the "attitude", the "attitude" of an arbitrary object such as the body 2 of the slave device 1 means the spatial orientation of the object. In addition, among the "postures" of an object, the posture of an object in a direction (so-called yaw direction) around an axis in the vertical direction (vertical direction or almost vertical direction) is referred to as the "orientation" of the object. The pose of an object in a direction (eg, pitch or roll) about an axis of direction (horizontal or near-horizontal) is called the "tilt" of the object. These "orientation" and "inclination" are expressed using, for example, Euler angles.

Therefore, the actual body inclination estimated by the posture estimator 23c of the body posture detector 23 is, in other words, the actual posture of the body 2 in the direction around the lateral axis. In this case, the actual body inclination estimated by the posture estimating unit 23c is, more specifically, two horizontal axes that are perpendicular to each other (for example, the X and Y axes of the slave-side global coordinate system Cgs, which will be described later). A set of inclinations of the upper body 2 in the circumferential direction.

Supplementally, the attitude estimation unit 23c may be mounted on the slave device 1 at a position distant from the mounting positions of the acceleration sensor 23a and the angular velocity sensor 23b. Also, the attitude estimation unit 23c

may be included in the slave control unit 31 to be described later. Moreover, the posture estimation unit 23c can be configured to estimate the posture including not only the inclination of the body 2 of the slave device 1 but also the orientation of the body 2 . In addition, the body posture detector 23 detects the inclination of the body 2 (or the posture of the body 2 including its orientation) by known motion capture processing, for example, using the image captured by the slave device 1 with a camera. can be estimated. In addition, as a method of estimating the inclination (or posture including orientation) of the body 2, various known methods capable of estimating the self-position and posture of any object can be employed.

Each floor reaction force detector 25 is composed of, for example, a 6-axis force sensor capable of detecting 3-axis translational force (3-dimensional translational force vector) and 3-axis moment (3-dimensional moment vector). is attached to the slave device 1 so as to detect the actual foot floor reaction force, which is the floor reaction force (translational force and moment) actually acting on the foot portion 6 of the foot. For example, as shown in FIG. 1, each floor reaction force detector 25 is interposed between the foot portion 6 of each leg 3 and the ankle joint mechanism 9 . In this specification, "moment" means "moment of force" unless otherwise specified.

The slave device 1 further includes a slave control unit 31 having a function of executing a process for determining the motion target of the entire slave device 1, and a joint control unit having a function of controlling the operation of each joint via the joint actuator 21. 32, and a communication device 33 for performing wireless communication with a master control unit 81, which will be described later. These are mounted at arbitrary appropriate locations of the slave device 1, such as the upper body 2 and the like.

Each of the slave control unit 31 and the joint control unit 32 is configured by an electronic circuit unit including, for example, a microcomputer, a memory, an interface circuit, and the like. Observation data detected or estimated by each of the body posture detector 23 and each floor reaction force detector 25 is input to the slave control unit 31, and command information regarding the operation of the slave device 1 is sent to the master control unit 31, which will be described later. It is input from the unit 81 via the communication device 33 . Note that the observation data and command information input to the slave control unit 31 may be filtered values that have undergone filtering processing such as a low-pass filter.

The slave control unit 31, as a function realized by either or both of the implemented hardware configuration and the program (software configuration), controls the slave device 1 based on command information and the like input from the master control unit 81. a slave motion goal determination unit 31a that determines a basic motion goal; a composite compliance motion determination unit 31b that appropriately modifies, among the basic motion goals, a motion target related to the motion of each leg 3 using compliance control processing; A joint displacement determination unit 31c that determines a target joint displacement, which is a target value of the displacement (rotation angle) of each joint, according to the movement target of the slave device 1, and an actual lateral position of the body 2 of the slave device 1 and a lateral body position estimator 31d for estimating the actual slave lateral body position.

In the present embodiment, the basic motion goals determined by the slave motion goal determining unit 31a include a desired slave body motion that is a desired motion of the body 2, a desired slave leg motion that is a desired motion of each leg 3, A target slave floor reaction force, which is a target value of the floor reaction force acting on the slave device 1 from the floor surface, is included.

In this case, the desired slave body motion is represented in detail by the time series of the desired body position/posture, which are the target values of the set of positions and postures of the body 2, and the desired slave leg motion is represented in detail by each leg 3 is represented by the time series of the desired foot position/posture, which is the desired value of the set of positions and postures of the foot 6 of No. 3.

Here, in this specification, the "position" of an arbitrary object such as the upper body 2 of the slave device 1 means the spatial position of a preset (defined) representative point of the object. The "position and orientation" of an arbitrary object means a set of the position and orientation of the object.

Further, unless otherwise specified, the target positions and target postures of the body 2 and each leg 6 of the slave device 1 are global coordinates that are set (defined) for the operating environment of the slave device 1 by design. It is expressed as target values of position and orientation as viewed in the system (world coordinate system). The global coordinate system (hereinafter referred to as the slave-side global coordinate system Cgs) is, for example, a three-axis orthogonal coordinate system Cgs (the vertical direction is the Z-axis direction, the Z-axis is orthogonal to the Z-axis, and two orthogonal coordinate systems are orthogonal to each other), as shown in FIG. An XYZ coordinate system, in which two horizontal directions are the X-axis direction and the Y-axis direction, is used. The position of the origin of the slave-side global coordinate system Cgs and the orientations of the X-axis direction and the Y-axis direction can be updated (reset) at any time as the slave device 1 moves.

In this case, the Z-axis direction of the slave-side global coordinate system Cgs is the vertical direction, and the X-axis direction and the Y-axis direction are horizontal directions. represents the orientation of the body 2 viewed in the slave side global coordinate system Cgs, and the posture of the body 2 in the directions around the X and Y axes is the orientation of the body 2 viewed in the slave side global coordinate system Cgs. Represents tilt. The same applies to the posture (orientation and inclination) of each leg 6 .

Also. The "horizontal position" of an arbitrary object such as the body 2 of the slave device 1 as viewed in the slave-side global coordinate system Cgs is defined by the X-axis and the Y-axis, which are the horizontal coordinate axes of the slave-side global coordinate system Cgs. It is expressed as a set of positions in the respective coordinate axis directions (positions in the X-axis direction and positions in the Y-axis direction).

In the present embodiment, the target slave floor reaction force among the basic motion targets determined by the slave motion target determining unit 31a is a target value of the total floor reaction force acting on the slave device 1 from the floor surface. A floor reaction force, a desired total floor reaction force center point as a desired position of the center of pressure (COP) of the desired total floor reaction force, and a target floor reaction force acting on each leg 6 of the slave device 1 from the floor surface. and the desired foot floor reaction force center point as the target position of the center of pressure (COP) of the desired foot floor reaction force at each foot 6. expressed. The "total floor reaction force" is the resultant force of the floor reaction forces acting on the two legs 6, 6 of the slave device 1, respectively. Further, when the slave device 1 receives no external force other than from the floor, the desired total floor reaction force central point is the desired position of ZMP (Zero Moment Point).

Although not shown in FIG. 2, the slave device 1 of the present embodiment includes an arm 10 and a head 17 that are movable with respect to the upper body 2. Therefore, the slave motion goal determination unit 31a further includes: It also includes the ability to determine a desired slave arm motion, which is the desired motion of each arm 10 , and a desired slave head motion, which is the desired motion of the head 17 .

In this case, the target slave arm motion is, for example, when each arm 10 is a target position/posture relative to the body 2 of the hand unit 13 (an apparent target position/posture in the local coordinate system set for the body 2). Represented by a series. Similarly, the desired slave head motion is represented by, for example, a time series of the desired position and orientation of the head 17 relative to the body 2 (the apparent target position and orientation in the local coordinate system set for the body 2). be done.

Alternatively, the target slave arm motion may comprise, for example, a time series of target joint displacements for each joint of each arm 10. Similarly, the target slave head motion may be configured, for example, for each joint of the neck joint mechanism 18. may be composed of a time series of target joint displacements of

The joint controller 32 receives the actual joint displacement (observed value) of each joint detected by each joint displacement detector 22 and the target joint displacement of each joint determined by the slave controller 31 . be. Then, the joint control unit 32 controls each joint actuator 21 so that the actual joint displacement follows the target joint displacement for each joint by the function realized by the implemented hardware configuration and program (software configuration). do.

Specifically, the joint control unit 32 uses the deviation between the target joint displacement and the actual joint displacement detected by the joint displacement detector 22 for each joint, and converges the deviation to zero according to the feedback control law. A target driving force of the joint actuator 21 is determined so as to Then, the joint control unit 32 controls the joint actuator 21 so that the joint actuator 21 outputs the determined target driving force. In this case, as the feedback control law, a known feedback control law such as P law (proportional law), PD law (proportional/differential law), PID law (proportional/integral/differential law) can be used.

[Configuration of master device]
Next, the configuration of the master device 51 will be described with reference to FIGS. 3 to 5. FIG. In the following description, the "front-rear direction", the "left-right direction", and the "vertical direction" of the master device 51 are respectively the Xm-axis direction and the Ym-axis of the three-axis orthogonal coordinate system Cm illustrated in FIG. 3 or FIG. direction, the Zm-axis direction. The coordinate system Cm is a local coordinate system set (defined) for the master device 51, and hereinafter referred to as the master coordinate system Cm.

3 and 4, the master device 51 includes a moving mechanism 52 capable of moving on the floor surface of the operation environment of the operator P (shown in FIG. 4) who operates the slave device 1, and an upper body support portion 65 to be mounted on the upper body. It should be noted that the operator P will be referred to as an operator P in the following description.

The moving mechanism 52 includes a base 53 and a plurality of moving ground portions 54 attached to the base 53. The plurality of moving ground portions 54 are arranged with a gap between the base 53 and the floor surface. grounded on the floor. In the illustrated example, the base 53 is bifurcated (substantially U-shaped) when viewed from above, but the shape of the base 53 may be any shape. OK.
The moving mechanism 52 includes, for example, four moving ground portions 54(1) and 54(1), 5
4(2), 54(3), 54(4). Two moving ground portions 54(1) and 54(4) are attached to the left and right sides of the front portion of the base 53, and two moving ground portions 54(2) are attached to the left and right sides of the rear portion of the base 53. , 54(3)) are attached.

3 and 4, each moving grounding portion 54 is simply illustrated as a wheel, but more specifically, it is constructed so as to be able to move in all directions on the floor surface while being in contact with the floor surface. ing. Specifically, each moving grounding portion 54 has the same structure as the main wheels described in Japanese Patent Application Laid-Open No. 2013-237329, for example. Therefore, a detailed description of the configuration of each moving grounding portion 54 and its driving mechanism is omitted in this specification.

Although detailed illustration is omitted in the moving mechanism 52 having such a moving grounding portion 54, two electric motors 55a and 55b (shown in FIG. 5) as power sources for movement are provided for each moving grounding portion 54. A movement drive mechanism 55 (shown in FIG. 5) is mounted. Note that FIG. 5 representatively shows only the movement drive mechanism 55 corresponding to one movement grounding portion 54 .

The movement driving mechanism 55 corresponding to each moving grounding portion 54, as described in Japanese Patent Application Laid-Open No. 2013-237329, transmits power from two electric motors 55a and 55b to the moving grounding portion 54. , the moving grounding portion 54 can be moved in all directions.

In this case, each moving contact portion 54 has a moving speed vector whose speed component in the longitudinal direction (Xm-axis direction) of the master device 51 is proportional to the sum of the rotational speeds of the two electric motors 55a and 55b. , and the two electric motors 55a and 55b are driven so that the speed component in the left-right direction (Ym-axis direction) becomes a speed proportional to the difference between the respective rotation speeds of the two electric motors 55a and 55b.

In addition, the configuration of each moving grounding portion 54 that can move in all directions is not limited to that described in JP-A-2013-237329. good too. Further, the number of moving grounding portions 54 provided in the moving mechanism 52 is not limited to four, and may be, for example, three or five or more. Further, the power source for each moving grounding portion 54 is not limited to the electric motors 55a and 55b, and a hydraulic motor, for example, can be used.

An upper body support portion 65 is attached to the base 53 via a lifting mechanism 60 . The elevating mechanism 60 has a post 61 erected upward from the central portion (the central portion in the left-right direction) of the rear portion of the base 53, and can move vertically (elevate) with respect to the post 61. and a slide member 62 assembled in such a manner.

As a guide mechanism for guiding the movement of the slide member 62 with respect to the column 61, for example, a guide rail 61a extending in the vertical direction is attached to the front portion of the column 61. As shown in FIG. The slide member 62 is engaged with the guide rail 61a so as to move up and down along the guide rail 61a. Note that the guide mechanism may be different from that described above.

Although detailed illustration is omitted, the master device 51 is equipped with a slide drive actuator 66 (shown in FIG. 5) that is an actuator for moving the slide member 62 up and down with respect to the column 61 . The slide drive actuator 66 is composed of, for example, an electric motor.

The slide drive actuator 66 applies a driving force for moving the slide member 62 up and down with respect to the post 61 to the slide member 62 via a rotation/linear motion converting mechanism (not shown) such as a ball screw mechanism. It is attached to the post 61 or the slide member 62 so as to move the slide member 62 up and down. Note that the slide drive actuator 66 is not limited to an electric motor, and for example, a hydraulic motor or a hydraulic cylinder can also be used.

In the present embodiment, the upper body support portion 65 is configured to be able to follow a predetermined portion of the upper body of the operator P, for example, the outer periphery of the waist, from the back side. For example, the upper body support portion 65 is configured by a plate-like member curved into a substantially semi-arc shape (or U-shape). The upper body support portion 65 is attached to the slide member 62 via a support shaft 63 and a force detector 64 .

More specifically, the support shaft 63 is attached to the slide member 62 via the force detector 64 with its axis directed in the front-rear direction (Xm-axis direction). The force detector 64 (hereinafter referred to as the upper body strength detector 64) detects the actual body support reaction force, which is the reaction force (contact reaction force) that the waist of the operator P actually receives from the upper body support portion 65. , and like the floor reaction force detector 25 of the slave device 1, it is composed of, for example, a 6-axis force sensor.

The upper body support portion 65 is attached to the support shaft 63 at the central portion between both ends thereof. In this case, the upper body support part 65 is attached to the support shaft 63 so as to freely rotate about the axis of the support shaft 63 (in other words, in the roll direction) with respect to the slide member 62 and the upper body force detector 64 . supported by

When the operator P operates the slave device 1, the upper body support part 65 is arranged along the waist of the operator P's upper body from the back side, as shown in FIG.

A flexible belt 65x (indicated by a chain double-dashed line in FIG. 4) is connected to both ends of the upper body support section 65 so as to extend along the front side circumference of the waist of the operator P. be.

As a result, the upper body support part 65 is attached to the waist of the operator P via the belt 65x so that the upper waist of the operator P is surrounded by the upper body support part 65 and the belt 65x. In this case, the upper body support part 65 is arranged in a state in which the front-rear direction (Xm-axis direction) and the left-right direction (Ym-axis direction) of the master device 51 face substantially the same directions as the front-rear direction and the left-right direction of the operator P, respectively. , is attached to the waist of the operator P.

Further, the height (vertical position) of the body support part 65 can be adjusted by appropriately moving the slide member 62 up and down. An elastic member such as a pad (not shown) is attached to the inner peripheral surface of the upper body support portion 65, and the elastic member is brought into contact with the waist of the operator P. As shown in FIG.

With the upper body support part 65 attached to the waist of the operator P in this way, the operator P can walk on the floor by walking (moving each leg in the air and landing (grounding) on the floor alternately). As it moves on the surface, the upper body support section 65 and the moving mechanism 52 are brought into a state of being able to move together with the operator P. In this case, the operator P can tilt his upper body forward, backward, leftward, and rightward. Further, the upper body force detector 64 detects the reaction force (actual body support reaction force) that the waist of the operator P receives from the upper body support portion 65 .

Referring to FIG. 5, the master device 51 includes, in addition to the upper body strength detector 64, a motor rotation detector as a detector for detecting the drive state of each movement grounding portion 54 by each movement drive mechanism 55. 56, a slide displacement detector 67 for detecting the displacement (vertical position) of the slide member 62, an operator movement detector 70 for detecting the movement state of the operator P, and a floor at each foot of the operator P. and a floor reaction force detector 75 for detecting the floor reaction force acting from the surface.

The motor rotation detector 56 detects, for example, the actual rotation angles of the rotation shafts (or rotating members that rotate in conjunction with the rotation shafts) of the electric motors 55a and 55b for each movement drive mechanism 55. , is a detector for detecting as a state quantity indicating the driving state of the movement driving mechanism 55 .

The motor rotation detector 56 is composed of, for example, a rotary encoder, a resolver, a potentiometer, and the like. It is connected to the. Although the motor rotation detector 56 is provided for each of the electric motors 55a and 55b of each movement drive mechanism 55, only one motor rotation detector 56 is representatively illustrated in FIG. .

The slide displacement detector 67 is composed of, for example, a known contact-type or non-contact-type displacement sensor. installed in the For example, when the power transmission mechanism from the slide drive actuator 66 to the slide member 62 is configured such that the displacement of the slide member 62 is proportional to the rotation angle of the rotation shaft of the slide drive actuator 66, the A detector that can detect the rotation angle of the rotation shaft of the slide drive actuator 66 (or a rotating member that rotates in conjunction with this) can also be used as the slide displacement detector 67 . In this case, the slide displacement detector 67 may be a detector similar to the motor rotation detector 56 described above.

Operator motion detector 70 includes one or more cameras 71 in this embodiment. The camera 71 is mounted, for example, on a post 73 erected on the base 53 as shown in FIGS. In this case, a plurality of cameras 71 are mounted on the column 73 so that the movement of the upper body and the movement of each leg of the operator P who wears the upper body support section 65 on the waist can be photographed by one or more cameras 71 respectively. installed. Note that the arrangement position of the camera 71 is not limited to the position shown in FIG. 1, and may be another position. Further, the cameras 71 may be arranged at a plurality of locations around the operator P wearing the upper body support portion 65 . In addition, markers may be attached to the operator P's upper body and legs.

The operator motion detector 70 includes a motion estimator 72 that executes processing for estimating the motion state of the operator P from the image captured by the camera 71 . The motion estimator 72 is composed of, for example, an electronic circuit unit including a microcomputer or processor, a memory, an interface circuit, etc., and is mounted at an arbitrary position of the master device 51 such as the base 53 . Note that the motion estimation unit 72 may be included in the master control unit 81, which will be described later.

The motion estimating unit 72 estimates the motion state of the operator P by, for example, executing known motion capture processing from the captured image input from the camera 71, and outputs data indicating the estimated motion state. . In this case, in this embodiment, the motion state estimated by the motion estimating unit 72 is the inclination (actual operator body inclination) of the actual operator body posture, which is the actual posture of the operator P's body, and the operator P actual operator foot position and orientation, which is the actual position and orientation of each foot of the operator.

In this case, the actual operator's upper body inclination and the actual operator's foot position/posture estimated by the motion estimating unit 72 are in a global coordinate system (world coordinate system) set (defined) by design for the operating environment in which the operator P moves. ) are the actual operator's upper body inclination and the actual operator's foot position/posture.

The global coordinate system (hereinafter referred to as the master-side global coordinate system Cgm) is, for example, a three-axis orthogonal coordinate system Cgm (the vertical direction is the Z-axis direction, perpendicular to the Z-axis, and mutually An XYZ coordinate system in which two orthogonal horizontal directions are the X-axis direction and the Y-axis direction) is used. As with the slave-side global coordinate system Cgs, the position of the origin of the master-side global coordinate system Cgm and the directions in the X-axis direction and the Y-axis direction change as the operator P wearing the upper body support section 65 moves. May be updated at any time.

In addition to the actual operator's body inclination and the actual operator's foot position/orientation, the motion estimator 72 also calculates the actual operator's body orientation, which is the actual orientation of the operator's P's upper body as viewed in the master-side global coordinate system Cgm. It is also possible to configure it so that it can be estimated.

The actual operator body inclination and the actual operator foot position/posture estimated by the motion estimating unit 72 are, for example, the actual operator body inclination as viewed in the master coordinate system Cm, which is a local coordinate system set for the master device 51 . It may be tilt and actual operator foot position and posture. In this case, the observed values of the actual operator's upper body inclination and the actual operator's foot position/posture viewed in the master coordinate system Cm are used as the actual position and orientation of the upper body support section 65 estimated by the master control section 81, which will be described later. Estimated values of actual operator body inclination and actual operator foot position/posture viewed from master-side global coordinate system Cgm are obtained by performing coordinate transformation according to (position and orientation viewed from master-side global coordinate system Cgm). can be done.

Supplementally, the method for estimating the motion state of the upper body and each leg of the operator P by the operator motion detector 70 may be a method other than motion capture using images captured by the camera 71 . For example, inertial sensors including an acceleration sensor and an angular velocity sensor may be attached to the upper body and each leg of the operator P, respectively. Then, from the acceleration and angular velocity detected by this inertial sensor, the motion state of the operator's upper body and each leg (actual operator's upper body inclination, actual operator's upper body orientation, and It is also possible to estimate the actual operator foot position and posture. In addition, as a method of estimating the motion state of the upper body and each leg of the operator P, various known methods capable of estimating the self-position and posture of an object can be used.

Also, for example, each leg may be equipped with a joint displacement sensor capable of detecting the displacement of each joint (hip joint, knee joint, and ankle joint) of each leg of the operator P. Then, the position and orientation of each leg relative to the upper body of the operator P may be estimated from the detected displacement values of the joints of each leg using the rigid body link model of each leg. Then, the observed values of the relative positions and orientations of the feet of the operator and the observed values of the actual positions and orientations of the upper body of the operator P (actual operator body positions and orientations) estimated by an appropriate method such as motion capture. , the actual operator's foot position and orientation may be estimated.

The floor reaction force detector 75, like the floor reaction force detector 25 of the slave device 1, is composed of, for example, a 6-axis force sensor. The floor reaction force detector 75 detects the actual floor reaction force (translational force and moment) acting on each foot of the operator P so that the actual operator P foot floor reaction force can be detected. Attached to the foot. For example, each floor reaction force detector 75 is attached to the bottom of a shoe Sh worn on each foot of the operator P, as shown in FIG. 4 and 5, only one floor reaction force detector 75 corresponding to one leg of the operator P is illustrated as a representative.

The master device 51 further includes a master control unit 81 having a function of controlling the operation of the moving drive mechanism 55 and the lifting mechanism 60, a function of outputting (transmitting) command information regarding the operation of the slave device 1 to the slave control unit 31, and the like. and a communication device 83 for wireless communication with the slave control unit 31 . These are mounted at arbitrary suitable locations of the master device 51 such as the base 53 .

The master control unit 81 is configured by an electronic circuit unit including, for example, a microcomputer, memory, interface circuit, and the like. The master control unit 81 includes observations detected or estimated by the upper body strength detector 64, each motor rotation detector 56, the slide displacement detector 67, the operator movement detector 70, and each floor reaction force detector 75, respectively. Along with inputting data, an observed value of the actual slave lateral body position is input from the slave control unit 31 via the communication device 83 . Note that each observation data input to the master control unit 81 may be a filtered value subjected to filtering processing such as a low-pass filter.

The master control unit 81 includes a master movement control unit 81a that controls the operation of the movement drive mechanism 55 and the lifting mechanism 60 as functions realized by both or one of the implemented hardware configuration and program (software configuration). , and a desired upper body support motion determination unit 81 b that determines a desired upper body support motion, which is the desired motion of the upper body support member 65 .

Here, in the present embodiment, the desired upper body support portion motion determined by the desired upper body support portion motion determining portion 81b is the desired upper body support portion position, which is the target position of the upper body support portion 65, and the desired upper body support portion position. and a target upper body support orientation, which is the target orientation of 65. These target body support position and target body support orientation are the target position and target orientation in the master side global coordinate system Cgm.

The master control unit 81 also provides the actual operator body posture (orientation and inclination), which is the actual posture of the upper body of the operator P, and the actual height of the body support unit 65 as command information regarding the operation of the slave device 1 . The actual height of the body support portion (or the actual height of operator P's upper body (vertical position)), which is the height (vertical position), and the actual height of each leg of operator P. It has a function of transmitting the observed values of the operator's foot position/orientation and the actual operator's foot floor reaction force of each foot of the operator P to the slave control unit 31 via the communication device 83 .

[Control processing and operation]
Next, the details of the control processing of the slave control section 31 and the master control section 81 and the operations of the slave device 1 and the master device 51 will be described. Here, as a supplement to the reference signs in the following description, the actual value of any state quantity (position, orientation, force, etc.) or the reference sign indicating the observed value is a reference with the suffix "_act" added. As a reference code indicating the target value of an arbitrary state quantity, a reference code with a suffix "_aim" added is used. Further, the coordinate axes (X-axis, Y-axis, Z-axis) relating to the control processing of the slave control unit 31 mean the coordinate axes of the slave-side global coordinate system Cgs, unless otherwise specified. Similarly, the coordinate axes (X-axis, Y-axis, Z-axis) relating to the control processing of the master control unit 81 mean the coordinate axes of the master-side global coordinate system Cgm unless otherwise specified.

[Control processing of master control unit]
First, control processing of the master control unit 81 will be described. The master control unit 81 sequentially executes the control processing shown in the flowchart of FIG. 6 at predetermined control processing cycles. In STEP 1, the master control unit 81 uses the desired upper body support motion determination unit 81b to determine the desired upper body support motion (desired upper body support position and desired upper body support orientation).

In this case, the desired body support motion determination unit 81b receives the observation value of the actual slave lateral body position viewed on the slave side global coordinate system Cgs from the slave control unit 31 of the slave device 1 via the communication device 83. Get (receive). Then, the desired body support portion motion determination unit 81b determines the actual lateral position of the body support portion (actual lateral position of the body support portion 65) and the actual slave lateral body position in the master-side global coordinate system Cgm. with the goal of satisfying a predetermined relationship represented by the following equations (1a) and (1b) (the predetermined relationship being the target correspondence relationship), the actual slave lateral body position obtained from the slave control unit 31 A desired upper body support lateral position, which is the lateral position of the desired upper body support positions, is determined according to the observed value of . In this specification, "*" is used as an arithmetic symbol representing multiplication.
P_mb_x_act = Kpmb * P_sb_x_act + Cpmb_x
...... (1a)
P_mb_y_act = Kpmb * P_sb_y_act + Cpmb_y
...... (1b)

Here, Kpmb, Cpmb_x, and Cpmb_y in formulas (1a) and (1b) are predetermined constants. Note that each of Cpmb_x and Cpmb_y may be zero.

Also, P_mb_x_act and P_mb_y_act are the X-axis direction position and Y-axis direction position, respectively, of the lateral position of the actual body support portion viewed in the master side global coordinate system Cgm, and P_sb_x_act and P_sb_y_act are the slave side global coordinates. It is the X-axis direction position and the Y-axis direction position of the lateral body position of the actual slave as viewed by the system Cgs.

Therefore, the desired body support portion motion determination unit 81b uses the following equations (2a) and (2b) to determine the desired values of the actual slave lateral body position (P_sb_x_act, P_sb_y_act) obtained from the slave control unit 31. Determine the upper body support lateral position (P_mb_x_aim, P_mb_y_aim).
P_mb_x_aim=Kpmb*P_sb_x_act+Cpmb_x
...... (2a)
P_mb_y_aim=Kpmb*P_sb_y_act+Cpmb_y
...... (2b)

Here, P_mb_x_aim and P_mb_y_aim are the X-axis direction position and Y-axis direction position, respectively, of the lateral position of the apparent base body support portion in the master-side global coordinate system Cgm.

In this embodiment, when the master device 51 starts moving the slave device 1 or at an appropriate timing thereafter, the X-axis direction (or the Y-axis direction) of the master-side global coordinate system Cgm relative to the front-rear direction of the master device 51 is changed. direction) in the yaw direction and the yaw direction of the X-axis direction (or Y-axis direction) of the slave-side global coordinate system Cgs with respect to the front-rear direction of the slave device 1 are the same. The X-axis direction (or Y-axis direction) of each global coordinate system Cgm, Cgs is set. For example, the X-axis directions of the master-side global coordinate system Cgm and the slave-side global coordinate system Cgs are set to match the front-rear directions of the master device 51 and the slave device 1, respectively.

However, the direction in the yaw direction of the X-axis direction (or Y-axis direction) of the master-side global coordinate system Cgm with respect to the front-back direction of the master device 51 and the X-axis direction of the slave-side global coordinate system Cgs with respect to the front-back direction of the slave device 1 (or the Y-axis direction) may be different from the yaw direction. In this case, based on these orientations, if the observed value of the actual slave body lateral position seen in the slave side global coordinate system Cgs is coordinate-transformed to the lateral position seen in the master side global coordinate system Cgm good.

Supplementally, the lateral position obtained by the calculation of the right sides of the above equations (2a) and (2b) is determined as the target operator lateral body lateral position, which is the target lateral position of the operator P's body. A target upper body support lateral position may be determined according to the position.

In this case, the processing for determining the desired upper body support portion lateral position using the above equations (2a) and (2b) is, in other words, the calculation of the right side of the equations (2a) and (2b). It can be said that this is a process of determining a position and determining the desired operator lateral body position as it is as the desired lateral position of the upper body support portion.

On the other hand, an elastic member such as a pad interposed between the upper body support portion 65 and the upper body of the operator P is elastically deformed according to the force acting between the upper body support portion 65 and the upper body of the operator P. Due to this elastic deformation, the lateral position of the upper body supporting portion 65 and the lateral position of the operator P's body are displaced relative to each other.

Therefore, taking this into consideration, the target operator body lateral position determined by the calculation of the right sides of the above equations (2a) and (2b) is calculated as the actual body support reaction force detected by the body strength detector 64. The desired upper body support portion lateral position may be determined by correcting according to the lateral translational force (translational force in the X-axis direction and the Y-axis direction).
Specifically, for example, the desired upper body support portion lateral position (P_mb_x_aim, P_mb_y_aim) may be determined by the following equations (2a-1) and (2b-1).
P_mb_x_aim
= P_opb_x_aim + kspring_fx * F_mb_x_act
= (Kpmb * P_sb_x_act + Cpmb_x) + kspring_fx * F_mb_x_act
...... (2a-1)
P_mb_y_aim
＝P_opb_y_aim＋kspring_fy*F_mb_y_act
= (Kpmb * P_sb_y_act + Cpmb_y) + kspring_fy * F_mb_y_act
...... (2b-1)

Here, P_opb_x_aim and P_opb_y_aim are the X-axis and Y-axis positions of the target lateral position of the operator's upper body, and F_mb_x_act and F_mb_y_act are the actual positions detected by the upper body strength detector 64. The X-axis direction translational force, the Y-axis direction translational force, and kspring_fx among the upper body support reaction forces relate to the X-axis direction translational force generated between the upper body of the operator P and the body support portion 65. A value preset as the reciprocal of the spring constant (rigidity), kspring_fy, is preset as the reciprocal of the spring constant (rigidity) relating to the translational force in the Y-axis direction generated between the upper body of the operator P and the upper body support section 65. It is the set value.

Further, in the present embodiment, the desired body support portion motion determination unit 81b is the actual body support portion reaction force detected by the upper body force detector 64, which is the translational force in the vertical direction (Z-axis direction). The observed value of the body support vertical reaction force F_mb_z_act, and the observed value of the actual body support yaw moment M_mb_z_act, which is the moment in the yaw direction (direction around the Z-axis) of the actual body support reaction force to get

Then, the desired body support motion determining unit 81b uses the observed value of the actual body support vertical reaction force F_mb_z_act to determine the vertical position (the height from the floor surface) of the desired body support position. ), and the observed value of the actual body support yaw moment M_mb_z_act is used to determine the desired body support orientation θ_mb_z_aim.

More specifically, in the process of determining the desired body support height P_mb_z_aim, the desired body support motion determining unit 81b determines that the actual body support vertical reaction force F_mb_z_act satisfies the following expression (3): Determine the desired upper body support height P_mb_z_aim so that F_mb_z_act converges to Cz.
F_mb_z_act - Cz = 0 (3)

Here, Cz is a target value (predetermined value) of an upward translational force exerted on the operator P from the upper body support section 65 in order to reduce the load on the operator's P legs. The target value Cz can be set, for example, to a predetermined proportion of the force of gravity acting on the operator P. However, the target value Cz may be zero.

Specifically, the desired body support motion determination unit 81b determines the difference between the observed value of the actual body support vertical reaction force F_mb_z_act and its target value (=Cz) (value ), the target vertical translational velocity V_mb_z_aim of the body support portion 65 is determined so that the deviation converges to zero by a feedback control law (for example, P law, PD law, PID law, etc.).

Then, the desired upper body support motion determination unit 81b determines the desired upper body support height P_mb_z_aim by integrating the determined desired translational velocity V_mb_z_aim. As a result, the desired body support height P_mb_z_aim is determined such that the actual body support vertical reaction force F_mb_z_act converges to its target value (=Cz).

In the process of determining the desired body support orientation θ_mb_z_act, the desired body support motion determination unit 81b determines the desired body support orientation θ_mb_z_act such that the actual body support yaw-direction moment M_mb_z_act converges to zero. decide.

In this case, the desired body support portion motion determination unit 81b controls the actual body support portion yaw-direction moment M_mb_z_act to converge to zero by a feedback control law (for example, P law, PD law, PID law, etc.). A target angular velocity ω_mb_z_aim of the body support 65 in the yaw direction is determined according to the observed value of the body support yaw moment M_mb_z_act.

Then, the desired body support motion determination unit 81b determines the desired body support orientation θ_mb_z_aim by integrating the determined target angular velocity ω_mb_z_aim. Thereby, the desired body support orientation θ_mb_z_aim is determined such that the actual body support yaw direction moment M_mb_z_act converges to zero.

In STEP 1, as described above, the desired upper body support position (P_mb_x_aim, P_mb_y_aim, P_mb_z_aim) and the desired upper body support orientation θ_mb_z_aim are determined as the desired upper body support motion.

Supplementally, the method of determining the desired upper body support orientation θ_mb_z_aim in the desired upper body support motion is not limited to the above method. For example, the actual orientation of operator P's upper body (as viewed in the master-side global coordinate system Cgm orientation) may be estimated. Then, the estimated value of the actual operator body orientation may be determined as the desired upper body support orientation θ_mb_z_aim. In this case, the operator motion detector 70 may estimate the actual operator body direction.

Also, the method for determining the desired upper body support portion height P_mb_z_aim is not limited to the above method. For example, the desired upper body support height P_mb_z_aim may be determined based on the vertical translation force among the floor reaction forces detected by the floor reaction force detector 75 for each foot of the operator P.

Specifically, the desired upper body support motion determination unit 81b calculates the observed value of the resultant force of the floor reaction forces acting on each of the feet of the operator P (hereinafter referred to as the operator's total floor reaction force) as is obtained based on the floor reaction force detected by the floor reaction force detector 75 of the foot.

Next, the desired upper body support portion motion determination unit 81b is configured so that the vertical translational force F_opf_total_z_act of the actual total floor reaction force of the operator satisfies the relationship of the following expression (3-1) (so that F_opf_total_z_act converges to Ctotalfz). , determine the desired upper body support height P_mb_z_aim.
F_opf_total_z_act-Ctotalfz=0 (3-1)

Here, Ctotalfz is the target value (predetermined value) of the vertical translation force component of the total floor reaction force acting on the legs of the operator P from the floor. The target value Ctotalfz can be set, for example, to a predetermined proportion of the force of gravity acting on the operator P.

Specifically, the desired upper body support motion determination unit 81b determines the deviation (equation (3-1) (value on the left side of )), the target vertical translational velocity V_mb_z_aim of the upper body support 65 is determined so that the deviation converges to zero by a feedback control law (e.g., P law, PD law, PID law, etc.) do.

Then, the desired upper body support motion determination unit 81b determines the desired upper body support height P_mb_z_aim by integrating the determined desired translational velocity V_mb_z_aim. As a result, the desired upper body support height P_mb_z_aim is determined so that the vertical translational force F_opf_total_z_act of the actual operator total floor reaction force converges to its desired value (=Ctotalfz).

Next, the master control section 81 executes the processing of STEP2 to STEP5 by the master movement control section 81a. In STEP 2, the master movement control section 81a uses the actual motor rotation angle detected by the motor rotation detector 56 and the actual slide displacement detected by the slide displacement detector 67 to determine the actual movement of the upper body support section 65. Estimate the real body support motion, which is the state of motion.

More specifically, in this embodiment, the real body support motion estimated by the master movement control unit 81a is the real body support motion, which is the actual position of the body support 65 as viewed in the master-side global coordinate system Cgm. and the actual body support orientation, which is the actual orientation of the body support 65 .

and. First, the master movement control unit 81a, for each movement drive mechanism 55, performs a differentiation process to determine the temporal change rate (differential value) of the observed value of the actual motor rotation angle of each of the electric motors 55a and 55b. The actual motor rotation speed, which is the actual rotation speed (angular speed) of each of the rotation shafts 55a and 55b, is estimated. In this case, in order to suppress the influence of high-frequency noise components in the observed value of the actual motor rotation angle, it is preferable to use pseudo differentiation (in other words, incomplete differentiation) as the differentiation processing.

In the following description, the actual motor rotations of the electric motors 55a and 55b of the movement drive mechanism 55 corresponding to the four movement grounding portions 54(n) (n=1, 2, 3, 4) of the master device 51 will be described. ω_mw_mota_act(n) and ω_mw_motb_act(n) (n=1, 2, 3, 4) are used as reference codes representing the velocity, respectively.

Further, the master movement control unit 81a controls the movement of each moving contact portion 54(n) in the Xm-axis direction of the master coordinate system Cm shown in FIGS. 3 and 4 (the longitudinal direction of the master device 51). The actual translational velocity V_mw_local_x_act(n) of (n) and the actual translational velocity V_mw_local_y_act(n) of the moving grounding portion 4(n) in the Ym-axis direction of the master coordinate system Cm (the front and left direction of the master device 51) are , and the observed values of the actual motor rotational speeds ω_mw_mota_act(n) and ω_mw_motb_act(n) of the electric motors 55a and 55b, respectively, are calculated by the following equations (4a) and (4b).
V_mw_local_x_act(n)
= Cmwx * (ω_mw_mota_act(n) + ω_mw_motb_act(n))
...... (4a)
V_mw_local_y_act(n)
= Cmwy * (ω_mw_mota_act(n) - ω_mw_motb_act(n))
...... (4b)

Each of the coefficients Cmwx and Cmwy is a predetermined value defined depending on the structure of the moving drive mechanism 55 and the like.

Then, the master movement control unit 81a controls the actual translational velocities V_mw_local_x_act(1) to V_mw_local_x_act(4) in the Xm-axis direction (front-rear direction) of the four movement grounding portions 54(1) to 54(4), respectively, and the four Based on the actual translational velocities V_mw_local_y_act(1) to V_mw_local_y_act(4) in the Ym-axis direction of each of the moving grounding parts 4(1) to 4(4), the body support part in the Xm-axis direction of the master coordinate system Cm The actual translational velocity V_mb_local_x_act of 65 and the actual translational velocity V_mb_local_y__act of the body support portion 65 in the Ym-axis direction of the master coordinate system Cm are estimated by the following equations (5a) and (5b), respectively.
V_mb_local_x_act
= (-V_mw_local_x_act(1)*Lmwy(4)+V_mw_local_x_act(4)*Lmwy(1))
/(2*(Lmwy(1)-Lmwy(4)))
+ (-V_mw_local_x_act(2)*Lmwy(3)+V_mw_local_x_act(3)*Lmwy(2))
/(2*(Lmwy(2)-Lmwy(3)))
...... (5a)
V_mb_local_y_act
= (-V_mw_local_y_act(1)*Lmwx(2)+V_mw_local_y_act(2)*Lmwx(1))
/(2*(Lmwx(1)-Lmwx(2)))
+ (-V_mw_local_y_act(4)*Lmwx(3)+V_mw_local_y_act(3)*Lmwx(4))
/(2*(Lmwx(4)-Lmwx(3)))
...... (5b)

Lmwy(1), Lmwy(2), Lmwy(3), and Lmwy(4) in the above equation (5a) are respectively predetermined reference points set for the master device 51 as shown in FIG. Reference point Qm, left front moving ground contact portion 4(1), left rear moving ground contact portion 4(2), right rear moving ground contact portion 4(3), and right front moving ground contact portion It is the distance in the Ym-axis direction (horizontal direction) between each grounding portion of the portion 4(4). In this case, Lmwy(1)>0, Lmwy(2)>0, Lmwy(3) <0, Lmwy(4)<0. The master reference point Qm is, for example, the middle point between the left and right sides of the upper body support section 65 on the axis of the support shaft 63 (the center of the waist of the operator P wearing the upper body support section 65). point in the vicinity).

Lmwx(1), Lmwx(2), Lmwx(3), and Lmwx(4) in the above equation (5b) are, as shown in FIG. between the ground contact portion 4(1), the left rear moving ground contact portion 4(2), the right rear moving ground contact portion 4(3), and the right front moving ground contact portion 4(4). It is the distance in the Xm-axis direction (front-rear direction). In this case, the respective positive and negative polarities of Lmwx(1), Lmwx(2), Lmwx(3), and Lmwx(4) are Lmwx(1)>0, Lmwx(2)<0, Lmwx(3) It is defined as <0, Lmwx(4)>0.

In addition, the master movement control unit 81a estimates the actual angular velocity ω_mb_local__z_act of the base 53 in the direction (yaw direction) around the Zm axis direction (vertical direction) of the master coordinate system Cm using the following equation (6).
ω_mb_local_z_act
= (V_mw_local_x_act(4) - V_mw_local_x_act(1))
/(2*(Lmwy(1)-Lmwy(4)))
+ (V_mw_local_x_act(3)-V_mw_local_x_act(2))
/(2*(Lmwy(2)-Lmwy(3)))
……(6)

Here, in this embodiment, the Z-axis direction of the master-side global coordinate system Cgm is the same direction (vertical direction) as the Zm-axis direction of the master coordinate system Cm. The estimated value of ω_mb_local_z_act matches the actual body support yaw direction angular velocity ω_mb_z_act, which is the actual angular velocity of the body support 65 in the yaw direction as viewed in the master-side global coordinate system Cgm. Therefore, the estimated value of the actual body support yaw angular velocity ω_mb_z_act is obtained from equation (6).

Then, the master movement control unit 81a estimates the actual body support orientation θ_mb_z_act viewed on the master side global coordinate system Cgm by integrating the actual body support yaw direction angular velocity ω_mb_z_act obtained as described above.

Further, the master movement control unit 81a generates a vector (a two-dimensional vector on the XmYm coordinate plane of the master coordinate system Cm) having two components of the translational velocities V_mb_local_x_act and V_mb_local_y_act obtained by the above equations (5a) and (5b) as follows: By rotationally transforming in the yaw direction (direction around the Z-axis) by an angle that matches the direction of the actual body support portion θ_mb_z_act obtained as described above, the lateral direction of the body support portion 65 viewed in the master-side global coordinate system Cgm Estimate the actual body support lateral velocities V_mb_x_act, V_mb_y_act, which are the actual translational velocities of . Here, V_mb_x_act is the X-axis component of the lateral velocity of the actual body support viewed in the master-side global coordinate system Cgm, and V_mb_y_act is the lateral velocity of the actual body support viewed in the master-side global coordinate system Cgm. It is the Y-axis component of the directional velocity.

Then, the master movement control unit 81a integrates the actual body support lateral velocities V_mb_x_act and V_mb_y_act, respectively, so that the lateral position ( A set of a position P_mb_x_act in the X-axis direction and a position P_mb_y_act in the Y-axis direction of the actual body support portion lateral direction position) is estimated.

In addition, the master movement control unit 81a determines the vertical (Z-axis) position (in other words, floor position) of the actual body support position from the actual slide displacement detected by the slide displacement detector 67. Estimate the actual body support height P_mb_z_act, which is the height from the plane). Specifically, by adding the detection value of the actual sliding displacement to the height (predetermined value) of the body supporting portion 65 when the actual sliding displacement is zero, the actual body supporting portion height P_mb_z_act is estimated. be done.

In this embodiment, the actual body support portion motion (the actual body support portion positions P_mb_x_act, P_mb_y_act, P_mb_z_act, and the actual body support portion orientation θ_mb_z_act) is estimated by the processing of STEP 2 described above. Supplementally, the actual body support position P_mb_x_act, P_mb_y_act, P_mb_z_act, and the actual body support orientation θ_mb_z_act are based on environment recognition information such as landmarks around the master device 51 in order to prevent accumulation of integration errors. Based on this, correction may be made at any time. Further, for example, a camera capable of photographing the body support section 65, an inertial sensor attached to the body support section 65, or the like may be used to estimate the actual body support section motion through motion capture processing. .

Next, in STEP 3, the master movement control unit 81a controls the movement mechanism so as to realize the desired upper body support lateral positions P_mb_x_aim, P_mb_y_aim and the desired upper body support orientation θ_mb_z_aim of the desired upper body support motion. A target translational velocity for each moving ground 54 of 52 is determined. Then, the master movement control section 81a controls the electric motors 55a and 55b corresponding to the movement grounding sections 54 so as to achieve the target translation speed.

Specifically, the master movement control unit 81a calculates the following equation (7a) from the observed values of the actual body support portion lateral positions (P_mb_x_act, P_mb_y_act) and the desired body support portion lateral positions (P_mb_x_aim, P_mb_y_aim). , (7b) to obtain the upper body support lateral position deviations P_mb_x_err and P_mb_y_err.
P_mb_x_err = P_mb_x_act - P_mb_x_aim (7a)
P_mb_y_err = P_mb_y_act - P_mb_y_aim (7b)

Then, the master movement control section 81a converts the lateral position deviations P_mb_x_err and P_mb_y_err of the body support portion determined as described above to an angle (=- θ_mb_z_act) in the yaw direction (direction around the Z-axis) to obtain lateral position deviations P_mb_local_x_err and P_mb_local_y_err of the body support in the Xm-axis direction and the Ym-axis direction of the master coordinate system Cm, respectively. .

Next, the master movement control unit 81a adjusts the X-direction positional deviation P_mb_local_x_err of the body support part in the Xm-axis direction of the master coordinate system Cm so as to approach zero by a feedback control law (for example, P law, PD law, PID law, etc.). , determines a target upper body support lateral velocity V_mb_local_x_aim, which is a target translational velocity of the body support 65 in the Xm-axis direction, according to the calculated value of the deviation P_mb_local_x_err. Similarly, the master movement control unit 81a controls the body support portion position deviation P_mb_local_y_err in the Ym-axis direction of the master coordinate system Cm so as to approach zero by a feedback control law (for example, P law, PD law, PID law, etc.). A desired upper body support lateral velocity V_mb_local_y_aim in the Ym-axis direction is determined according to the calculated value of the deviation P_mb_local_y_err.

Further, the master movement control section 81a obtains the body support portion orientation deviation θ_mb_z_err from the observed value of the actual body support portion orientation θ_mb_z_act and the desired body support portion orientation θ_mb_z_aim using the following equation (8).
θ_mb_z_err = θ_mb_z_act - θ_mb_z_aim (8)

Then, the master movement control unit 81a adjusts the calculated value of the deviation θ_mb_z_err so that the deviation θ_mb_z_err toward the upper body support portion approaches zero by a feedback control law (for example, P law, P law, PD law, PID law, etc.). Accordingly, the desired body support portion local yaw direction angular velocity ω_mb_local_z_aim is determined. When the desired body support portion orientation θ_mb_z_aim is determined by integration of the target angular velocity in the yaw direction of the body support portion 65 in STEP 1, the target angular velocity ω_mb_z_aim determined in STEP 1 is It may be determined as the yaw angular velocity ω_mb_local_z_aim.

Then, the master movement control unit 81a implements the following formula (9a) so as to realize the apparent target body support portion lateral velocities V_mb_local_x_aim, V_mb_local_y_aim and the desired body support portion local yaw direction angular velocity ω_mb_local_z_aim in the master coordinate system Cm. , (9b), the target translational velocity V_mw_local_x_aim(n) in the Xm-axis direction and the target Determine the translational velocity V_mw_local_y_aim(n).
V_mw_local_x_aim(n)
= V_mb_local_x_aim - Lmwy(n) * ω_mb_local_z_aim
...... (9a)
V_mw_local_y_aim(n)
= V_mb_local_y_aim + Lmwx(n) * ω_mb_local_z_aim
...... (9b)

Further, the master movement control unit 81a controls the target rotational speeds of the electric motors 55a and 55b for achieving the target translational speeds V_mw_local_x_aim(n) and V_mw_local_y_aim(n) for each of the movement grounding portions 54(n). Target motor rotation speeds ω_mw_mota_aim(n) and ω_mw_motb_aim(n), which are values, are calculated by the following equations (10a) and (10b) obtained from the equations (4a) and (4b).
ω_mw_mota_aim(n)
= (Cmwy*V_mw_local_x_aim(n)
+Cmwx*V_mw_local_y_aim(n))/(2*Cmwx*Cmwy)
...... (10a)
ω_mw_motb_aim(n)
= (Cmwy*V_mw_local_x_aim(n)
-Cmwx*V_mw_local_y_aim(n))/(2*Cmwx*Cmwy)
...... (10b)

Next, the master movement control unit 81a sets the actual motor rotational speeds ω_mw_mota_act(n) and ω_mw_motb_act(n) of the electric motors 55a and 55b to the target motor rotational speeds ω_mw_mota_aim(n) for each movement grounding portion 54(n). ), ω_mw_motb_aim(n), the target motor driving forces Tq_mw_mota_aim(n) and Tq_mw_motb_aim(n), which are target values of the respective driving forces (rotational driving forces) of the electric motors 55a and 55b, are expressed by the following equations, for example: Determined by (11a) and (11b). These target motor drive forces Tq_mw_mota_aim(n) and Tq_mw_motb_aim(n) (n=1, 2, 3, 4) are target movement drive forces shown in FIG.
Tq_mw_mota_aim(n)
＝Kv_mw_mota*(ω_mw_mota_aim(n)－ω_mw_mota_act(n))
...... (11a)
Tq_mw_motb_aim(n)
＝Kv_mw_motb*(ω_mw_motb_aim(n)－ω_mw_motb_act(n))
...... (11b)

Kv_sw_mota and Kv_sw_motb are gains of predetermined values. Further, the actual motor rotation speeds ω_mw_mota_act(n) and ω_mw_motb_act(n) are estimated by differentiating the detected values of the actual motor rotation angles, as in STEP2.

Supplementally, the equations (11a) and (11b) are equations for determining Tq_mw_mota_aim(n) and Tq_mw_motb_aim(n) by the P-law (proportional law) as an example of the feedback control law. Tq_mw_mota_aim(n) and Tq_mw_motb_aim(n) may be determined by (for example, PD law, PID law, etc.).

Next, the master movement control section 81a causes the electric motors 55a and 55b corresponding to each movement grounding section 54(n) to output the target motor driving forces Tq_mw_mota_aim(n) and Tq_mw_motb_aim(n) determined as described above. operate as As a result, movement control of the movement mechanism 52 is performed such that the desired body support portion lateral velocities V_mb_x_aim, V_mb_y_aim and the desired body support portion angular velocity ω_mb_z_aim are achieved. Consequently, the actual body support lateral positions P_mb_x_act, P_mb_y_act and the actual body support orientation θ_mb_z_act follow the desired body support lateral positions P_mb_x_aim, P_mb_y_aim and the desired body support orientation θ_mb_z_aim, respectively. Movement control of the movement mechanism 52 is performed so as to do so.

Further, in STEP4, the master movement control unit 81a controls the slide drive actuator 66 so as to achieve the target upper body support height P_mb_z_aim. Specifically, the master movement control unit 81a uses a feedback control rule (for example, P-law, PD-law, PID-law, etc.) is used to determine a desired vertical velocity V_mb_z_aim of the body supporting part 65, which is a desired translational velocity of the body supporting part 65 in the vertical direction (Z-axis direction).

Then, the master movement control unit 81a controls the body support part 65 calculated by differentiating the desired body support part vertical velocity V_mb_z_aim determined as described above and the actual body support part height P_mb_z_act obtained in STEP 2. According to the deviation from the vertical velocity V_mb_z_act of the actual body support, which is the actual vertical translational velocity of the actual body support, the deviation is converged to zero by a feedback control law (for example, P law, PD law, PID law, etc.) , the target drive force of the slide drive actuator 66 is determined. Then, the master movement control section 81a controls the slide drive actuator 66 so as to generate this target drive force.

Thereby, the slide drive actuator 66 is controlled such that the actual body support height P_mb_z_act follows the desired body support height P_mb_z_aim.

Next, in STEP 5, the master movement control unit 81a estimates the actual operator's body direction (actual direction of the operator's body). In this process, the master movement control unit 81a uses, for example, the estimated value of the actual body support portion orientation θ_mb_z_act obtained in STEP 2 and the yaw direction of the actual body support portion reaction force detected by the upper body strength detector 64. from the moment M_mb_z_act (actual body support portion yaw direction moment), the actual operator body direction θ_opb_z_act is estimated by the following equation (12).
θ_opb_z_act = θ_mb_z_act - kspring_mz * M_mb_z_act
……(12)

Here, kspring_mz is a value set in advance as the reciprocal of the spring constant (rigidity) relating to the rotational force in the yaw direction generated between the upper body of the operator P and the upper body support section 65 . Supplementally, the method of estimating the real operator's upper body orientation is not limited to the above method, and other methods may be employed. For example, it is possible to estimate the real operator's upper body orientation by processing a motion capture using a camera that captures the operator P or an inertial sensor attached to the upper body of the operator P or the like.

Alternatively, for example, the relative displacement (relative rotation angle) of the operator P's upper body in the yaw direction with respect to the upper body support section 65 is detected by an appropriate displacement sensor provided in the upper body support section 65 or the like, and the relative displacement is detected. By adding the observed value to the actual body support orientation θ_mb_z_act, it is also possible to estimate the actual operator's body orientation.

Alternatively, for example, it is also possible to use a distance measuring device capable of measuring distances to a plurality of locations on the upper body of the operator P, and estimate the actual operator's upper body orientation based on the distance values observed by the distance measuring device. be.

If the operator motion detector 70 is a detector capable of estimating the actual operator's upper body orientation as viewed in the master-side global coordinate system Cgm, the processing of STEP5 can be omitted.

As described above, the master control unit 81 executes the processing of the master movement control unit 81a (the processing of STEP2 to STEP5), and then, in STEP6, transmits command information regarding the operation of the slave device 1 to the slave control unit 31. .

Specifically, the master control unit 81 controls the actual operator body orientation estimated in STEP 6 (or the actual operator body orientation estimated by the operator motion detector 70) and the actual operator body orientation estimated by the operator motion detector 70. The actual operator's body posture as a set with the inclination of the operator's body, the actual operator's foot position/posture estimated by the operator motion detector 70, and the actual operator's foot floor reaction force detected by the floor reaction force detector 75. , and the actual body support portion height estimated in STEP 2 are transmitted to the slave control unit 31 as components of the command information.

In this case, each command information output from the master control unit 81 to the slave control unit 31 (actual operator body posture, actual operator foot position/posture, actual operator foot floor reaction force, and actual body support height) (observed value) is an observed value seen in the master-side global coordinate system Cgm.

Note that each command information (observed value) output (transmitted) from the master control unit 81 to the slave control unit 31 may be a filtering value that has undergone filtering processing such as a low-pass filter.

In addition, when the actual operator's upper body height can be estimated by the output of an inertial sensor attached to the upper body of the operator P, a motion capture technique, etc., the actual operator's The observed value of the body height may be transmitted to the slave control unit 31 . Alternatively, the actual operator body height P_opb_z_act is obtained from the actual body support height P_mb_z_act estimated in STEP 2 and the vertical translational force F_mb_z_act of the body support reaction force detected by the body strength detector 64. For example, the estimated value of the actual operator upper body height P_opb_z_act may be estimated by the following equation (13) and transmitted to the slave control unit 31 instead of the actual body support height.
P_opb_z_act = P_mb_z_act - kspring_fz * F_mb_z_act ...... (13)

Here, kspring_fz is a value set in advance as a reciprocal of a spring constant (rigidity) relating to the translational force generated between the upper body of the operator P and the upper body support section 65 in the vertical direction.
In this embodiment, the control processing of the master control unit 81 is executed as described above.

[Control processing of the slave controller]
Next, control processing of the slave control unit 31 will be described. The slave control unit 31 sequentially executes the processing of each functional unit described above at a predetermined control processing cycle. In the following description, in order to distinguish between left and right constituent elements of the slave device 1, master device 51, and operator P, and state quantities related to each of them, the left constituent elements and the constituent elements will be described as needed. "L" and "R" are added to the reference numerals of the state quantity related to and the reference numerals of the state quantity related to the component on the right side and the component, respectively. For example, if a certain state quantity A is a state quantity related to the component on the left side of the slave device 1, the reference sign is written as "A_L", and if it is a state quantity related to the component on the right side, , and its reference code is written as "A_R".

When state quantities such as position, orientation, and floor reaction force are expressed as three-dimensional vectors composed of three coordinate axis components of a coordinate system such as the master-side global coordinate system Cgm, the reference sign of the state quantity A sign "↑" is added to the beginning of the . For example, when expressing a state quantity A as a three-dimensional vector viewed from the master-side global coordinate system Cgm or the slave-side global coordinate system Cgs, it is expressed as "state quantity ↑ A". In this case, the "state quantity ↑A" means a vector having three components: a component A_x in the X-axis direction, a component A_y in the Y-axis direction, and a component A_z in the Z-axis direction of the coordinate system Cgm or Cgs. .

Further, in the following description, when representing the actual value or target value of the state quantity related to the slave device 1, "slave" is added between "actual" or "target" and the name of the state quantity. may be written as For example, the desired foot position/orientation of the slave device 1 may be referred to as the desired slave foot position/orientation.

[Processing of Slave Operation Target Determining Unit]
First, the processing of the slave operation target determination unit 31a of the slave control unit 31 will be described. 2, the actual operator body posture (orientation, inclination) and the actual body support height (or Observed values of the actual operator's body height), the actual operator's foot position/orientation, and the actual operator's foot floor reaction force are sequentially input, and the actual values estimated by the lateral body position estimator 31d as described later. Observed values of the lateral body position of the slave are sequentially input.

Then, the slave operation target determination unit 31a executes the process shown in the flowchart of FIG. 8 at a predetermined control process cycle. In STEP 11, the slave motion target determination unit 31a determines the actual operator foot position/orientation of the left and right feet of the operator P and the actual slave foot position/orientation of the left and right feet 6L and 6R of the slave device 1, With the goal of satisfying the predetermined relationships represented by the following equations (21a) to (21d), according to the actual operator's foot position and orientation (observed values) of the left and right feet of the operator P received from the master control unit 81 Then, the target slave foot position and orientation of the left and right feet 6L and 6R of the slave device 1 are determined.

That is, the slave motion target determining unit 31a determines the left and right positions of the slave device 1 from the observed values of the actual operator's foot positions and orientations of the left and right feet of the operator P using the following equations (21a-1) to (21d-1). Determine the target slave foot position and orientation of each foot 6L, 6R.
↑P_sf_act_L = Kpsf * ↑P_opf_act_L + ↑Cpsf
...... (21a)
↑P_sf_act_R＝Kpsf*↑P_opf_act_R＋↑Cpsf
... (21b)
↑θ_sf_act_L = ↑θ_opf_act_L … (21c)
↑θ_sf_act_R = ↑θ_opf_act_R … (21d)

↑P_sf_aim_L=Kpsf*↑P_opf_act_L+↑Cpsf
... (21a-1)
↑P_sf_aim_R＝Kpsf*↑P_opf_act_R＋↑Cpsf
……(21b-1)
↑θ_sf_aim_L=↑θ_opf_act_L ……(21c-1)
↑θ_sf_aim_R = ↑θ_opf_act_R ……(21d-1)

Here, ↑P_sf_act_L and ↑P_sf_act_R are the actual positions of the left and right legs 6L and 6R of the slave device 1 (actual slave leg positions), respectively, and ↑θ_sf_act_L and ↑θ_sf_act_R are the slave device 1 on the left and right legs 6L. The actual posture of 6R (actual slave foot posture), ↑P_sf_aim_L and ↑P_sf_aim_R, are the target positions (target slave foot positions) of the left and right feet 6L and 6R of the slave device 1, respectively.

Also, ↑θ_sf_act_L and ↑θ_sf_act_R are the left and right leg portions 6L . 6R's actual postures (actual slave foot postures), ↑θ_sf_aim_L and ↑θ_sf_aim_R, are the left and right feet of the slave device 1, respectively, 6L. 6R target posture (desired slave foot posture).

↑P_opf_act_L and ↑P_opf_act_R are the actual positions of the operator P's left and right feet (actual operator's foot positions), respectively; It is the actual posture of each foot (actual operator foot posture).

Kpsf is a coefficient of a predetermined value or a diagonal matrix having a diagonal component of a predetermined value, and ↑Cpsf is a constant vector having a component of a predetermined value. This ↑Cpsf may be a zero vector. Note that the predetermined relationship between each of ↑θ_sf_aim_L and ↑θ_sf_aim_L and each of ↑θ_opf_act_L and ↑θ_opf_act_R is, for example, a relationship expressed by a linear function similar to Equation (21a) or Equation (21b). There may be.

Next, in STEP 12, the slave action target determination unit 31a determines that the actual foot floor reaction forces of the left and right feet of the operator P and the actual slave floor reaction forces of the left and right feet 6L and 6R of the slave device 1 are , the actual operator foot floor reaction forces (observed values) of the left and right feet of the operator P received from the master control unit 81, with the goal of satisfying the predetermined relationships represented by the following equations (22a) to (22d): , the target slave foot position and orientation of the left and right feet 6L and 6R of the slave device 1 are determined.

That is, the slave motion target determination unit 31a determines the left and right motions of the slave device 1 from the observed values of the actual operator foot floor reaction forces of the left and right feet of the operator P according to the following equations (22a-1) to (22d-1). determine the target slave foot floor reaction force of each foot 6L, 6R.
↑F_sf_act_L＝mtotal_ratio*↑F_opf_act_L
...... (22a)
↑F_sf_act_R＝mtotal_ratio*↑F_opf_act_R
...... (22b)
↑M_sf_act_L＝mtotal_ratio*↑F_opf_act_R
...... (22c)
↑F_sf_act_L＝mtotal_ratio*↑F_opf_act_R
...... (22d)

↑F_sf_aim_L＝mtotal_ratio*↑F_opf_act_L
... (22a-1)
↑F_sf_aim_R＝mtotal_ratio*↑F_opf_act_R
……(22b-1)
↑M_sf_aim_L＝mtotal_ratio*↑F_opf_act_R
...... (22c-1)
↑F_sf_aim_L＝mtotal_ratio*↑F_opf_act_R
...... (22d-1)

Here, ↑F_sf_act_L and ↑F_sf_act_R are the translational forces (actual slave foot translational forces) of the actual slave foot floor reaction forces of the left and right feet 6L and 6R of the slave device 1, respectively. F_sf_aim_L and ↑F_sf_aim_R are the translational forces (target slave foot translational forces) of the desired slave foot floor reaction forces of the left and right feet 6L and 6R of the slave device 1, respectively.

Also, ↑M_sf_act_L and ↑M_sf_act_R are the moments (actual slave foot moment) of the actual slave foot floor reaction forces of the left and right feet 6L and 6R of the slave device 1, respectively, M_sf_aim_L and ↑M_sf_aim_R are the moments (target slave foot moments) of the desired slave foot floor reaction forces of the left and right feet 6L and 6R of the slave device 1, respectively.

Also, ↑F_opf_act_L and ↑F_opf_act_R are the translational forces (actual operator foot translational forces) of the actual operator foot floor reaction forces of the left and right feet of the operator P, respectively, and ↑M_opf_act_L and ↑M_opf_act_R are , respectively, are the moments of the actual operator foot floor reaction forces on the left and right sides of the operator P (actual operator foot moments).

Also, mtotal_ratio is the mass ratio (=slave total mass/operator total mass) between the slave total mass, which is the mass of the entire slave device 1, and the operator total mass, which is the mass of the operator P overall.

Next, in each of STEPs 13 to 15, the slave action target determination unit 31a determines the desired slave foot floor reaction force center point, the desired slave total floor reaction force, and the desired slave total floor reaction force, among the desired slave floor reaction forces. Determine each center point.

Specifically, in STEP 13, the slave movement target determination unit 31a determines the left and right feet of the slave device 1 from the target slave foot floor reaction forces (↑F_sf_aim_L, ↑F_sf_aim_R, ↑M_sf_aim_L, ↑M_sf_aim_R) determined in STEP 12. The lateral position of the target slave foot floor reaction force central point of the parts 6L and 6R is obtained. In this case, the center point (COP) of the floor reaction force of each leg 6 is the point at which the moment about the lateral direction (the X-axis direction and the Y-axis direction of the slave-side global coordinate system Cgs) becomes zero. Therefore, the lateral position of the target slave foot floor reaction force center point of each of the left and right feet 6L, 6R is calculated by the following equations (23a) to (23d).
COP_sf_x_aim_L = M_sf_y_aim_L / F_sf_z_aim_L
...... (23a)
COP_sf_x_aim_R = M_sf_y_aim_R / F_sf_z_aim_R
... (23b)
COP_sf_y_aim_L = -M_sf_x_aim_L/F_sf_z_aim_L
...... (23c)
COP_sf_y_aim_R = -M_sf_x_aim_R/F_sf_z_aim_R
...... (23d)

Here, COP_sf_x_aim_L and COP_sf_x_aim_R are the target positions in the X-axis direction of the desired slave foot floor reaction force center points of the left and right feet 6L and 6R of the slave device 1 respectively, and COP_sf_y_aim_L and COP_sf_y_aim_R are respectively: This is the target position in the Y-axis direction of the target slave foot floor reaction force center point of the left and right foot portions 6L and 6R of the slave device 1, respectively.

Further, Msf_y_aim_L and Msf_y_aim_R are the target slave leg moments ↑M_sf_aim_L and ↑M_sf_aim_R of the left and right legs 6L and 6R of the slave device 1, respectively. Target slave foot moments ↑M_sf_aim_L and ↑M_sf_aim_R of the left and right feet 6L and 6R of the slave device 1, respectively, in the direction around the X axis, F_s_z_aim_L and F_sf_z_aim_R ↑F_sf_aim_L, ↑F_sf_aim_R of the target slave foot translational forces ↑F_sf_aim_L, ↑F_sf_aim_R of the feet 6L, 6R in the Z-axis direction (vertical direction).

In STEP 14, the slave movement target determining unit 31a calculates the target slave foot floor reaction force (↑F_sf_aim_L, ↑F_sf_aim_R, ↑M_sf_aim_L, ↑M_sf_aim_R) determined in STEP 12 by the following equations (24a) and (24b). Obtain the total floor reaction force (translational force ↑F_sf_total_aim and moment ↑M_sf_total_aim). That is, the slave motion target determining unit 31a obtains the resultant force of the desired slave foot floor reaction forces of the left and right feet 6L and 6R as the desired slave total floor reaction force. Note that ↑F_sf_total_aim and ↑M_sf_total_aim are the translational force and moment of the desired slave total floor reaction force, respectively.
↑F_sf_total_aim = ↑F_sf_aim_L + ↑F_sf_aim_R
...... (24a)
↑M_sf_total_aim = ↑M_sf_aim_L + ↑M_sf_aim_R
...... (24b)

In STEP 15, the slave movement target determining unit 31a calculates the same equations (23a) to (23d) used in STEP 13 from the target slave total floor reaction force (↑F_sf_total_aim, ↑M_sf_total_aim) obtained in STEP 14. The lateral position of the desired slave total floor reaction force center point is obtained from equations (25a) and (25b). COP_sf_total_x_aim and COP_sf_total_y_aim are the X-axis direction position and Y-axis direction position of the desired slave total floor reaction force center point, respectively.
COP_sf_total_x_aim = M_sf_total_y_aim/F_sf_total_z_aim
...... (25a)
COP_sf_total_y_aim=-M_sf_total_x_aim_L/F_sf_total_z_aim
...... (25b)

Next, in STEP 16, the slave motion target determining unit 31a determines that the actual operator body posture (orientation and inclination) and the actual slave body posture (orientation and inclination) are given by, for example, the following equation (26). With the goal of satisfying the relationship, the target slave body posture is determined according to the observed values of the actual operator body posture (actual operator body orientation and actual operator body inclination) received from the master control unit 81 .
That is, the slave motion target determining unit 31a determines the desired slave body posture from the observed value of the actual operator body posture, for example, by the following equation (26-1).
↑θ_sb_act = ↑θ_opb_act … (26)
↑θ_sb_aim = ↑θ_opb_act … (26-1)

Here, ↑θ_sb_act is the actual slave body posture, ↑θ_sb_aim is the target slave body posture, and ↑θ_opb_act is the actual operator body posture. In this case, the observed value of the actual operator's body inclination is used as the value of the component of ↑θ_opb_act around the X-axis and the Y-axis, and the value of the component of ↑θ_opb_act around the Z-axis (yaw direction) is: Real operator upside observations are used.

Note that the predetermined relationship between ↑θ_sb_act and ↑θ_opb_act may be, for example, a relationship represented by a linear function in the same form as Equation (21a) or Equation (21b).

Next, in STEP 17, the slave operation target determining unit 31a determines the actual height of the body support portion of the master device 51 (the actual vertical position of the body support portion 65) and the actual slave state height (the position of the slave device 1). The actual vertical position of the body 2) and the observed value of the actual body support height received from the master control unit 81, with the goal of satisfying a predetermined relationship represented by, for example, the following equation (27). Accordingly, the height (target slave body height) of the target slave body position is determined.
That is, the slave motion target determination unit 31a determines the target slave upper body height from the observed value of the actual body support height using the following equation (27-1).
P_sb_z_act = Kpsb_z * P_mb_z_act + Cpsb_z
……(27)
P_sb_z_aim = Kpsb_z * P_mb_z_act + Cpsb_z
……(27-1)

Here, P_sb_z_act is the actual slave body height, P_sb_z_aim is the target slave body height, P_mb_z_act is the actual body support portion height, and Kpsb_z and Cpsb_z are preset constants. Note that Cpsb_z may be zero.

Supplementally, when the master control unit 81 can acquire the observed value of the actual operator body height P_opb_z_act, the actual operator body height P_opb_z_act and the actual slave body height P_sb_z_act are, for example, given by the above equation (27). The target slave upper body height P_sb_z_aim may be determined according to the observed value of P_opb_z_act_ so as to satisfy the relationship obtained by replacing P_mb_z_act with P_opb_z_act. That is, the target slave upper body height P_sb_z_aim may be determined by a formula in which P_mb_z_act in formula (27-1) is replaced with P_opb_z_act.

Next, in STEP 18, the slave motion target determining unit 31a updates the latest value of the actual slave lateral body position estimated by the lateral body position estimating unit 31d as will be described later to the lateral position of the target slave body position. is determined as the desired slave lateral body position. That is, the slave movement target determining unit 31a determines the desired slave lateral body position from the observed value (latest value) of the actual slave lateral body position using the following equations (28a) and (28b).
P_sb_x_aim = P_sb_x_act (28a)
P_sb_y_aim = P_sb_y_act (28b)

Here, P_sb_x_aim is the X-axis position of the desired slave lateral body position, P_sb_y_aim is the Y-axis position of the desired slave lateral body position, and P_sb_x_act is the actual slave lateral body position. Among them, the X-axis direction position, P_sb_y_act, is the Y-axis direction position of the real slave body lateral direction positions.

The processing of the slave operation target determination unit 31a is executed as described above. Therefore, in the present embodiment, the desired slave foot position/orientation of the left and right feet 6L and 6R of the slave device 1 is set to the real operator's It is determined according to the observed values of foot posture.

Further, the desired slave body posture and the desired slave body height are determined by the actual operator body posture and the actual body support height, respectively, with the constant linear relationship shown by the above equations (26) and (27) as targets. height (or actual operator torso height).

On the other hand, as for the target slave body lateral position, the actual slave body lateral position is the target slave body lateral position as it is, regardless of the actual lateral position of the operator P's body and the body support section 65 . is determined as

Further, among the target slave foot floor reaction forces, the desired slave foot floor reaction forces (translational force and moment) of the left and right feet 6L and 6R are the actual operator foot floor reaction forces of the operator P's left and right feet. is proportional to the mass ratio of the slave device 1 and the operator P (=total slave mass/total mass of operator). The desired slave foot floor reaction force central point, the desired slave total floor reaction force, and the desired slave total floor reaction force central point have a predetermined necessary relationship with the desired slave foot floor reaction forces of the feet 6L and 6R. determined to meet

Therefore, the desired slave body motion other than the desired slave body lateral position, the desired slave leg motion, and the desired slave floor reaction force are respectively the actual motion of operator P's upper body and each leg of operator P. It is determined so that the actual movement of the body and the actual floor reaction force acting on the operator P change in the same pattern.

Supplementally, in the present embodiment, the target slave upper body height P_sb_z_aim is determined according to the observed value of the actual body support height P_mb_z_act (or the actual operator body height P_opb_z_act). However, the target slave body height P_sb_z_aim may be set to a predetermined value, for example, regardless of the actual body support height P_mb_z_act (or the actual operator body height P_opb_z_act). In this case, it is not necessary to output (transmit) the observed value of the actual body support height P_mb_z_act (or the actual operator body height P_opb_z_act) from the master control unit 81 to the slave control unit 31 .

Also, although the description in the flowchart of FIG. 8 is omitted, in the present embodiment, the slave device 1 has the arm 10 and the head 17 that are movable with respect to the body 2, so the slave motion target determination unit 31a A target motion for each arm 10 and head 17 is also determined. In this case, when the slave device 1 is moved by the operation of the operator P, the desired motion of each arm 10 and head 17 is, for example, the hand portion 13 and head portion 17 of each arm 10 are kept at a constant position relative to the upper body 2. It can be determined to keep the orientation.

However, for example, the target motion of each arm 10 may be determined such that each arm 10 is synchronized with the motion of the leg 3 and is swung back and forth with respect to the body 2 . Also, the desired motion of the head 17 may be determined so as to move the head 17 relative to the upper body 2 as appropriate. Further, for example, the actual motion of each arm and head of the operator P is estimated by a detector similar to the operator motion detector 70, and the target motion of each arm 10 and head 17 of the slave device 1 (upper body 2 ) may be determined to be similar to the actual movements of the operator P's arms and head.

[Processing of Body Lateral Position Estimating Unit]
Next, the processing of the lateral body position estimator 31d will be described. As shown in FIG. 2, the lateral body position estimator 31d stores the actual body inclination detected by the body posture detector 23 and the target slave body position/posture determined by the slave motion target determiner 31a. The inclination (desired body inclination) and the lateral position (desired body lateral position) are sequentially input. Then, the lateral body position estimator 31d uses these input values to estimate the actual slave lateral body position.

Here, the slave device 1 basically operates so as to follow the desired slave body position/posture and the desired slave foot position/posture determined by the slave motion target determining unit 31a, but the uneven state of the floor surface Also, the inclination of the actual posture of the body 2 may deviate from the target slave body inclination due to correction of the desired slave foot position/posture by compliance control, which will be described later. Then, when the deviation of the inclination of the body 2 occurs, the actual lateral position of the body 2 deviates from the desired slave body lateral position.

Therefore, the lateral body position estimating unit 31d estimates the actual slave lateral body position by, for example, the following equations (29a) and (29b).
P_sb_x_act
= P_sb_x_aim + P_sb_z_act * sin(θ_sb_y_act - θ_sb_y_aim)
... (29a)
P_sb_y_act
= P_sb_y_aim - P_sb_z_act * sin(θ_sb_x_act - θ_sb_x_aim)
... (29b)

Here, P_sb_x_act and P_sb_y_act are the observed values of the X-axis direction position and the Y-axis direction position of the actual slave lateral body position, respectively, X-axis position and Y-axis position, P_sb_z_act is the actual slave body height, θ_sb_x_aim, θ_sb_y_aim are the inclination around the X axis and the inclination around the Y axis, respectively, of the target slave body inclination, θ_sb_x_act, θ_sb_y_act is the observed value of the inclination around the X-axis and the inclination around the Y-axis among the real slave body inclinations.

In this case, as the values of the target slave lateral body positions P_sb_x_aim and P_sb_y_aim, the target values determined by the slave operation target determining unit 31a in the control processing cycle before the current control processing of the lateral body position estimating unit 31d are is used. Also, as the values of the actual slave body inclinations θ_sb_x_act and θ_sb_y_act, estimated values obtained by the body posture detector 23 are used.

Also, as the value of the actual slave body height P_sb_z_act, for example, from the detected value of the actual joint displacement of each joint of one of the left and right legs 3L and 3R of the slave device 1 in the grounded state, kinematics An estimated value estimated by the calculation of is used. When both legs 3L and 3R are in contact with the ground, for example, the height of body 2 may be estimated for each leg 3L and 3R by kinematic calculation. Then, the average value of the estimated values for each of the legs 3L and 3R may be used as the value of the actual slave body height P_sb_z_act. Alternatively, instead of the actual slave body height P_sb_z_act, for example, the target slave body height P_sb_z_aim determined by the slave movement target determination unit 31a may be used.

If the absolute value of the deviation around the Y-axis (=θ_sb_y_act-θ_sb_y_aim) or the deviation around the X-axis (=θ_sb_x_act-θ_sb_x_aim) between the target slave body inclination and the actual slave body inclination is sufficiently small, , sin(θ_sb_y_act−θ_sb_y_aim)≈θ_sb_y_act−θ_sb_y_aim, or sin(θ_sb_x_act−θ_sb_x_aim)≈θ_sb_x_act−θ_sb_x_aim may be used to calculate the right side of Equation (29a) or Equation (29b).

Supplementally, the method of estimating the actual slave lateral body position is not limited to the above method. For example, it is possible to estimate the actual slave lateral body position by integrating lateral translational acceleration detected by the acceleration sensor 23a of the body posture detector 23 (second-order integration).

Further, for example, it is also possible to estimate the actual slave lateral body position by combining the estimation method based on the above equations (29a) and (29b) and the estimation method using the acceleration sensor 23a using a Kalman filter. is. In addition, it is possible to estimate the real slave lateral body position by various known methods that can estimate the self-position of an object.

[Processing of Composite Compliance Operation Determination Unit]
Next, processing of the composite compliance operation determination unit 31b will be described. As shown in FIG. 2, the composite compliance motion determination unit 31b stores the desired slave leg motion (desired slave foot position/orientation) determined by the slave motion target determination unit 31a and the desired slave floor reaction force (desired slave foot floor reaction force, desired slave foot floor reaction force central point, desired slave total floor reaction force, desired slave total floor reaction force central point) are sequentially input. Then, the composite compliance motion determination unit 31b uses these input values to correct the desired slave foot position/posture through compliance control processing, thereby obtaining a corrected desired slave leg motion (corrected desired slave foot position/posture). to decide.

The processing (compliance control processing) of the composite and composite compliance operation determination unit 31b can be generalized as follows. In order to prevent an excessive floor reaction force from acting on the slave device 6 or to prevent the overall attitude of the slave device 1 from collapsing, the actual slave floor reaction force, which is the floor reaction force actually acting on the slave device 1, is controlled. Required state quantities (translational force in a predetermined direction, moment in a predetermined axial direction, position of the center point of the floor reaction force of each leg 6, position of the center point of all floor reaction forces, etc.) is a process of correcting the desired slave foot position/orientation of the overall desired motion of the slave device 1 so as to approach the required desired value defined by the determined desired slave floor reaction force and the like.

In this embodiment, known control processing described in paragraphs 0123 to 0207 of Japanese Patent Application Laid-Open No. 10-277969, for example, is executed as the processing of the composite compliance operation determining section 31b. Therefore, detailed description of the processing of the composite compliance operation determination unit 31b is omitted in this specification. However, in the processing of the composite compliance operation determining section 31b of this embodiment, the "compensating total floor reaction force moment Mdmd" described in Japanese Patent Application Laid-Open No. 10-277969 is set to zero.

In the processing of the composite compliance motion determination unit 31b, both legs 6L and 6R of the slave device 1 are moved around the X-axis and Y-axis around the desired slave total floor reaction force central point (in other words, desired ZMP). and a combined motion of moving the legs 6L and 6R around the X-axis and the Y-axis in directions opposite to each other around the target total floor reaction force center point of the slave. , the desired slave foot position of each foot 6 so that the moment of the actual floor reaction force generated around the desired slave total floor reaction force center point (the moment about the X-axis and the Y-axis) approaches zero. The posture (desired slave foot position/posture determined by the slave movement target determination unit 31a) is corrected. As a result, the corrected target slave foot position/orientation of each foot 6 is determined.

In this case, the amount of correction of the desired slave foot position/orientation is the observed value of the actual slave foot floor reaction force detected by the floor reaction force detector 25 and the desired slave foot floor reaction force determined by the slave movement target determination unit 31a. force (desired slave foot ground reaction force, desired slave foot ground reaction force center point, desired slave total floor reaction force, desired slave total floor reaction force central point).

[Processing of Joint Displacement Determination Unit]
Next, processing of the joint displacement determination unit 31c will be described. As shown in FIG. 2, the joint displacement determination unit 31c stores the desired slave body motion (desired slave body position/posture) determined by the slave motion target determination unit 31a and the correction determined by the composite compliance motion determination unit 31b. A desired slave leg motion (corrected desired slave foot position/posture) is sequentially input. Then, the joint displacement determination unit 31c calculates the target joints of the legs 3 of the slave device 1 from the target slave body position/posture and the target slave leg position/posture of each leg 6 by inverse kinematics calculation. Determine joint displacement.

Although not shown in FIG. 2, in the present embodiment, the joint displacement determination unit 31c further includes the target motion of each arm 10 of the slave device 1 determined by the slave motion target determination unit 31a. , and the desired motion of the head 17 are input. Then, the joint displacement determination unit 31c determines the target joint displacement of each joint of each arm 10 according to the target motion of each arm 10, and each joint of the neck joint mechanism 18 according to the target motion of the head 17. Determine the target joint displacement of

In this case, if the desired motion of each arm 10 is, for example, the desired position/posture of the hand portion 13 of each arm 10 (the desired position/posture relative to the body 2), each A target joint displacement for each joint of arm 10 may be determined. Further, when the desired motion of each arm 10 is composed of, for example, the desired joint displacement of each joint of each arm 10, the desired joint displacement is directly determined as the desired joint displacement of each joint. The same applies to the head 17 as well.

Control processing of each functional unit of the slave control unit 31 is executed as described above. Then, the slave control section 31 outputs the target joint displacement of each joint determined by the joint displacement determination section 31 c to the joint control section 32 . The slave control unit 31 also outputs (transmits) the actual slave lateral body position estimated by the lateral body position estimation unit 31 d to the master control unit 81 via the communication device 33 .

In the present embodiment, since the target slave lateral body position is determined so as to match the actual slave lateral body position, the observed value of the actual slave lateral body position is output to the master control unit 81. Instead of (transmitting), the target slave lateral body body position may be output to the master control unit 81 .

[Regarding the correspondence with the invention]
Here, the corresponding relationship between the present embodiment described above and the present invention will be supplemented. In this embodiment, the movement mechanism 52 and the electric motors 55a and 55b of the master device 51, the lifting mechanism 60 and the slide drive actuator 66 constitute the upper body support section drive mechanism of the present invention. In this case, the electric motors 55a and 55b correspond to the first actuator of the invention, and the slide drive actuator 66 corresponds to the third actuator of the invention.

Further, the desired upper body support movement determination section 81b of the master control section 81 and the slave motion target determination section 31a and the composite compliance motion determination section 31b of the slave control section 31 constitute the motion target determination section of the present invention. In this case, the desired upper body support motion (the desired upper body support position and the desired upper body support orientation) determined by the desired upper body support motion determination unit 81b corresponds to the master-side motion target of the present invention. The 201st processing section of the present invention is implemented by the function of the desired upper body support portion motion determining section 81b determining the desired upper body support portion lateral position.

Also, the target slave body motion, target slave arm motion, target slave head motion, and target slave floor reaction force target slave determined by the slave motion goal determination unit 31a, and correction determined by the composite compliance motion determination unit 31b A set with the target slave leg motion corresponds to the slave side motion target in the present invention.

Further, the lateral position of the body 2 of the slave device 1 corresponds to the lateral position of the slave-side reference part in the present invention, and the lateral position of the body support part 65 (or the lateral position of the operator's P body) is This corresponds to the horizontal position of the master-side reference portion in the present invention. When the lateral position of the upper body support portion 65 is the lateral position of the master-side reference portion, the relationship represented by the above formulas (1a) and (1b) corresponds to the first target correspondence relationship in the present invention, When the lateral position of the upper body of operator P is the lateral position of the master-side reference portion, the left sides of the above equations (1a) and (1b) are replaced with the lateral position of the upper body of operator P. corresponds to the first target correspondence in the present invention.

In addition, the slave motion goal determination unit 31a functions as a first processing unit in the present invention together with the composite compliance motion determination unit 31b, and together with the desired upper body support motion determination unit 81b, functions as the first processing unit in the present invention. It has a function as a second processing unit. Furthermore, the slave operation target determination unit 31a has a function as a third processing unit in the present invention.

In this case, the process of determining the desired slave foot position/orientation by the slave movement target determination section 31a corresponds to the process of the 101st processing section of the first processing section of the present invention, and the target slave The foot position/posture corresponds to the basic desired slave leg position/posture in the present invention. In addition, the processing of the composite compliance motion determination unit 31b corresponds to the processing of the 102nd processing unit of the first processing unit in the present invention, and the corrected desired foot position/posture obtained by this processing is the corrected desired foot position/posture in the present invention. corresponds to the target slave leg position and orientation of .

In addition, the slave movement target determination unit 31a determines a target slave lateral body position (target lateral position of the body 2) and determines a target slave body height (target vertical position of the body 2). and the processing of determining the desired slave body posture (the target posture of the body 2) are the processing of the 202nd processing unit, the processing of the 206th processing unit, and the processing of the 207th processing unit in the present invention. correspond respectively to

In this case, the relationship represented by Equation (27) corresponds to the second target correspondence relationship in the present invention, and the relationship represented by Equation (26) corresponds to the third target correspondence relationship in the present invention. Further, the process of determining the desired slave floor reaction force by the slave movement target determination unit 31a corresponds to the process of the third processing unit in the present invention.

Further, the master movement control section 81a of the master control section 81 corresponds to the master side control section of the present invention, and the joint control section 32 of the slave control section 31 corresponds to the slave side control section of the present invention.

[About actions and effects]
According to the first embodiment described above, basically, as the operator P moves on the floor by walking, the slave device 1 moves on the floor by walking like the operator P. . During this movement, the slave device 1 may lose its posture due to disturbances such as unevenness of the floor surface and contact with obstacles.

Here, in the present embodiment, the desired body support lateral position is defined by the combination of the actual body support lateral position (or actual operator lateral body position) and the actual slave lateral body lateral position (1a ) and (1b) (or the relationship represented by the equations in which the left sides of (1a) and (1b) are replaced by the actual operator lateral positions). In addition, in the processing of the slave movement target determination unit 31a of the slave control unit 31, the desired slave lateral body position is not based on the lateral position of the body of the upper body support unit 65 or the operator P. Determined to match observed values of lateral body position.

Therefore, when the posture of the slave device 1 collapses, the desired body support portion lateral position is determined according to the actual slave lateral body lateral position in the collapsed posture. As a result, the upper body of the operator P receives a lateral translational force (a translational force that tries to break the posture of the operator P in the same way as the slave device 1) in accordance with the collapse of the posture of the slave device 1. It works from For example, when the slave device 1 loses its posture in the forward direction, a forward translational force acts on the upper body of the operator P from the upper body support portion 65 .

As a result, the operator P can appropriately and quickly bodily recognize that the slave device 1 has lost its posture and in which direction the slave device 1 has lost its posture.

[Second embodiment]
Next, a second embodiment of the invention will be described with reference to FIGS. 9 and 10. FIG. The present embodiment differs from the first embodiment only in part of the control processing of the master control unit 81 and the slave control unit 31. FIG. Therefore, in the description of this embodiment, the description of the same items as in the first embodiment will be omitted.

First, referring to FIG. 9, in the present embodiment, a master movement control section 81a2 of the master control section 81, in addition to the functions of the master movement control section 81a of the first embodiment, actually moves the upper body of the operator P. It has a function of estimating the actual operator body lateral position (a pair of X-axis direction position P_opb_x_act and Y-axis direction position P_opb_y_act seen in the master-side global coordinate system Cgm), which is the lateral position.

In this case, the master movement control unit 81a2 detects, for example, the lateral position of the actual body support portion (the X-axis direction position P_mb_x_act and the Y-axis direction position P_mb_y_act) estimated by the processing of STEP 2, and the upper physical strength detector 64. The actual operator Body lateral positions P_opb_x_act, P_opb_y_act are estimated.
P_opb_x_act = P_mb_x_act - kspring_fx * F_mb_x_act
...... (31a)
P_opb_y_act = P_mb_y_act - kspring_fy * F_mb_y_act
... (31b)

Here, kspring_fx and kspring_fy are the same as those shown in the above equations (1a-1) and (1b-1) (reciprocal setting values of spring constants). Supplementally, the method of estimating the actual operator lateral body position is not limited to the above method, and other methods may be employed. For example, it is possible to estimate the actual lateral position of the operator's body by processing a motion capture using a camera that captures the operator P or an inertia sensor attached to the operator's P upper body or the like.

Alternatively, for example, the relative displacement (relative rotation angle) of the operator P's upper body in the yaw direction with respect to the upper body support section 65 may be detected by an appropriate displacement sensor provided in the upper body support section 65 or the like. Then, by adding the observed value of the relative displacement to the actual body support portion orientation θ_mb_z_act, it is possible to estimate the actual operator's body orientation.

Alternatively, for example, it is also possible to use a distance measuring device capable of measuring distances to a plurality of locations on the upper body of the operator P, and estimate the actual operator's upper body orientation based on the distance values observed by the distance measuring device. be. The operator motion detector 70 may detect the actual lateral position of the operator's body.

In this embodiment, the master control unit 81 combines the actual operator's body lateral position estimated as described above with the actual operator's body orientation, the actual operator's foot position/orientation, and the actual operator's foot floor reaction force as command information. , and is output (transmitted) to the slave control unit 31 .

Next, referring to FIG. 10, in this embodiment, the slave control unit 31 includes a compensating floor reaction force determining unit 31e in addition to the functions described in the first embodiment. In the present embodiment, the compensating floor reaction force determination unit 31e determines that the actual slave lateral body positions P_sb_x_act and P_sb_y_act are obtained from the following equations (32a) and (32b) with respect to the actual operator lateral body positions P_opb_x_act and P_opb_y_act. It is a processing unit that determines a floor reaction force that should be additionally applied to the slave device 1 so as to reduce the deviation from the state that satisfies the indicated predetermined correspondence relationship.
P_opb_x_act = Kpmb * P_sb_x_act + Cpmb_x
...... (32a)
P_opb_y_act = Kpmb * P_sb_y_act + Cpmb_y
... (32b)

Here, Kpmb, Cpmb_x, and Cpmb_y in equations (32a) and (32b) are constants of predetermined values shown in equations (1a) and (1b).

In this embodiment, the floor reaction force to be additionally applied to the slave device 1 is, for example, around the desired total floor reaction force center point (in other words, the desired ZMP) in the horizontal direction around the axis (X-axis direction and Y-axis direction). Hereinafter, this moment will be referred to as a compensating total floor reaction force moment. Also, the component of the compensating total floor reaction force moment around the X-axis is denoted by Mdmd_x, and the component around the Y-axis is denoted by Mdmd_y.

The compensating floor reaction force determining unit 31e receives the observed value of the actual operator lateral body position P_opb_x_act received from the master control unit 81 and the actual slave lateral body position P_sb_x_act estimated by the lateral body position estimating unit 31d. are input sequentially.

Then, the compensating floor reaction force determining unit 31e determines the correspondence relationship between the observed value of the actual operator lateral body position P_opb_x_act and the observed value of the actual slave lateral body position P_sb_x_act, using equations (32a) and (32b): The amount of deviation (a set of the amount of deviation Err_x in the X-axis direction and the amount of deviation Err_y in the Y-axis direction) as an index value representing the degree of deviation from the given relationship shown is expressed by the following equation (33a), ( 33b).
Err_x = P_sb_x_act - (P_opb_x_act - Cpmb)/Kpmb
...... (33a)
Err_y=P_sb_y_act-(P_opb_y_act-Cpmb)/Kpmb
... (33b)

The second term on the right side of equation (33a) is the slave lateral body position required to satisfy the relationship of equation (1a) for the observed value of the actual operator lateral body position P_opb_x_act in the X-axis direction. means the X-axis direction position (target position) of . The second term on the right side of equation (33b) is the observed value of the actual operator body lateral position P_opb_y_act in the Y-axis direction, and the slave body lateral position required to satisfy the relationship of equation (21b). It means the Y-axis direction position (target position) of the direction position.

Therefore, the deviation amounts Err_x and Err_y calculated by the equations (33a) and (33b) are, in other words, the observed values of the actual slave lateral body positions P_sb_x_act and P_sb_y_act and the actual operator lateral body positions P_opb_x_act and P_opb_y_act is the deviation from the target value of the lateral body position of the slave that can satisfy the required relationships (target correspondence relationships) expressed by the above equations (32a) and (32b) with respect to the observed value of .

Then, the compensating floor reaction force determination unit 31e determines the compensating total floor reaction force moments M_dmd_x and M_dmd_y so that the deviation amounts Err_x and Err_y converge to zero according to the feedback control law. For example, the compensating floor reaction force determining unit 31e determines the compensating total floor reaction force moments M_dmd_x and M_dmd_y using the PD rule and the following equations (34a) and (34b).
M_dmd_x=Kpdmd*Err_y+Kvdmd*dErr_y/dt
...... (34a)
M_dmd_y=-Kpdmd*Err_x-Kvdmd*dErr_x/dt
... (34b)

Here, Kpdmd and Kvdmd are gains of predetermined values, and dErr_x/dt and dErr_y/dt are differential values (temporal rate of change) of Err_x and Err_y, respectively. Note that the compensating total floor reaction force moment may be determined by a feedback control law other than the PD law (eg, P law, PID law).

Also, the deviation amounts Err_x and Err_y may be determined by the following equations (35a) and (35b) instead of the equations (33a) and (33b).
Err_x = (Kpmb * P_sb_x_act + Cpmb) - P_opb_x_act
...... (35a)
Err_y = (Kpmb * P_sb_y_act + Cpmb) - P_opb_y_act
...... (35b)

In this case, the second term on the right side of equation (35a) is the operator body It means the X-axis position (target position) of the lateral position. In addition, the second term on the right side of equation (35b) is the observed value of the actual slave body lateral position P_sb_y_act in the Y-axis direction, and the operator body lateral direction required to satisfy the relationship of equation (32b). It means the Y-axis direction position (target position) of the direction position.

Therefore, the deviation amounts Err_x and Err_y calculated by the equations (35a) and (35b) are, in other words, the observed values of the actual slave lateral body positions P_sb_x_act and P_sb_y_act, and the equations (32a) and (32b) ) and the observed values of the actual operator lateral body positions P_opb_x_act and P_opb_y_act.

In this embodiment, the compensating total floor reaction force moments M_dmd_x and M_dmd_y determined by the compensating floor reaction force determining section 31e as described above are input to the composite compliance action determining section 31b. Then, in the composite compliance operation determination unit 31b, the moment of the actual floor reaction force generated around the center point of the desired slave total floor reaction force (the moment around the X-axis and the Y-axis) is calculated as a compensating total floor reaction force moment M_dmd_x. , M_dmdy.

This embodiment is the same as the first embodiment except for the matters described above. Here, the corresponding relationship between the present embodiment and the present invention will be supplemented. In this embodiment, the compensating floor reaction force determining section 31e corresponds to the sixth processing section of the present invention, and the composite compliance motion determining section 31b corresponds to the 103rd processing section of the present invention. Correspondence other than this is the same as the correspondence between the first embodiment and the present invention.

According to the present embodiment described above, when the posture of the slave device 1 is disturbed, as in the first embodiment, the upper body of the operator P is provided with a lateral direction corresponding to the deformation of the slave device 1 . can be applied from the upper body support portion 65 . As a result, the operator P can appropriately and quickly perceptually recognize that the slave device 1 has lost its posture and in which direction the slave device 1 has lost its posture.

In addition, each leg 3 of the slave device 1 generates compensating total floor reaction force moments M_dmd_x and M_dmd_y determined so as to bring the deviation amounts Err_x and Err_y closer to zero around the target slave total floor reaction force center point. exercise. Therefore, the slave device 1 can operate so as to reduce the deviation amounts Err_x and Err_y by itself to some extent by adjusting the positions and orientations of the legs 6 . In a situation where the slave device 1 can operate so as to reduce the deviation amounts Err_x and Err_y by itself (in other words, a situation where the slave device 1 can recover from its posture collapse by itself), It is possible to suppress the lateral translational force from acting on the upper body of the operator P from the upper body support portion 65 .

[Third embodiment]
Next, a third embodiment of the invention will be described with reference to FIGS. 11 to 13. FIG. This embodiment differs from the first embodiment only in part of the control processing of the master control section 81 and the slave control section 31 . Therefore, in the description of this embodiment, the description of the same items as in the first embodiment will be omitted.

First, referring to FIG. 11, in the present embodiment, the master control unit 81 controls the target body position of the body 2 of the slave device 1 determined by the slave control unit 31 instead of the actual slave lateral body position. A target slave lateral body position of the motion is received via the communication device 83 . Note that the desired slave body position/posture is determined by a process (details will be described later) different from that of the first embodiment.

Then, in the present embodiment, the desired upper body support part motion determination part 81 b 3 of the master control part 81 determines the desired upper body support part lateral position of the desired upper body support part motion as the target position received from the slave control part 31 . Determined according to the lateral position of the slave body. Specifically, the desired body support portion motion determination unit 81b3 determines that the desired body support portion lateral position satisfies the same relationship as the above equations (1a) and (1b) with respect to the desired slave body lateral position. Determine the target upper body support lateral position as follows: That is, the desired upper body support motion determination unit 81b3 determines the desired upper body support lateral position (P_mb_x_aim, P_mb_y_aim) by the following equations (1a-2) and (1b-2).
P_mb_x_aim=Kpmb*P_sb_x_aim+Cpmb_x
...... (1a-2)
P_mb_y_aim=Kpmb*P_sb_y_aim+Cpmb_y
...... (1b-2)

In this embodiment, the control processing of the master control unit 81 is the same as in the first embodiment except for the items described above.

Next, referring to FIG. 12, in the present embodiment, the slave control unit 31 performs slave control by different processing (processing using the dynamic model of the slave device 1) from the slave operation target determination unit 31a of the first embodiment. A slave motion goal determining unit 31a3 that determines the motion goals of the device 1 (desired slave upper body motion, desired slave leg motion, and desired slave floor reaction force); A virtual external force determination unit 31f that determines a virtual external force to be virtually acted on, a calculation unit 31g that calculates an input to the virtual external force determination unit 31f, a composite compliance operation determination unit 31b described in the first embodiment, and a joint displacement determination and a portion 31c.

In this embodiment, instead of the estimated value of the actual slave lateral body position, the slave control unit 31 determines the desired slave lateral body motion among the desired slave body motions determined by the slave movement target determination unit 31a3. The directional position is transmitted to the master controller 81 via the communication device 33 . Therefore, in the slave controller 31 of the present embodiment, the lateral body position estimator 31d described in the first embodiment is omitted.

The processing of the calculation section 31g, the virtual external force determination section 31f and the slave operation target determination section 31a3 will be specifically described below. These processes are executed at predetermined control processing cycles as follows. The actual slave body inclination estimated by the body posture detector 23 and the target slave body inclination of the target slave body motion determined by the slave motion target determination section 31a3 are input to the calculation section 31g. be. Then, the calculation unit 31g calculates a body inclination deviation, which is the deviation between the actual slave body inclination and the target slave body inclination (=actual slave body inclination-target slave body inclination). The body tilt deviation is composed of a tilt deviation about the X-axis and a tilt deviation about the Y-axis of the slave-side global coordinate system Cgs.

The body inclination deviation calculated by the calculation unit 31g is input to the virtual external force determination unit 31f. Then, the virtual external force determination unit 31f uses a known feedback control law (for example, P law, PD law, PID law, etc.) to converge the body tilt deviation to zero from the input body tilt deviation. Determine external force.

Here, in the present embodiment, the virtual external force is, for example, a moment generated around the center of the desired total floor reaction force of the slave in lateral directions (directions around the X-axis and Y-axis). It is called external force moment. Then, the components of the virtual external force moment around the X-axis and the components around the Y-axis are respectively determined from the components of the body inclination deviation around the X-axis and the components around the Y-axis by the feedback control rule. be.

As in the first embodiment, the slave movement target determination unit 31a3 receives command information (actual operator body posture (orientation, inclination), actual body support height) received by the slave control unit 31 from the master control unit 81. (or observed values of the actual operator's upper body height), the actual operator's foot position/orientation, and the actual operator's foot floor reaction force). Further, in the present embodiment, the virtual external force moment determined by the virtual external force determination unit 31f is input to the slave motion target determination unit 31a3 instead of the actual slave lateral body body position.

Then, the slave operation target determination unit 31a3 executes the processing shown in the flowchart of FIG. 13 at a predetermined control processing cycle. In this case, the slave operation target determination unit 31a3 executes the same processing as the slave operation target determination unit 31a of the first embodiment in STEP11-17. Thereby, a desired slave body motion other than the desired slave lateral body position, a desired slave leg motion, and a desired slave floor reaction force are determined.

Next, in STEP 18a, the slave motion target determination unit 31a3 applies the virtual external force determined by the virtual external force determination unit 31f around the desired slave total floor reaction force central point (desired ZMP) on the dynamic model of the slave device 1. A target slave lateral body position is determined to generate a moment.

As the dynamic model of the slave device 1, for example, the dynamic model described in paragraphs 0128 to 0134 and FIG. 10 of Japanese Patent No. 4246638, or the dynamic model described in Japanese Patent No. 4126061, paragraphs 0163 to 0168 and FIG. A kinetic model similar to these or similar kinetic models may be used. FIG. 14 schematically shows an example dynamic model used in this embodiment. The dynamic model is the same as that described in Japanese Patent No. 4246638.

This dynamic model includes a body mass point Q1, which is a mass point that translates according to the translational motion of the upper body 2 of the slave device 1, and a mass point that translates according to the translational motion of the foot 6 of each leg 3. A leg mass point Q2, a flywheel FH1 that rotates in the roll direction according to the tilting motion of the body 2 in the roll direction (the direction around the longitudinal axis) of the slave device 1, and the pitch direction (the lateral direction) of the slave device 1. and a flywheel FH2 that rotates in the pitch direction according to the tilting motion of the body 2 in the direction around the axis of the flywheel FH2.

A mass is defined in advance for the body mass point Q1 and each leg mass point Q2, and an inertia is defined for the flywheels FH1 and FH2. In this case, the mass of the body mass point Q1 and the masses of the two leg mass points Q2 and Q2 are set so that the total mass of them matches the mass of the slave device 1 as a whole. The position of the body mass point Q1 is defined according to the position (or position and posture) of the body 2, and the position of each leg mass point Q2 is defined according to the position (or position and posture) of the foot 6 of each leg 3. defined according to Note that the flywheels FH1 and FH2 have no mass.

The dynamics of the slave device 1 in this dynamics model are the inertial force (translational inertial force) generated according to the translational acceleration of the body mass point Q1 and each leg mass point Q2, and the body mass point Q1 and each leg mass point An equation expressing the relationship that the total resultant force (translational force) with the gravity acting on each of Q2 is balanced with the translational force of the total floor reaction force acting on the slave device 1, and the resultant force and the flywheel FH1 , FH2, and the total moment generated around an arbitrary point of action (for example, the central point of the desired slave total floor reaction force) acts on the slave device 1. It is expressed by an equation that expresses the relationship that the moment generated around the point of action is balanced by the total floor reaction force.

In this case, the processing of STEP18a can be executed, for example, as follows. In the explanation here, for convenience of explanation, the slave-side global coordinate system Cgs is set such that its X-axis direction is the same or substantially the same as the front-rear direction of the slave device 1 (slave-side global coordinate system Cgs). The direction around the X-axis and the direction around the Y-axis are appropriately updated so that the direction around the X-axis and the direction around the Y-axis correspond to the roll direction and the pitch direction of the slave device 1, respectively, as shown in FIG. . However, coordinate conversion between the slave-side global coordinate system Cgs and a coordinate system whose coordinate axis direction is aligned with the front-rear direction of the slave device 1 can be appropriately performed.

Based on the time series of the target slave body inclination, the rotational angular acceleration of each of the flywheels FH1 and FH2 of the dynamic model is calculated, and the moment of inertia ( A moment corresponding to the inclination of the body, which is the moment of inertia around the X-axis and the Y-axis, is calculated.

Further, the translational acceleration of each leg mass point Q2 of the dynamic model is calculated based on the time series of the desired slave foot position/orientation of each leg 6 of the slave device 1, and each leg mass point Q2 is calculated according to the translational acceleration. A leg motion-corresponding moment, which is a moment generated around the desired slave total floor reaction force stop point by the resultant force of the generated inertial force and the gravitational force acting on each leg mass point Q2, is calculated.

Also, the translational acceleration in the vertical direction (Z-axis direction) of the body mass point Q1 of the dynamic model is calculated based on the time series of the target slave body height. The vertical translational acceleration of the body mass point Q1 is calculated from the vertical inertial force generated by the body mass point Q1 in accordance with the translational acceleration and the time series of the desired slave foot position/orientation. The resultant force of the vertical inertial force generated by each leg mass point Q2 in response to the vertical translational acceleration of the leg mass point Q2 and the gravitational force acting on the entire center of gravity of the slave device 1 is the desired slave total floor reaction force in the vertical direction. may be calculated so as to balance the translational force of

Then, with the lateral translational acceleration of the body mass point Q1 as an unknown, the inertial force generated in accordance with the lateral translational acceleration of the body mass point Q1 and the vertical translational acceleration of the body mass point Q1, and A moment corresponding to body motion, which is a moment generated around the center point of the desired total floor reaction force of the slave due to the resultant force with the gravity acting on the body mass point Q1, the moment corresponding to body inclination, and the moment corresponding to leg motion. Lateral translation of the body mass point Q1 so as to satisfy the condition that the components of the resultant force moment in the directions around the X axis and the direction around the Y axis match the virtual external force moment determined by the virtual external force determination unit 31f. Acceleration is calculated.

Then, by integrating (second-order integration) the lateral translational acceleration of the body mass point Q1, the lateral position of the body mass point is determined. A directional position is determined.

In STEP 18a, the processing described above is performed so that the virtual external force moment determined by the virtual external force determination unit 31f is generated around the desired slave total floor reaction force center point (desired ZMP) on the dynamic model (in detail , the resultant force of the inertial force generated by the motion of the slave device 1 and the gravitational force acting on the slave device 1 causes the moment generated around the desired slave total floor reaction force central point (desired ZMP) to match the virtual external force moment. ), a target slave lateral body position is determined.

This embodiment is the same as the first embodiment except for the matters described above. Here, the corresponding relationship between the present embodiment and the present invention will be supplemented. In this embodiment, the virtual external force determination unit 31f of the slave control unit 31 corresponds to the fourth processing unit in the present invention, and the body inclination deviation input to the virtual external force determination unit 31f corresponds to the first deviation in the present invention. The virtual external force moment determined by the virtual external force determination unit 31f corresponds to the first virtual external force in the present invention.

Further, among the processes of the slave movement target determination section 31a3, the process of determining the desired slave floor reaction force corresponds to the process of the third processing section in the present invention. Among the processes of the slave movement target determination section 31a3, the process of determining the desired slave lateral body body position corresponds to the process of the 203rd processing section of the present invention, and the dynamic model (Fig. 14 ) corresponds to the first dynamic model in the present invention. Correspondences other than the above are the same as those between the first embodiment and the present invention.

According to the present embodiment described above, when the posture of the slave device 1 is disturbed, as in the first embodiment, the upper body of the operator P is provided with a lateral direction corresponding to the deformation of the slave device 1 . can be applied from the upper body support portion 65 . As a result, the operator P can appropriately and quickly perceptually recognize that the slave device 1 has lost its posture and in which direction the slave device 1 has lost its posture.

In addition, in the present embodiment, since the target slave body motion can be determined using the dynamic model, the slave control unit 31 may become the master due to communication disconnection or communication failure between the master control unit 81 and the slave control unit 31. Even if command information cannot be acquired from the control unit 81, for example, the target motion of the slave device 1 for stopping the movement of the slave device 1 is appropriately determined (dynamically feasible). It is possible.

[Fourth embodiment]
A fourth embodiment of the present invention will now be described with reference to FIG. It should be noted that this embodiment differs from the third embodiment only in a part of the control processing of the slave control unit 31 . Therefore, in the description of this embodiment, the description of the same matters as those of the third embodiment will be omitted.

Referring to FIG. 15, a slave control unit 31 of this embodiment includes a compensating floor reaction force determining unit 31h in addition to the functions described in the third embodiment. In this embodiment, the compensating floor reaction force determination unit 31h determines that the actual slave body inclination estimated by the body posture detector 23 deviates from the target slave body inclination determined by the slave motion target determination unit 31a3. In this case, the processing unit determines the floor reaction force to be additionally applied to the slave device 1 so as to reduce the deviation.

In the present embodiment, the floor reaction force to be additionally applied to the slave device 1 is calculated around the desired total floor reaction force center point (desired ZMP) in the same manner as the compensating total floor reaction force moment described in the second embodiment. is the moment generated in the lateral direction around the axis (direction around the X-axis and direction around the Y-axis). Therefore, also in the present embodiment, the floor reaction force determined by the compensating floor reaction force determining section 31h is referred to as the compensating total floor force moment.

The compensating floor reaction force determination unit 31h of the present embodiment stores the body inclination deviation calculated by the calculation unit 31g as the actual slave body inclination relative to the target slave body inclination determined by the slave movement target determination unit 31a3. Input as a deviation amount. Then, the compensating floor reaction force determining section 31h determines the compensating total floor reaction force moment according to the input body tilt deviation so that the body tilt deviation converges to zero by the feedback control rule.

For example, similarly to the compensating floor reaction force determining unit 31e of the second embodiment, the compensating total floor reaction force moment is determined using the PD rule. In this case, each of Err_x and Err_y in the above equations (34a) and (34b) is a value (-1) times (= −θerr_x), the compensating total floor reaction force moments M_dmd_x and M_dmd_y can be determined. Note that the compensating total floor reaction force moment may be determined by a feedback control law other than the PD law (eg, P law, PID law).

The compensating total floor reaction force moment determined by the compensating floor reaction force determining section 31h as described above is input to the composite compliance motion determining section 31b. Then, in the composite compliance operation determination unit 31b, the moment of the actual floor reaction force generated around the center point of the desired slave total floor reaction force (the moment around the X-axis and the Y-axis) is converted into the compensating total floor reaction force moment. The desired slave foot position/orientation of each foot 6 (the desired slave foot position/orientation determined by the slave movement target determination unit 31a) is corrected so as to bring them closer together.

This embodiment is the same as the third embodiment except for the matters described above. Here, the corresponding relationship between the present embodiment and the present invention will be supplemented. In this embodiment, the compensating floor reaction force determining section 31h corresponds to the seventh processing section of the present invention, and the composite compliance motion determining section 31b corresponds to the 103rd processing section of the present invention. Correspondence other than this is the same as the correspondence between the third embodiment and the present invention.

According to the present embodiment described above, when the posture of the slave device 1 is disturbed, as in the first embodiment, the upper body of the operator P is provided with a lateral direction corresponding to the deformation of the slave device 1 . can be applied from the upper body support portion 65 . As a result, the operator P can appropriately and quickly perceptually recognize that the slave device 1 has lost its posture and in which direction the slave device 1 has lost its posture.

In addition, each leg 3 of the slave device 1 is adjusted so as to generate compensating total floor reaction force moments M_dmd_x and M_dmd_y determined so as to bring the body inclination deviation closer to zero around the desired slave total floor reaction force center point. You can exercise. Therefore, the slave device 1 can operate so as to reduce the body inclination deviation by itself to some extent by adjusting the position and orientation of each leg 6 . Then, in a situation where the slave device 1 can operate so as to reduce the upper body inclination deviation by itself (in other words, a situation where the slave device 1 can recover its collapsed posture by itself), the operator It is possible to suppress the lateral translational force from acting on P's upper body from the upper body support portion 65 .

[Fifth embodiment]
Next, a fifth embodiment of the present invention will be described with reference to FIGS. 16-19. This embodiment differs from the first embodiment only in part of the control processing of the master control unit 81 and the slave control unit 31 . Therefore, in the description of this embodiment, the description of the same items as in the first embodiment will be omitted.

Referring to FIG. 16, in the present embodiment, the master control unit 81 calculates the reaction force (specifically, the translational force) that the operator P receives from the upper body support portion 65 instead of the desired upper body support portion motion determination unit 81b. A desired upper body support reaction force determination unit 81c having a function of determining a desired upper body support reaction force, which is a target value of . Further, in the present embodiment, the master movement control section 81a5 of the master control section 81 corresponds to each movement contact portion 54 using the desired upper body support reaction force determined by the desired upper body support reaction force determination portion 81c. It is configured to control the electric motors 55 a and 55 b of the moving drive mechanism 55 and the slide drive actuator 66 of the lifting mechanism 60 .

The master control unit 81 of this embodiment, which has the master movement control unit 81a5 and the desired upper body support reaction force determination unit 81c, executes the process shown in the flowchart of FIG. 17 at a predetermined control process cycle. In STEP 1a, the master control unit 81 uses the desired body support reaction force determination unit 81c to determine the desired body support reaction force (lateral and vertical translational forces).

In this case, the desired body support reaction force determination unit 81c stores the actual slave body lateral position transmitted from the slave control unit 31 to the master control unit 81, and The actual operator's body lateral direction position estimated by the processing of STEP 5a, which will be described later, is input.

Then, the desired body support reaction force determination unit 81c determines the relationship between the actual slave body lateral position and the actual operator body lateral position using the formulas (32a) and (32b) described in the second embodiment. ), which is the translational force in the lateral direction (X-axis direction and Y-axis direction) of the desired body support reaction force so as to converge to a state that satisfies the predetermined relationship shown by determine power.

Specifically, the desired body support reaction force determination unit 81c determines the correspondence relationship between the observed values of the actual operator lateral body positions P_opb_x_act and P_opb_y_act and the observed values of the actual slave lateral body positions P_sb_x_act and P_sb_y_act. , equations (32a) and (32b) (the set of the deviation amount Err_x in the X-axis direction and the deviation amount Err_y in the Y-axis direction) from the predetermined relationship, for example, in the second embodiment It is calculated by the formulas (35a) and (35b) described in .

It should be noted that the deviation amounts Err_x and Err_y may be calculated by the formulas (33a) and (33b). Further, instead of the observed values of the actual operator body lateral positions P_opb_x_act and P_opb_y_act, the observed values of the actual body support portion lateral positions P_mb_x_act and P_mb_y_act are used to obtain equations (35a) and (35b) or equation (33a). ) and (33b) by replacing P_opb_x_act and P_opb_y_act with P_mb_x_act and P_mb_y_act, respectively, to calculate the deviation amounts Err_x and Err_y.

Then, the desired body support portion reaction force determination unit 81c controls the desired body support portion lateral translational force (X-axis direction translational force F_mb_x_aim and X-axis direction translational force F_mb_x_aim and The Y-axis direction translational force F_mb_y_aim is determined.For example, the desired body support reaction force determination unit 81c uses the PD rule to obtain the desired body support lateral translational force F_mb_y_aim using the following equations (40a) and (40b): Determine F_mb_x_aim, F_mb_y_aim.
F_mb_x_aim=Kpfvir*Err_x+Kvfvir*dErr_x/dt
...... (40a)
F_mb_y_aim=Kpfvir*Err_y+Kvfvir*dErr_y/dt
...... (40b)

Here, Kpfvir and Kvfvir are gains of predetermined values, and dErr_x/dt and dErr_y/dt are differential values (temporal rate of change) of Err_x and Err_y, respectively. Note that the desired body support portion lateral translational force may be determined by a feedback control law other than the PD law (for example, the P law, the PID law).

Further, the desired body support reaction force determining unit 81c determines a desired body support vertical translational force F_mb_z_aim, which is the vertical translational force of the desired body support reaction force, as shown in the following equation (41). As shown, it is set to a predetermined value Cz. Cz is the target value of the upward translational force exerted on the operator P from the upper body support 65 in order to reduce the load on the operator P's legs, as described with respect to equation (3) above. However, Cz may be zero.
F_mb_z_aim=Cz ……(41)

In STEP 1a, the desired upper body support reaction force determination unit 81c also executes processing for determining the desired upper body support orientation. In this case, the processing for determining the desired upper body support portion orientation is the same as the processing in STEP 1 of the first embodiment. That is, the desired upper body support orientation is determined so that the actual body support yaw moment detected by the upper body strength detector 64 converges to zero. Alternatively, the actual operator body orientation may be estimated by an appropriate method such as motion cabcha, and the estimated value may be determined as the desired upper body support portion orientation.

In STEP 1a, the desired upper body support reaction force (F_mb_x_aim, F_mb_y_aim, F_mb_z_aim) and the desired upper body support direction are determined as described above.

Next, the master control section 81 executes the processing of STEP2 to STEP5a by the master movement control section 81a5. In STEP 2, the master movement control section 81a5 uses the actual motor rotation angle detected by the motor rotation detector 56 and the actual slide displacement detected by the slide displacement detector 67 to determine the actual body support portion motion (actual Estimate the body support position and actual body support orientation). This processing is the same as in the first embodiment.

Next, in STEP 3a, the master movement control unit 81a5 realizes the lateral translational force of the desired body support reaction force (desired body support lateral translational force) and the desired body support orientation. A target translational speed of each moving grounding portion 54 is determined, and the electric motors 55a and 55b corresponding to each moving grounding portion 54 are controlled so as to achieve the target translational speed.

Specifically, the master movement control unit 81a5 controls the lateral direction translational force of the desired upper body support determined in STEP1a (the X-axis translational force F_mb_x_aim and the Y-axis translational force F_mb_y_aim), and the upper physical strength detector 64 Acquire the actual body support horizontal translational force (X-axis translational force F_mb_x_act and Y-axis translational force F_mb_y_act), which is the horizontal translational force of the actual body support reaction force detected in . Note that the horizontal translational forces F_mb_x_act and F_mb_y_act of the actual body support are obtained by converting the detection values of the translational force seen in the local coordinate system set for the upper physical strength detector 64 to the values seen in the master-side global coordinate system. obtained by conversion.

Then, the master movement control unit 81a5 reduces the deviation to zero according to the feedback control rule according to the deviation between the desired body support portion lateral translational force F_mb_x_aim, F_mb_y_aim and the actual body support portion lateral translational force F_mb_x_act, F_mb_y_act. , a target body support lateral velocity (X-axis velocity V_mb_x_aim and Y-axis velocity V_mb_y_aim), which is a target lateral translational velocity of the body support part 65, is determined. For example, the master movement control section 81a5 determines the desired upper body support portion lateral velocities V_mb_x_aim and V_mb_y_aim by the following equations (42a) and (42b) using the P-law.
V_mb_x_aim = Kpdr * (F_mb_x_aim - F_mb_x_act)
...... (42a)
V_mb_y_aim = Kpdr * (F_mb_y_aim - F_mb_y_act)
... (42b)

Here, Kpdr is a gain of a predetermined value. Note that the desired upper body support portion lateral velocity may be determined by a feedback control law other than the P law (for example, PD law, PID law, etc.).

Then, the master movement control unit 81a5 converts the desired body support lateral velocities V_mb_x_aim, V_mb_y_aim determined as described above to an angle (=- θ_mb_z_act) in the yaw direction (direction around the Z-axis) to obtain the desired upper body support lateral velocities V_mb_local_x_aim and V_mb_local_y_aim in the respective axial directions of the Xm-axis direction and the Ym-axis direction of the master coordinate system Cm. .

Further, the master movement control unit 81a5, similarly to the processing in STEP3 of the first embodiment, estimates the desired body support portion orientation θ_mb_z_aim determined in STEP1a and the actual body support portion orientation θ_mb_z_act obtained in STEP2. , the target body, which is the target angular velocity in the yaw direction of the body support section 65, is adjusted so that the deviation converges to zero by a feedback control law (for example, P law, PD law, PID law, etc.) Determine the support portion local yaw angular velocity ω_mb_local_z_aim.

For example, the actual body support portion yaw moment M_mb_z_act detected by the upper physical strength detector 64 is converged to zero by a feedback control law (for example, P law, PD law, PID law, etc.). A desired body support local yaw angular velocity ω_mb_local_z_aim may be determined according to the observed value of the yaw moment M_mb_z_act. In this case, the process of determining the desired orientation of the upper body support portion in STEP1a can be omitted.

After that, the master movement control unit 81a5 calculates the equations (9a), (9b), (10a), (10b), (11a), and (11b) in the same manner as in the processing of STEP 3 of the first embodiment. After processing, the target motor driving forces Tq_mw_mota_aim(n) and Tq_mw_motb_aim(n) of the electric motors 55a and 55b corresponding to the respective moving ground contact portions 54(n) (n=1, 2, 3, 4) are determined. .
Further, the master movement control section 81a5 controls the electric motor corresponding to each movement grounding section 54(n).

Each of 55a and 55b is operated to output target motor drive forces Tq_mw_mota_aim(n) and Tq_mw_motb_aim(n). As a result, movement control of the moving mechanism 52 is performed so that the desired body support portion lateral translational forces F_mb_x_aim, F_mb_y_aim and the desired body support portion orientation θ_mb_z_aim are realized.

Next, in STEP 4a, the master movement control unit 81a5 controls the slide drive actuator 66 so as to achieve the vertical translational force (actual body support vertical translational force) of the target body support reaction force. do. Specifically, the master movement control section 81a5 determines the vertical translational force of the actual body support portion detected by the upper body strength detector 64 and the desired vertical translational force of the body support portion determined in STEP 1a (=Cz). , the desired translation in the vertical direction (Z-axis direction) of the body support part 65 so as to converge the deviation to zero by a feedback control law (for example, P law, PD law, PID law, etc.) A target upper body support vertical velocity V_mb_z_aim is determined.

Then, the master movement control unit 81a5 calculates the time rate of change of the desired body support portion vertical velocity V_mb_z_aim determined as described above and the actual body support portion height P_mb_z_act obtained in STEP 2 through differentiation processing. According to the deviation from the actual vertical translation speed of the body support portion 65, V_mb_z_act, the deviation is calculated by a feedback control law (for example, P law, PD law, PID law, etc.). is determined to converge to zero.

Then, the master movement control section 81a5 controls the slide drive actuator 66 so as to generate this target drive force. As a result, the slide drive actuator 66 is controlled such that the target upper body support portion up-down translational force is achieved.

Next, in STEP5a, the master movement control section 81a5 estimates the actual operator's body direction and the actual operator's body lateral direction position. In this case, the process of estimating the actual operator's body orientation is the same as the process of STEP4 in the first embodiment. That is, the actual operator's body orientation is estimated by the calculation of the above equation (12).

Further, the process of estimating the actual operator's lateral body position is the same as the process described in the second embodiment. That is, the actual operator's body lateral position is estimated by the formulas (31a) and (31b).

As described above, the master control unit 81 executes the processing of the master movement control unit 81a5 (the processing of STEP2 to STEP5a), and then, in STEP6a, transmits command information regarding the operation of the slave device 1 to the slave control unit 31. .

Specifically, as in the processing of STEP 6 of the first embodiment, the master control unit 81 controls the actual operator body posture (orientation and inclination), the actual operator foot position and posture, the actual operator foot floor reaction force, and The actual body support portion height is transmitted to the slave control unit 31 as a component of the command information. In addition, in this embodiment, the master control unit 81 also transmits the observed value of the actual operator's lateral body position estimated in STEP 5a to the slave control unit 31 as a component of the command information.

As supplemented with respect to the processing of STEP 6 of the first embodiment, the command information (observed value) output (transmitted) from the master control unit 81 to the slave control unit 31 is a filtered value obtained by filtering such as a low-pass filter. may be Also, an observed value of the actual operator's body height may be transmitted to the slave control unit 31 instead of the actual body support height.
In this embodiment, the control processing of the master control unit 81 is executed as described above.

Next, referring to FIG. 18, the slave control unit 31 of the present embodiment includes a composite compliance motion determining unit 31b, a joint displacement determining unit 31c, and a lateral body position estimating unit 31d described in the first embodiment. On the other hand, there is provided a slave operation target determination unit 31a5 that partially differs from the slave operation target determination unit 31a of the first embodiment.

The slave operation target determination unit 31a5 stores the actual operator body posture (orientation, inclination), the actual operator body lateral position, the actual body position, and the actual operator body posture (orientation, inclination) received by the slave control unit 31 from the master control unit 81 via the communication device 33. Observed values (detected values or estimated values) of the support height (or the actual operator's upper body height), the actual operator's foot position/orientation, and the actual operator's foot floor reaction force are input.

Here, in the present embodiment, the actual slave lateral body position estimated by the lateral body position estimator 31d is not input to the slave motion target determiner 31a5. Then, as an input value instead of the slave lateral body position, the observed value of the actual operator lateral body position is input to the slave motion target determination unit 31a5.

Then, the slave operation target determination unit 31a5 executes the process shown in the flowchart of FIG. 19 at a predetermined control process cycle. In this case, the slave operation target determination unit 31a5 executes the same processing as in the first embodiment in STEP11-17. As a result, the desired slave body motion (desired slave body posture and desired slave body height) other than the desired slave lateral body position, the desired slave leg motion, and the desired slave floor reaction force are the same as in the first embodiment. determined in the same way.

Next, in STEP 18 b , the slave movement target determination unit 31 a 5 determines the target slave lateral body position according to the observed value of the actual operator lateral body position received from the master control unit 81 .

Specifically, the slave motion target determination unit 31a5 determines that the actual operator body lateral direction position and the actual slave body lateral direction position are represented by the equations (32a) and (32b) described in the second embodiment. A target slave lateral body position is determined according to the observed value of the actual operator lateral body position, with the goal of satisfying a predetermined relationship given by the operator.

That is, the slave motion target determining unit 31a5 calculates the desired slave lateral body position from the X-axis position P_opb_x_act and the Y-axis position P_opb_y_act of the actual operator lateral body position by the following equations (50a) and (50b). Determine the X-axis position P_sb_x_aim and the Y-axis position P_sb_y_aim.
P_sb_x_aim = (P_opb_x_act - Cpmb) / Kpmb
...... (50a)
P_sb_y_aim = (P_opb_y_act - Cpmb) / Kpmb
...... (50b)

The processing of the slave operation target determination unit 31a5 of this embodiment is executed as described above. This embodiment is the same as the first embodiment except for the matters described above. Here, the corresponding relationship between the present embodiment and the present invention will be supplemented. In the present embodiment, among the processes of the slave movement target determination unit 31a5, the process of determining the desired slave body lateral position corresponds to the process of the 204th processing unit of the present invention. The internal reaction force determination unit 81c corresponds to the 205th processing unit in the present invention. Correspondence other than this is the same as the correspondence between the first embodiment and the present invention.

According to the present embodiment described above, when the posture of the slave device 1 is disturbed, as in the first embodiment, the upper body of the operator P is provided with a lateral direction corresponding to the deformation of the slave device 1 . can be applied from the upper body support portion 65 . As a result, the operator P can appropriately and quickly perceptually recognize that the slave device 1 has lost its posture and in which direction the slave device 1 has lost its posture.

[Sixth Embodiment]
A sixth embodiment of the present invention will now be described with reference to FIG. Note that this embodiment differs from the fifth embodiment only in part of the control processing of the slave control unit 31 . Therefore, in the description of this embodiment, the description of the same matters as those of the fifth embodiment will be omitted.

Referring to FIG. 20, in this embodiment, a slave control unit 31 includes a compensating floor reaction force determination unit 31e described in the second embodiment in addition to the functions of the slave control unit 31 of the fifth embodiment. ing. As described in the second embodiment, the compensating floor reaction force determination unit 31e determines the correspondence relationship between the observed value of the actual operator lateral body position P_opb_x_act and the observed value of the actual slave lateral body position P_sb_x_act. According to the deviation amounts Err_x and Err_y from the predetermined relationship shown by the equations (32a) and (32b), the compensating total floor reaction force moment is calculated according to the feedback control rule (for example, by the calculation of the equations (34a) and (34b)). to decide.

Then, using this compensating total floor reaction force moment, the processing of the composite compliance operation determination unit 31b is executed in the same manner as in the second embodiment. This embodiment is the same as the fifth embodiment except for the matters described above.

Here, the corresponding relationship between the present embodiment and the present invention will be supplemented. In this embodiment, the compensating floor reaction force determining section 31e corresponds to the sixth processing section of the present invention, and the composite compliance motion determining section 31b corresponds to the 103rd processing section of the present invention. Correspondence other than this is the same as the correspondence between the fifth embodiment and the present invention.

According to the present embodiment described above, when the posture of the slave device 1 is disturbed, as in the first embodiment, the upper body of the operator P is provided with a lateral direction corresponding to the deformation of the slave device 1 . can be applied from the upper body support portion 65 . As a result, the operator P can appropriately and quickly perceptually recognize that the slave device 1 has lost its posture and in which direction the slave device 1 has lost its posture.

In addition, each leg 3 of the slave device 1 generates compensating total floor reaction force moments M_dmd_x and M_dmd_y determined so as to bring the deviation amounts Err_x and Err_y closer to zero around the target slave total floor reaction force center point. exercise. Therefore, as in the second embodiment, the slave device 1 can operate to reduce the body tilt deviation by itself (in other words, the slave device 1 can prevent its posture from collapsing). In the situation where the operator P can recover by himself/herself, it is possible to suppress the lateral translational force acting on the upper body of the operator P from the upper body support portion 65 .

[Seventh Embodiment]
A seventh embodiment of the present invention will now be described with reference to FIGS. 21A to 25. FIG. This embodiment differs from the first embodiment in part of the mechanism of the master device 51 and part of the control processing of the master control unit 81 and the slave control unit 31. The description of the same matters as those in the above is omitted.

21A and 21B, in this embodiment, the master device 51 includes a body support section 65' having a structure different from that of the body support section 65 of the first embodiment. The upper body support portion 65′ is semicircular (or U-shaped) attached to the slide member 62 so as to be rotatable in the roll direction (the direction around the Xm axis of the master coordinate system Cm) with respect to the slide member 62. ), a backrest member 65b attached to the outer member 65a so as to be rotatable in the pitch direction (direction around the Ym axis of the master coordinate system Cm) with respect to the outer member 65a, and the outer member 65a. It has a first actuator 68 that rotates in the roll direction and a second actuator 69 that rotates the backrest member 65b in the pitch direction.

The outer member 65 a is attached to the slide member 62 via the first actuator 68 and the upper force detector 64 . In this case, the first actuator 68 is configured by, for example, an electric motor, and the upper body strength detection is performed with the rotation axis of the drive shaft 68a directed in the front-rear direction of the master device 51 (the Xm-axis direction of the master coordinate system Cm). It is attached to the slide member 62 via the container 64 . A central portion of the outer member 65a is fixed to the drive shaft 68a of the first actuator 68. As shown in FIG. As a result, the outer member 65a can be rotationally driven in the roll direction by the driving force of the first actuator 68. As shown in FIG. The first actuator 68 is hereinafter referred to as a first backrest drive actuator 68 .

The backrest member 65b is formed in a semi-cylindrical shape having a semi-arcuate (or U-shaped) transverse cross section, extends generally vertically, and has an inner peripheral surface facing forward of the master device 51. It is arranged inside the outer member 65a so as to face. The inner peripheral surface of the backrest member 65b is a surface on which the operator P can follow the back surface of the upper body.

Both left and right ends of the lower portion of the backrest member 65b are connected to respective ends of the outer member 65a via a support shaft 65c having an axial center in the left-right direction of the master device 51 (the Ym-axis direction of the master coordinate system Cm). pivoted on. This allows the backrest member 65b to rotate in the pitch direction with respect to the outer member 65a.

Also, the second actuator 69 is configured by, for example, an electric motor. The second actuator 69 is attached to the outer member 65a on the axial center of the support shaft 65c on the one end side of the outer member 65a, and its drive shaft (not shown) connects the backrest member 65b to the support shaft 65c. is connected to the backrest member 65b so as to be rotatably driven (in the pitch direction) about its axis. The second actuator 69 is hereinafter referred to as a second backrest drive actuator 69 .

In the upper body support section 65′ having such a configuration, the backrest member 65b corresponds to the upper body support section 65 in the first embodiment, and the backrest member 65b is provided on the upper body of the operator P (for example, the waist and upper part thereof). It is attached via a belt or the like (not shown) so as to be along from the side. An elastic member such as a pad (not shown) interposed between the upper body of the operator P is attached to the inner peripheral surface of the backrest member 76b.

In a state where the upper body support portion 65' is attached to the upper body of the operator P, a rotational force in the roll direction is applied from the first backrest drive actuator 68 to the upper body of the operator P via the outer member 65a and the backrest member 65b. It is possible to apply a rotational force in the direction of tilting the upper body left and right. In addition, it is possible to apply a rotational force in the pitch direction (rotational force in the direction of tilting the upper body back and forth) to the upper body of the operator P from the second backrest drive actuator 69 via the backrest member 65b.

Power transmission from the first backrest drive actuator 68 to the outer member 65a and power transmission from the second backrest drive actuator 69 to the backrest member 65b can be performed via an arbitrary power transmission mechanism such as a reduction gear. is. The first backrest drive actuator 68 and the second backrest drive actuator 69 are not limited to electric motors, and hydraulic actuators can also be used.

Next, referring to FIG. 22, the master control unit 81 of the present embodiment includes a backrest control unit 81d that controls the operation of the first backrest drive actuator 68 and the second backrest drive actuator 69 of the upper body support part 65', A target body inclination determination unit 81e for determining a target operator body inclination, which is a target value of the operator P's upper body inclination (inclinations in the directions around the X-axis and the Y-axis). Although not shown in FIG. 22, the master control unit 81 of the present embodiment includes the same master movement control unit 81a and desired upper body support motion determination unit 81b as in the first embodiment.

In this embodiment, the master control unit 81 can receive the observed value of the actual slave body inclination from the slave control unit 31 via the communication device 83 . Also, the master control unit 81 omits transmission of observed values of the actual operator's body posture (orientation and inclination) to the slave control unit 31 . Although FIG. 22 omits detailed illustration of transmission/reception data between the master control unit 81 and the slave control unit 31, the actual slave body inclination and the actual operator body posture (orientation and inclination) The transmission/reception data other than the observed values of are the same as in the first embodiment.

In the present embodiment, the master control section 81 executes the control processing of the desired upper body support portion exercise determination section 81b and the master movement control section 81a in the same manner as in the first embodiment, and simultaneously performs the desired body inclination. The control processing of the determination unit 81e and the backrest control unit 81d is executed at a predetermined control processing cycle.

In this case, the observed value of the actual slave body inclination received by the master control unit 81 from the slave control unit 31 is input to the desired body inclination determination unit 81e. Then, the desired body inclination determining unit 81e determines that the actual operator body inclination has a predetermined relationship (specifically, The target operator's body inclination is determined so as to converge to a state that satisfies the relation regarding the component of the inclination in Equation (26). That is, the target operator body inclination determination unit 81e calculates the following equations (55a) and (55b) from the observed values of the input real slave body inclination θ_sb_x_act in the direction around the X-axis and the inclination θ_sb_y_act in the direction around the Y-axis. Then, the inclination θ_opb_x_aim around the X-axis and the inclination θ_opb_y_aim around the Y-axis of the target operator's body inclination are determined.
θ_opb_x_aim = θ_sb_x_act (55a)
θ_opb_y_aim = θ_sb_y_act (55b)

Note that the predetermined relationship between ↑θ_sb_act and ↑θ_opb_act may be, for example, a relationship represented by a linear function in the same form as Equation (1a) or Equation (1b). In that case, θ_opb_x_aim and θ_opb_y_aim are respectively θ_opb_x_act. Determined by a linear function of θ_opb_x_act.

The backrest control unit 81d receives input of the target operator's body inclination determined by the target body inclination determination unit 81e and the actual operator's body inclination estimated by the operator movement detector 70 described in the first embodiment. be. Then, the backrest control unit 81d converts the reference operator's upper body inclination and the observed value of the actual operator's upper body inclination in the master-side global coordinate system Cgm to the actual body support part estimated by the master movement control unit 81a. Accordingly, the coordinates are transformed into values seen in the master coordinate system Cm (see FIGS. 3 and 4).

Furthermore, the backrest control unit 81d uses a feedback control rule (for example, P law, PD law, PID law) to calculate the deviation between the apparent operator's upper body inclination seen in the master coordinate system Cm and the actual operator upper body inclination seen in the master coordinate system Cm. etc.), the target driving force of each of the first backrest driving actuator 68 and the second backrest driving actuator 69 is determined according to the calculated value of the deviation so as to approach zero.

In this case, more specifically, the target driving force of the first backrest drive actuator 68 is determined according to the deviation between the target operator's body inclination and the actual operator's body inclination in the direction (roll direction) around the Xm axis of the master coordinate system Cm. is determined, and the target driving force of the second backrest drive actuator 69 is determined according to the deviation between the target operator's body inclination and the actual operator's body inclination in the direction (pitch direction) around the Ym axis of the master coordinate system Cm. be.

Then, the backrest control unit 81d operates each of the first backrest drive actuator 68 and the second backrest drive actuator 69 so as to output the target driving force determined as described above. As a result, the inclinations of the backrest member 65b of the upper body support part 65' in the roll direction and the pitch direction are adjusted so that the actual operator's body inclination approaches the target operator's body inclination determined corresponding to the actual slave body inclination. controlled.

Now refer to FIG. The slave control unit 31 includes a composite compliance motion determining unit 31b, a joint displacement determining unit 31c, and a body lateral position estimating unit 31d, which are the same as those in the first embodiment. Furthermore, there is provided a slave motion target determination unit 31a7 that determines motion targets (desired slave upper body motion, desired slave leg motion, and desired slave floor reaction force) of the slave device 1 by different processing.

Then, the slave control unit 31 transmits the actual slave lateral body position estimated by the lateral body position estimation unit 31 d and the actual slave body inclination estimated by the body posture detector 23 to the master control unit 81 . can be sent sequentially to

The slave operation target determination unit 31a7 of the slave control unit 31 of the present embodiment receives the actual body support portion height (or actual operator body height) received from the master control unit 81 via the communication device 33, the actual operator Observed values of the foot position/orientation and the actual operator foot floor reaction force are input, and the actual slave lateral body position estimated by the lateral body position estimator 31d is also input.

Then, the processing of the slave operation target determination unit 31a7 is executed as shown in the flowchart of FIG. 24 at a predetermined control processing cycle. Specifically, in STEP11 to STEP15 and STEP17, the slave movement target determination unit 31a7 performs the same processing as in the first embodiment, thereby determining the desired slave foot position/orientation, the desired slave floor reaction force, and the desired slave body height. determine the Note that the target slave upper body height may be set to a predetermined value, for example, regardless of the actual body support portion height (or the actual operator upper body support portion height).

Next, in STEP 19, the slave motion target determination unit 31a7 determines that the lateral moment generated around the desired slave total floor reaction force central point (desired ZMP) becomes zero on the dynamic model of the slave device 1, and , so that the lateral translational force (translational force in the X-axis direction and Y-axis direction) of the desired total floor reaction force can be realized, the desired slave body lateral position and desired body posture (inclination and orientation) are set to decide.

In this case, as the dynamic model of the slave device 1, for example, the dynamic model described in paragraphs 0163 to 0168 and FIG. 12 of Japanese Patent No. 4126061, or a similar dynamic model can be used. .

FIG. 25 schematically shows an example dynamic model used in this embodiment. This dynamics model has a body mass point Q1, a leg mass point Q2, flywheels FH1 and FH2, as well as the dynamics model (FIG. 14) described in the third embodiment. It further has a flywheel FH3 that rotates in the yaw direction according to the rotational motion of the body 2 in the yaw direction (the direction around the vertical axis), and the inertia of the flywheel FH3 is defined. The flywheel FH3 has no mass, like the flywheels FH1 and FH2.

The dynamics of the slave device 1 in this dynamics model are the inertial force (translational inertial force) generated according to the translational acceleration of the body mass point Q1 and each leg mass point Q2, and the body mass point Q1 and each leg mass point An equation expressing the relationship that the total resultant force (translational force) with the gravity acting on each of Q2 is balanced with the translational force of the total floor reaction force acting on the slave device 1, and the resultant force and the flywheel FH1 , FH2, and FH3, and the total moment generated around an arbitrary point of action (for example, the central point of the desired total floor reaction force) is applied to the slave device 1 by It is expressed by an equation that expresses the relationship that the total floor reaction force acting balances the moment generated around the point of action.

In this case, the processing of STEP19 can be executed, for example, as follows. In the explanation here, for convenience of explanation, the slave-side global coordinate system Cgs is set such that its X-axis direction is the same or substantially the same as the front-rear direction of the slave device 1 (slave-side global coordinate system Cgs). The direction around the X-axis and the direction around the Y-axis are appropriately updated so that the direction around the X-axis and the direction around the Y-axis correspond to the roll direction and the pitch direction of the slave device 1, respectively, as shown in FIG. . However, coordinate conversion between the slave-side global coordinate system Cgs and a coordinate system whose coordinate axis direction is aligned with the front-rear direction of the slave device 1 can be appropriately performed.

In the processing of STEP 19, for example, by dividing the desired total floor reaction force lateral translational force by the mass of the entire slave device 1, the center of gravity lateral acceleration, which is the lateral translational acceleration of the entire center of gravity of the slave device 1, is calculated. Further, by integrating (second-order integration) this center-of-gravity lateral acceleration, the lateral position of the entire center-of-gravity of the slave device 1 is calculated. Then, the lateral position of the body mass point Q1 of the dynamic model is calculated from the lateral position of the overall center of gravity and the lateral position of each leg mass point Q2 of the dynamic model defined by the desired slave foot position/posture. be done.

Furthermore, the total mass point (body mass point Q1 and two leg mass points Q2) and the resultant force of gravity acting on all mass points, the lateral direction of the moment generated around the desired slave total floor reaction force center point (desired ZMP) A lateral moment corresponding to all mass point motion is calculated, which is a moment (moment around the X-axis and around the Y-axis).

Then, using the rotational angular acceleration of the flywheels FH1 and FH2 in the dynamic model as unknowns, the moment of inertia generated by each of the flywheels FH1 and FH2 in accordance with the rotational angular acceleration (in the direction around the X axis and the direction around the Y axis) The rotational angular acceleration of each of the flywheels FH1 and FH2 is calculated so that the resultant force moment of the moment of inertia) and the lateral moment corresponding to the total mass motion calculated as described above becomes zero. Further, from the rotational angular acceleration, the target angular acceleration of the inclination of the body 2 of the slave device 1 (the target angular acceleration in the direction around the X-axis and the direction around the Y-axis) is calculated. Then, the target slave body inclination is determined by integrating (second-order integration) the target angular acceleration.

Further, the yaw direction corresponding to all mass point motion, which is the moment generated in the yaw direction around the origin of the slave side global coordinate system Cgs by the inertia force generated by all the mass points due to the translational acceleration of the body mass point Q1 and the leg mass point Q2. is calculated.

Then, using the rotational angular acceleration of the flywheel FH3 in the dynamic model as an unknown, the resultant force of the moment of inertia in the yaw direction generated by the flywheel FH3 in accordance with the rotational angular acceleration and the yaw direction moment corresponding to all-mass motion The rotational angular acceleration of the flywheel FH3 is adjusted so that the moment balances the moment in the yaw direction of the desired slave total floor reaction force (moment in the yaw direction about the origin of the slave-side global coordinate system Cgs). is calculated. Further, a target angular acceleration in the yaw direction of the body 2 of the slave device 1 is calculated from the rotational angular acceleration, and the target slave body orientation is determined by integrating (second-order integration) the target angular acceleration. Further, the target slave lateral body position is determined by a geometrical relationship according to the lateral position of the body mass point Q1 and the rotation angles of the flywheels FH1, FH2 and FH3.

Note that the left and right arms 10L and 10R of the slave device 1 are alternately swung back and forth with respect to the upper body 2 in synchronization with the movement of the left and right legs 6L and 6R, similar to human walking. When determining the target motion of each arm 10, the left and right arms 10L and 10R are alternately swung back and forth (hereinafter referred to as back and forth swing motion) as in the dynamic model described in Japanese Patent No. 4126061, for example. ) may be added to the dynamic model shown in FIG.

In this case, in the process of determining the target slave body orientation, the inertial force in the yaw direction generated by the flywheel FH3 according to the rotation angular acceleration of the flywheel FH3 corresponding to the rotation of the body 2 in the yaw direction moment, the yaw direction moment corresponding to the all-mass motion, and the moment of inertia generated by the flywheel corresponding to the forward and backward swing motion of the left and right arms 10L, 10R in response to the forward and backward swing motion of the left and right arms 10L, 10 target. The rotational angular acceleration of the flywheel FH3 may be calculated so as to satisfy the condition that the resultant moment of force balances the moment in the yaw direction of the desired slave total floor reaction force.

In STEP 19, from the processing described above, a condition (specifically, the motion and the gravity acting on the slave device 1, the components in the directions around the X-axis and the Y-axis of the moment around the desired slave total floor reaction force stop point become zero. and the condition that lateral translational force (translational force in the X-axis direction and Y-axis direction) of the desired total floor reaction force can be realized (specifically, the lateral inertia generated by the movement of the slave device 1 The desired slave lateral body position and desired body inclination are determined so as to satisfy the condition that the force balances the lateral translational force of the desired total floor reaction force.

In addition, the condition that the yaw moment of the desired total floor reaction force can be realized The target slave body orientation is determined so as to satisfy the condition that the moment is balanced. Note that the target slave body orientation may be determined according to the observed value of the actual operator's body inclination, as in the third embodiment.

Here, in the present embodiment, the target slave lateral body body position determined in STEP19 is a provisional value, and is determined again in the next STEP20. Specifically, in STEP 20, the target slave lateral body position is re-determined to be the actual slave lateral body position (latest value) estimated by the lateral body position estimator 31d.

In this embodiment, the processing of the slave operation target determining section 31a7 of the slave control section 31 is executed as described above. This embodiment is the same as the first embodiment except for the matters described above.

Here, the corresponding relationship between the present embodiment and the present invention will be supplemented. In the present embodiment, among the processes of the slave movement target determination section 31a7, the process of determining the desired slave body posture corresponds to the process of the 208th processing section of the present invention, and the dynamic model (Fig. 25 ) corresponds to the second dynamic model in the present invention.

Further, the desired body inclination determination section 81e of the master control section 81 corresponds to the 210th processing section as a component of the second processing section in the present invention, and the desired body inclination determined by the desired body inclination determination section 81e is It corresponds to the upper body support portion rotation target in the present invention. Also, the relationship between the inclination of the upper body of the operator P and the inclination of the upper body 2 of the slave device 1 represented by the above equations (55a) and (55b) corresponds to the third target correspondence relationship of the present invention. Further, the backrest control section 81d constitutes the master side control section in the present invention together with the master movement control section 81a. Also, the first backrest drive actuator 68 and the second backrest drive actuator 69 correspond to the second actuators as components of the upper body support part drive mechanism in the present invention. Correspondence other than these is the same as the correspondence between the first embodiment and the present invention.

According to the present embodiment described above, the same effects as those of the first embodiment can be obtained. In addition, in the present embodiment, the inclination of the operator P's upper body is controlled according to the inclination of the upper body 2 of the slave device 1 with the relationships expressed by the above equations (55a) and (55b) as targets. Therefore, the operator P can perceptually recognize in real time how the body 2 of the slave device 1 is tilted and how the tilt is changing.

Therefore, the operator P can quickly recognize a situation in which the attitude of the slave device 1 is likely to collapse, and can move so as to prevent the slave device 1 from losing its attitude.

[Eighth embodiment]
An eighth embodiment of the present invention will now be described with reference to FIG. This embodiment differs from the second embodiment in part of the mechanism of the master device 51 and part of the control processing of the master control unit 81 and the slave control unit 31, and also differs from the seventh embodiment. The form and some mechanisms and control processes are the same. Therefore, in this embodiment, the description of the same matters as those in the second embodiment or the seventh embodiment will be omitted.

Although illustration is omitted, the master device 51 of this embodiment has the same mechanical configuration as the master device 51 of the seventh embodiment, and instead of the upper body support section 65 of the master device 51 of the second embodiment, , and the upper body support portion 65' described in the seventh embodiment. Although not shown, the master control unit 81 of the present embodiment includes the same master movement control unit 81a2 and desired upper body support motion determination unit 81b (see FIG. 9) as in the second embodiment, and the seventh It includes the backrest control unit 81d and the desired body inclination determination unit 81e (see FIG. 22) described in the embodiment.

As for transmission/reception data between the master control unit 81 and the slave control unit 31, the master control unit 81 receives the observed value of the actual slave body inclination from the slave control unit 31, as in the seventh embodiment. In addition, transmission of observed values of the actual operator's body posture (orientation and inclination) from the master control unit 81 to the slave control unit 31 is omitted. Other transmission/reception data is the same as in the second embodiment.

The control processing of the master movement control section 81a2 and the desired upper body support movement determination section 81b of the master control section 81 of this embodiment is the same as that of the second embodiment, and the backrest control section 81d and the desired upper body inclination determination section are performed. The control processing of 81e is the same as in the seventh embodiment.

Next, referring to FIG. 26, the slave control unit 31 of the present embodiment includes a composite compliance motion determining unit 31b, a joint displacement determining unit 31c, and a compensating floor reaction force determining unit 31c, which are the same as the slave control unit 31 of the second embodiment. 31e, and instead of the slave motion target determination unit 31a of the second embodiment, the motion targets of the slave device 1 (desired slave upper body motion, desired slave leg motion, and and a slave motion target determination unit 31a7 that determines a target slave floor reaction force).

Then, the slave control unit 31 sequentially sends the target slave lateral body position determined by the slave movement target determination unit 31a7 and the actual slave body inclination estimated by the body posture detector 23 to the master control unit 81. can send. Therefore, in other words, the slave control unit 31 of the present embodiment is obtained by adding the compensating floor reaction force determination unit 31e of the second embodiment to the slave control unit 31 of the seventh embodiment.

The processing of the composite compliance motion determining unit 31b, the joint displacement determining unit 31c, and the compensating floor reaction force determining unit 31e of the slave control unit 31 of this embodiment is the same as that of the second embodiment, and the target slave motion determining unit 31a7 is the same as in the seventh embodiment. This embodiment is the same as the second embodiment or the seventh embodiment except for the matters described above.

Here, the corresponding relationship between the present embodiment and the present invention will be supplemented. In this embodiment, the compensating floor reaction force determining section 31e corresponds to the sixth processing section of the invention, and the composite compliance motion determining section 31b corresponds to the 103rd processing section of the invention. Correspondence other than this is the same as the correspondence between the seventh embodiment and the present invention.
According to the present embodiment described above, in addition to being able to achieve the same effect as the second embodiment, it is possible to achieve the same effect as the seventh embodiment.

[Ninth Embodiment]
Next, a ninth embodiment of the present invention will be described with reference to FIGS. 27 and 28. FIG. This embodiment differs from the third embodiment in part of the mechanism of the master device 51 and part of the control processing of the master control unit 81 and the slave control unit 31, and also differs from the seventh embodiment. The form and some mechanisms and control processes are the same. Therefore, in this embodiment, the description of the same items as those in the third embodiment or the seventh embodiment will be omitted.

Although illustration is omitted, the master device 51 of this embodiment has the same mechanical configuration as the master device 51 of the seventh embodiment, and instead of the upper body support section 65 of the master device 51 of the third embodiment, , and the upper body support portion 65' described in the seventh embodiment. Although not shown, the master control unit 81 of the present embodiment includes the same master movement control unit 81a and desired upper body support motion determination unit 81b3 (see FIG. 11) as in the third embodiment, and the seventh It includes the backrest control unit 81d and the desired body inclination determination unit 81e (see FIG. 22) described in the embodiment.

As for transmission/reception data between the master control unit 81 and the slave control unit 31 in this embodiment, the master control unit 81 receives the actual slave body inclination from the slave control unit 31 as in the seventh embodiment. Observed values can be received, and transmission of the observed values of the actual operator's body posture (orientation and inclination) from the master control unit 81 to the slave control unit 31 is omitted. Other transmission/reception data is the same as in the third embodiment.

The control processing of the master movement control section 81a and the desired upper body support movement determination section 81b3 of the master control section 81 of this embodiment is the same as that of the third embodiment, and the backrest control section 81d and the desired upper body inclination determination section are performed. The control processing of 81e is the same as in the seventh embodiment.

Next, referring to FIG. 27, the slave control unit 31 of the present embodiment includes a composite compliance motion determination unit 31b, a joint displacement determination unit 31c, a virtual external force determination unit 31f, and a calculation unit 31g, which are the same as those in the third embodiment. On the other hand, instead of the slave motion target determination unit 31a3 of the third embodiment, the motion targets of the slave device 1 (desired slave body movement, desired slave leg motion, and desired slave floor reaction force) are determined by processing different from this. A slave operation target determination unit 31a9 is provided.

Then, the slave control unit 31 sequentially sends the target slave lateral body position determined by the slave motion target determination unit 31a9 and the actual slave body inclination estimated by the body posture detector 23 to the master control unit 81. can send.

In the slave control unit 31 of this embodiment, the processes of the composite compliance motion determination unit 31b, the joint displacement determination unit 31c, the virtual external force determination unit 31f, and the calculation unit 31g are the same as in the third embodiment.

On the other hand, in the slave operation target determination unit 31a9 of the slave control unit 31 of the present embodiment, the actual body support portion height (or the actual operator body height) received from the master control unit 81 via the communication device 33, Observed values of the actual operator's foot position/orientation and the actual operator's foot floor reaction force are input, and the virtual external force moment determined by the virtual external force determining unit 31f is input.

Then, the processing of the slave operation target determination unit 31a9 is executed as shown in the flowchart of FIG. 28 at a predetermined control processing cycle. Specifically, in STEP11 to STEP15 and STEP17, the slave movement target determination unit 31a9 performs the same processing as in the third embodiment, thereby determining the desired slave foot position/orientation, the desired slave floor reaction force, and the desired slave body height. determine the Note that the target slave upper body height may be set to a predetermined value, for example, regardless of the actual body support portion height (or the actual operator upper body support portion height).

Next, in STEP 19, the slave motion target determining unit 31a9 determines the desired slave lateral body body position and desired body posture (inclination and orientation) by executing the same processing as in the seventh embodiment. Next, in STEP 19a, the slave motion target determination unit 31a9 obtains a correction amount for the desired slave lateral body position corresponding to the virtual external force moment determined by the virtual external force determination unit 31f, and calculates the correction amount on the target slave. Add to the lateral body position to get the final target slave lateral body position.

More specifically, the acceleration of the correction amount of the target slave lateral body position is obtained by the following equations (58a) and (58b), and the acceleration is integrated (second order integration) to obtain the desired slave lateral body position. A position correction amount is obtained. By adding the correction amount to the target slave lateral body position, the final target slave lateral body position is obtained.
Acceleration of correction amount of desired slave lateral body position in X-axis direction = virtual moment of external force in direction around Y-axis / (height of body mass point Q1 * mass of Q1)
...... (58a)
Acceleration of correction amount of desired slave lateral body position in Y-axis direction = - (virtual external force moment in direction around X-axis)
/ (Height of body mass point Q1 * Q1 mass)
...... (58b)

The processing of the slave operation target determination unit 31a9 of the slave control unit 31 of this embodiment is executed as described above. This embodiment is the same as the third embodiment or the seventh embodiment except for the matters described above.

Here, the corresponding relationship between the present embodiment and the present invention will be supplemented. In this embodiment, the virtual external force determination unit 31f of the slave control unit 31 corresponds to the fourth processing unit and the fifth processing unit in the present invention, and the body inclination deviation input to the virtual external force determination unit 31f is the The virtual external force moment determined by the virtual external force determination unit 31f corresponds to the first deviation and corresponds to the first virtual external force and the second virtual external force in the present invention.

Further, among the processes of the slave movement target determination unit 31a9, the process of determining the desired slave lateral body position corresponds to the process of the 203rd processing unit of the present invention, and the process of determining the desired slave body posture is It corresponds to the processing of the 209th processing unit in the present invention, and the dynamic model used in these processes (the dynamic model shown in FIG. 25) is the first dynamic model and the third dynamic model in the present invention. Equivalent to. Correspondences other than the above are the same as those between the seventh embodiment and the present invention.
According to the present embodiment described above, in addition to being able to achieve the same effect as the third embodiment, it is possible to achieve the same effect as the seventh embodiment.

[Tenth embodiment]
A tenth embodiment of the present invention will now be described with reference to FIG. This embodiment differs from the fourth embodiment in part of the mechanism of the master device 51 and part of the control processing of the master control unit 81 and the slave control unit 31, and also differs from the seventh embodiment. The form and some mechanisms and control processes are the same. Therefore, in this embodiment, the description of the same items as in the fourth embodiment or the seventh embodiment is omitted.

Although illustration is omitted, the master device 51 of this embodiment has the same mechanical configuration as the master device 51 of the seventh embodiment, and instead of the upper body support section 65 of the master device 51 of the fourth embodiment, , and the upper body support portion 65' described in the seventh embodiment. Although not shown, the master control unit 81 of the present embodiment includes the same master movement control unit 81a and desired upper body support motion determination unit 81b3 as in the third and fourth embodiments (see FIG. 11). , and the backrest control section 81d and the desired body inclination determination section 81e (see FIG. 22) described in the seventh embodiment.

As for transmission/reception data between the master control unit 81 and the slave control unit 31 in this embodiment, the master control unit 81 receives the actual slave body inclination from the slave control unit 31 as in the seventh embodiment. Observed values can be received, and transmission of the observed values of the actual operator's body posture (orientation and inclination) from the master control unit 81 to the slave control unit 31 is omitted. Other transmission/reception data is the same as in the fourth embodiment.

Next, referring to FIG. 29, the slave control unit 31 of the present embodiment includes a composite compliance operation determination unit 31b, a joint displacement determination unit 31c, a virtual external force determination unit 31f, a calculation unit 31g and a compensation unit 31g, which are the same as those in the fourth embodiment. While the floor reaction force determination unit 31h is provided, the operation target of the slave device 1 (target slave body motion, target slave body motion, and a slave motion target determination unit 31a9 that determines leg motion and target slave floor reaction force).

Then, the slave control unit 31 sequentially sends the target slave lateral body position determined by the slave movement target determination unit 31a9 and the actual slave body inclination estimated by the body posture detector 23 to the master control unit 81. can send. Therefore, in other words, the slave control unit 31 of the present embodiment is obtained by adding the compensating floor reaction force determining unit 31h of the fourth embodiment to the slave control unit 31 of the ninth embodiment.

The processes of the composite compliance motion determining unit 31b, the joint displacement determining unit 31c, the virtual external force determining unit 31f, the calculating unit 31g, and the compensating floor reaction force determining unit 31h of the slave control unit 31 of this embodiment are the same as those of the fourth embodiment. , and the processing of the slave operation target determination unit 31a9 is the same as that of the ninth embodiment. This embodiment is the same as the fourth embodiment except for the matters described above.

Here, in this embodiment, which supplements the correspondence relationship between the present embodiment and the present invention, the compensating floor reaction force determination unit 31h corresponds to the seventh processing unit in the present invention, and the composite compliance operation determination unit 31b corresponds to the present invention. It corresponds to the 103rd processing unit in the invention. Correspondence other than this is the same as the correspondence between the ninth embodiment and the present invention.
According to the present embodiment described above, in addition to being able to achieve the same effect as the fourth embodiment, it is possible to achieve the same effect as the seventh embodiment.

[Eleventh embodiment]
Next, an eleventh embodiment of the present invention will be described with reference to FIGS. 30 and 31. FIG. This embodiment differs from the fifth embodiment in part of the mechanism of the master device 51 and part of the control processing of the master control unit 81 and the slave control unit 31, and also differs from the seventh embodiment. The form and some mechanisms and control processes are the same. Therefore, in this embodiment, descriptions of the same items as in the fifth embodiment or the seventh embodiment are omitted.

Although illustration is omitted, the master device 51 of this embodiment has the same mechanical configuration as the master device 51 of the seventh embodiment, and instead of the upper body support section 65 of the master device 51 of the fifth embodiment, , and the upper body support portion 65' described in the seventh embodiment. Although illustration is omitted, the master control unit 81 of the present embodiment includes a master movement control unit 81a5 and a desired upper body support reaction force determination unit 81c (see FIG. 16), which are the same as those of the fifth embodiment. It includes the backrest control section 81d and the desired body inclination determination section 81e (see FIG. 22) described in the seventh embodiment.

As for transmission/reception data between the master control unit 81 and the slave control unit 31, the master control unit 81 receives the observed value of the actual slave body inclination from the slave control unit 31, as in the seventh embodiment. In addition, transmission of observed values of the actual operator's body posture (orientation and inclination) from the master control unit 81 to the slave control unit 31 is omitted. Other transmission/reception data is the same as in the fifth embodiment.

The control processing of the master movement control unit 81a5 and the desired upper body support reaction force determination unit 81c of the master control unit 81 of this embodiment is the same as in the fifth embodiment, and the backrest control unit 81d and the desired upper body inclination determination unit The control processing of the section 81e is the same as that of the seventh embodiment.

Next, referring to FIG. 30, the slave control unit 31 of the present embodiment includes a composite compliance motion determining unit 31b, a joint displacement determining unit 31c, and a lateral body position estimating unit 31d, which are the same as those in the fifth embodiment. On the other hand, instead of the slave motion target determination unit 31a5 of the fifth embodiment, the motion targets of the slave device 1 (desired slave body movement, desired slave leg motion, and desired slave floor reaction force) are determined by processing different from this. A slave operation target determination unit 31a11 is provided.

Then, the slave control unit 31 transmits the actual slave lateral body position estimated by the body lateral position estimation unit 31d and the actual slave body inclination estimated by the body posture detector 23 to the master control unit 81. can be sent sequentially to

In the slave control unit 31 of this embodiment, the processing of the composite compliance motion determining unit 31b, the joint displacement determining unit 31c, and the body lateral position estimating unit 31d is the same as that of the fifth embodiment.

On the other hand, in the slave operation target determination unit 31a11 of the slave control unit 31 of the present embodiment, the actual body support portion height (or the actual operator body height) received from the master control unit 81 via the communication device 33, Observed values of the actual operator's foot position/posture, the actual operator's foot floor reaction force, and the actual operator's lateral body position are input.

Then, the processing of the slave operation target determination unit 31a11 is executed as shown in the flowchart of FIG. 31 at a predetermined control processing cycle. Specifically, in STEP11 to STEP15 and STEP17, the slave movement target determining unit 31a11 performs the same processing as in the fifth embodiment, thereby determining the desired slave foot position/orientation, the desired slave floor reaction force, and the desired slave body height. determine the Note that the target slave upper body height may be set to a predetermined value, for example, regardless of the actual body support portion height (or the actual operator upper body support portion height).

Further, in STEP 19, the same processing as that of the slave motion target determining unit 31a7 of the seventh embodiment is executed using the dynamic model to obtain the desired slave body lateral position and the desired slave body posture (inclination and orientation). ).

However, in the present embodiment, the target slave lateral body position determined in STEP 19 is a provisional value, and is determined again in the next STEP 20a. Specifically, in STEP 20a, the slave movement target determination unit 31a11 determines the target slave lateral body position according to the observed value (latest value) of the actual operator lateral body position received from the master control unit 81. Decide again. Specifically, the slave movement target determination unit 31a11 again determines the target slave lateral body body positions P_sb_x_aim and P_sb_y_aim using the formulas (50a) and (50b) described in the fifth embodiment.

The processing of the slave operation target determination unit 31a11 of the slave control unit 31 of this embodiment is executed as described above. This embodiment is the same as the fifth embodiment except for the matters described above.

Here, the corresponding relationship between the present embodiment and the present invention will be supplemented. In the present embodiment, among the processes of the slave movement target determination section 31a11, the process of determining the desired slave body lateral position corresponds to the process of the 204th processing section of the present invention. The internal reaction force determination unit 81c corresponds to the 205th processing unit in the present invention. Correspondence other than this is the same as the correspondence between the seventh embodiment and the present invention.
According to the present embodiment described above, in addition to being able to achieve the same effect as the fifth embodiment, it is possible to achieve the same effect as the seventh embodiment.

[Twelfth embodiment]
A twelfth embodiment of the present invention will now be described with reference to FIG. This embodiment differs from the sixth embodiment in part of the mechanism of the master device 51 and part of the control processing of the master control unit 81 and the slave control unit 31, and also differs from the seventh embodiment. The form and some mechanisms and control processes are the same. Therefore, in this embodiment, the description of the same items as in the sixth or seventh embodiment is omitted.

Although illustration is omitted, the master device 51 of this embodiment has the same mechanical configuration as the master device 51 of the seventh embodiment, and instead of the upper body support section 65 of the master device 51 of the sixth embodiment, , and the upper body support portion 65' described in the seventh embodiment. Although not shown, the master control unit 81 of this embodiment includes the same master movement control unit 81a5 and desired upper body support reaction force determination unit 81c (see FIG. 16) as in the fifth and sixth embodiments. ), and the backrest control section 81d and the desired body inclination determining section 81e (see FIG. 22) described in the seventh embodiment.

As for transmission/reception data between the master control unit 81 and the slave control unit 31 in this embodiment, the master control unit 81 receives the actual slave body inclination from the slave control unit 31 as in the seventh embodiment. Observed values can be received, and transmission of the observed values of the actual operator's body posture (orientation and inclination) from the master control unit 81 to the slave control unit 31 is omitted. Other transmission/reception data is the same as in the sixth embodiment.

The control processing of the master movement control unit 81a5 and the desired upper body support reaction force determination unit 81c of the master control unit 81 of this embodiment is the same as in the sixth embodiment, and the backrest control unit 81d and the desired upper body inclination determination unit The control processing of the section 81e is the same as that of the seventh embodiment.

Next, referring to FIG. 32, the slave control unit 31 of the present embodiment includes the same composite compliance operation determination unit 31b, joint displacement determination unit 31c, lateral body position estimation unit 31d, and compensating floor anti-floor motion determination unit 31d as in the sixth embodiment. While the force determination unit 31e is provided, the operation targets of the slave device 1 (desired slave upper body motion, desired slave leg motion, and A slave operation target determination unit 31a11 that determines a target slave floor reaction force) is provided.

Then, the slave control unit 31 transmits the actual slave lateral body position estimated by the body lateral position estimation unit 31d and the actual slave body inclination estimated by the body posture detector 23 to the master control unit 81. can be sent sequentially to Therefore, in other words, the slave control unit 31 of the present embodiment is obtained by adding the compensating floor reaction force determination unit 31e of the sixth embodiment to the slave control unit 31 of the eleventh embodiment.

The processes of the composite compliance motion determining unit 31b, the joint displacement determining unit 31c, the body lateral position estimating unit 31d, and the compensating floor reaction force determining unit 31e of the slave control unit 31 of this embodiment are the same as those of the sixth embodiment. Yes, and the processing of the slave operation target determination unit 31a9 is the same as in the eleventh embodiment.

This embodiment is the same as the fourth embodiment except for the matters described above. Here, the corresponding relationship between the present embodiment and the present invention will be supplemented. In this embodiment, the compensating floor reaction force determining section 31e corresponds to the sixth processing section of the invention, and the composite compliance motion determining section 31b corresponds to the 103rd processing section of the invention. Correspondence other than this is the same as the correspondence between the eleventh embodiment and the present invention.
According to the present embodiment described above, in addition to being able to achieve the same effect as the sixth embodiment, it is possible to achieve the same effect as the seventh embodiment.

[Thirteenth Embodiment]
Next, a thirteenth embodiment of the present invention will be described. In this embodiment, a part of the configuration of the master device 51 is different from those in the first to twelfth embodiments. For example, in this embodiment, the slide drive actuator 66 of the elevating mechanism 60 of the master device 51 is omitted so that the slide member 62 can move vertically within a predetermined range with respect to the column 61 (free movement). The lifting mechanism 60 is configured so that it can move to

Then, the desired upper body support motion determination units 81b and 81b3 omit the process of determining the desired upper body support height of the desired upper body support motion, and the desired upper body support reaction force determination unit 81c , the process of determining the vertical translational force of the desired upper body support reaction force is omitted. Further, in the master movement control units 81a, 81a2, and 81a5, processing related to operation control of the slide drive actuator 66 is omitted. Others may be the same as any of the first to twelfth embodiments.

According to this embodiment, since the upper body support part 65 of the master device 51 can move freely in the vertical direction, it is made to follow the vertical movement of the upper body of the operator P without requiring an actuator. The body support portion 65 can be moved up and down.

[14th embodiment]
A fourteenth embodiment of the present invention will now be described with reference to FIG. In this embodiment, as shown in FIG. 33, the elevating mechanism 60' of the master device 51 includes a spring 77 such as a coil spring instead of the slide drive actuator 66. 61b and the slide member 62 in a compressed state. As a result, the upper body support portion 65 is urged upward by the elastic force of the spring 77 and can elastically move up and down.

In this case, the elastic force of the spring 77 is applied to the upper body of the operator P in a state where the upper body support section 65 is attached to the upper body of the operator P, for example. is set so that almost no force acts from the upper body supporting portion 65, or an upward biasing force acts from the upper body supporting portion 65 due to the elastic force of the spring 77. As shown in FIG. Others may be the same as those of the thirteenth embodiment.

According to this embodiment, the upper body of the operator P wearing the upper body support part 65 is prevented from being subjected to the weight of the upper body support part 65, the slide member 62, and the members interposed therebetween. It can be reduced or eliminated. Alternatively, an assisting force (upward translational force) that reduces the burden on the legs of the operator P against the gravity acting on the operator P can be applied to the operator P from the upper body support section 65 .

Instead of providing the master device 51 with the spring 77 as described above, the slide drive actuator 66 provided in the master device 51 of the first to twelfth embodiments causes the slide member 62 to be vertically displaced (in other words, A driving force similar to the elastic force of the spring 77 may be generated according to the vertical displacement of the upper body support portion 65 .

[Other embodiments]
The present invention is not limited to the embodiments described above, and other embodiments can be employed. Several other embodiments are described below.
In the first, second, seventh or eighth embodiment, or in the thirteenth or fourteenth embodiment combined with these, the master controller 81 determines the desired upper body support motion. In the process of determining the desired upper body support motion by the unit 81b, the desired slave lateral body position determined by the slave control unit 31 is used instead of the observed value (or its filtered value) of the actual slave lateral body position. may be used to determine the desired upper body support motion.

Similarly, in the fifth, sixth, eleventh or twelfth embodiment, or in the thirteenth or fourteenth embodiment combined with these, the desired upper body support of the master control unit 81 In the process of determining the desired body support reaction force by the internal reaction force determination unit 81c, instead of the observed value of the actual slave lateral body position (or its filtered value), the target slave determined by the slave control unit 31 The lateral body position may be used to determine a desired upper body support motion.

Further, in the third embodiment, fourth embodiment, ninth embodiment and tenth embodiment, or in the thirteenth embodiment or fourteenth embodiment combined with these, the target upper body support part of the master control part 81 In the process of determining the desired upper body support portion motion by the motion determining unit 81b3, instead of the desired slave lateral body lateral position, an observed value of the actual slave lateral lateral body position (or its filtered value) may be used. . In this case, the actual slave lateral body position may be estimated by the same processing as the lateral body position estimator 31d of the first embodiment.

In general, the desired slave lateral body position has less high-frequency noise components than the actual slave lateral body position. Determining the position has the advantage of increasing the stability of the determined target slave lateral body position. Alternatively, the desired body support lateral position may be determined using a filtered value obtained by filtering the observed value of the actual slave lateral body lateral position with a low-pass filter.

Further, in the first to fourteenth embodiments, the lateral position of the upper body support part 65 or 65' or the lateral position of the upper body of the operator P is used as the lateral position of the master side reference part in the present invention, A case in which the lateral position of the body 2 of the slave device 1 is adopted as the lateral position of the slave-side reference portion in the present invention has been exemplified.

However, for example, the horizontal position of the center of gravity of the operator P, which is the horizontal position of the center of gravity of the operator P, is adopted as the horizontal position of the master side reference portion, and the horizontal position of the center of gravity of the slave device 1 is adopted as the horizontal position of the center of gravity of the slave device 1 . The directional position, slave centroid lateral position, may be employed. Then, the relationship between the actual operator center-of-gravity lateral position (X-axis position P_opcog_x_act and Y-axis position P_opcog_y_act) and the actual slave center-of-gravity lateral position (X-axis position P_sbcog_x_act and Y-axis position P_sbcog_x_act) is , for example, with the goal of satisfying the following equations (60a) and (60b) in the same form as the above equations (1a) and (1b), according to the observed value or target value of the lateral position of the slave center of gravity 51 movement control may be performed.
P_opcog_x_act = Kpmb * P_sbcog_x_act + Cpmb
...... (60a)
P_opcog_y_act = Kpmb * P_sbcog_y_act + Cpmb
...... (60b)

More specifically, for example, any one of the first to fourth embodiments and the seventh to tenth embodiments, or the target in the thirteenth or fourteenth embodiment combined with the above embodiments In the process of the upper body support motion determination unit 81b or 81b3, the target value of the lateral center of gravity position of the operator is determined based on the above equations (60a) and (60b) from the observed value or the target value of the lateral center of gravity position of the slave. can. Then, the target lateral translational velocity of the upper body support sections 65, 65' can be determined by the feedback control law so that the difference between the target value and the observed value of the operator's lateral center of gravity position approaches zero. Further, by integrating the desired translational velocity, a desired upper body support lateral position of the desired upper body support motion can be determined.

In this case, the actual slave center-of-gravity lateral position is obtained by, for example, the position and orientation of any part of the body 2 of the slave device 1 (the position and orientation as viewed in the slave-side global coordinate system Cgs). can be estimated using the estimated position and orientation, observed values of actual joint displacements of the joints of the slave device 1 , and the rigid body link model of the slave device 1 . Also, the target value of the lateral position of the slave's center of gravity can be calculated using, for example, the desired overall motion of the slave device 1 including the desired slave upper body motion and desired slave leg motion, and the rigid body link model of the slave device 1. can be done.

Also, the actual lateral position of the center of gravity of the operator can be obtained by combining the position and orientation of any part of the upper body of the operator P (the position and orientation as viewed in the master-side global coordinate system Cgm) and the bending angles of each joint. It can be estimated by a known method such as cabcha, and using the observed values of the estimated position and orientation and bending angle, and the operator P's rigid body link model. The bending angles of each joint or some of the joints of the operator P may be detected by a displacement sensor or an inertia sensor (acceleration sensor and angular velocity sensor) attached to the operator P.

Further, for example, any one of the fifth, sixth, eleventh, and twelfth embodiments, or the target in the thirteenth or fourteenth embodiment combined with the above embodiments In the process of the body support reaction force determination unit 81c, the relation between the observed value of the lateral center of gravity position of the operator and the observed value or target value of the lateral position of the slave center of gravity is expressed by equations (60a) and (60b). The amount of deviation from the indicated target relationship (the amount of deviation in the X-axis direction and the Y-axis direction) can be calculated by the same arithmetic processing as in the fifth embodiment. Then, it is also possible to determine the lateral translational force of the desired upper body support reaction force by a feedback control rule so that this amount of deviation approaches zero.

Further, an embodiment including the compensating floor reaction force determination unit 31e (second embodiment, sixth embodiment, eighth embodiment, twelfth embodiment, or thirteenth embodiment or fourteenth embodiment combined with these) ), the compensating total floor reaction force moments M_dmd_x and M_dmd_y determined by the compensating floor reaction force determination unit 31e are set to, for example, the actual body The lateral translational force of the support floor reaction force may be determined to be close to zero. For example, as shown in the following equations (61a) and (61b), the deviation amounts Err_x and Err_y are linearly combined with the observed values of the horizontal translational forces F_mb_x_act and F_mb_y_act among the floor reaction forces of the body support part. The compensating total floor reaction force moments M_dmd_x and M_dmd_y may be determined according to the feedback control law so that the state quantity of is close to zero.
M_dmd_x
=Ke*Err_y+Ked*dErr_y/dt+Kf*F_mb_y_act
... (61a)
M_dmd_y
=-Ke*Err_x-Ked*dErr_x/dt-Kf*F_mb_x_act
... (61b)

Ke, Ked, and Kf are gains of predetermined values. Also. Although PD law is used as the feedback control law in equations (61a) and (61b), other feedback control law such as D law and PID law may be used.

Furthermore, the compensating total floor reaction force moments M_dmd_x and M_dmd_y are calculated by, for example, using the feedback control law (P law, PD law, PID law, etc.) may be determined so as to approach zero. For example, the compensating total floor reaction force moments M_dmd_x and M_dmd_y may be determined by the equations (61a) and (61b) with Err_x and Err_y set to zero.

As described above, M_dmd_x and M_dmd_y are determined so that the shift amounts Err_x and Err_y and the lateral translational force of the floor reaction force of the actual body support are close to zero, or the floor reaction force of the actual body support Even when M_dmd_x and M_dmd_y are determined so that the lateral translational force of the two is close to zero, in a situation where the slave device 1 can recover from its collapsed posture by itself, the upper body of the operator P is placed on the upper body. It is possible to suppress the lateral translational force acting from the body supporting portions 65, 65'.

Instead of the deviation amounts Err_x and Err_y described in the second embodiment, for example, the relationship between the observed value of the lateral center of gravity position of the operator and the observed value of the lateral position of the slave center of gravity can be expressed by the expression ( The amount of deviation from the target relationship indicated by 60a) and (60b) may be used to determine the compensating total floor reaction force moments M_dmd_x and M_dmd_y.

Further, in the embodiment (fourth embodiment, tenth embodiment, or thirteenth embodiment or fourteenth embodiment combined with these) including the compensating floor reaction force determining section 31h, the compensating floor reaction force determining section 31h The compensating total floor reaction force moments M_dmd_x and M_dmd_y determined by the actual body support part floor reaction force The lateral translational force of the forces may be determined to be close to zero. For example, as shown in the following equations (62a) and (62b), the compensating total floor reaction force moments M_dmd_x and M_dmd_y may be determined by a feedback control law.
M_dmd_x
=-Ke*θerr_x-Ked*dθrr_x/dt+Kf*F_mb_y_act
... (62a)
M_dmd_y
=-Ke*θerr_y-Ked*dθrr_y/dt-Kf*F_mb_x_act
... (62b)

Ke, Ked, and Kf are gains of predetermined values. Also, in equations (62a) and (62b), PD law is used as the feedback control law, but other feedback control law such as D law and PID law may be used.

In addition, the compensating total floor reaction force moments M_dmd_x and M_dmd_y, for example, without using the body inclination deviations θerr_x and θerr_y, the lateral translational force of the floor reaction force of the actual body support is calculated by the feedback control law (P law , PD law, PID law, etc.). For example, the compensating total floor reaction force moments M_dmd_x and M_dmd_y may be determined by the formulas (62a) and (62b) with Ke and Ked set to zero.

As described above, M_dmd_x and M_dmd_y are determined so that the body inclination deviation and the lateral translational force of the actual body support floor reaction force are close to zero, or Even if M_dmd_x and M_dmd_y are determined so that the horizontal translational force of the operator P approaches zero, in a situation where the slave device 1 can recover from its collapsed posture by itself, the upper body of the operator P is It is possible to suppress the lateral translational force acting from the support portions 65, 65'.

In addition, in the compensating floor reaction force determination unit 31h provided in the fourth embodiment and the like, instead of the body inclination deviation, for example, the deviation between the observed value of the actual slave lateral body position and the target slave lateral body position may be used to determine the compensating total floor reaction force moments M_dmd_x, M_dmd_y. Furthermore, in addition to bringing the deviation between the actual slave lateral body lateral position observation value and the target slave lateral body lateral position closer to zero, the lateral translational force of the body support floor reaction force is reduced to zero. Compensating total floor reaction force moments M_dmd_x and M_dmd_y may be determined so as to approximate each other.

Alternatively, the compensating total floor reaction force moments M_dmd_x and M_dmd_y may be determined using, for example, the deviation between the observed value and the target value of the lateral position of the slave's center of gravity instead of the body tilt deviation. Furthermore, in addition to bringing the deviation between the observed lateral position of the center of gravity and the target lateral position of the center of gravity closer to zero, the lateral translational force of the body support floor reaction force is brought closer to zero. Compensating total floor reaction force moments M_dmd_x, M_dmd_y may be determined as follows.

Further, in the embodiment (third embodiment, fourth embodiment, ninth embodiment, tenth embodiment, or thirteenth embodiment or fourteenth embodiment combined with these) having the virtual external force determination unit 31f , instead of the body inclination deviation, for example, the deviation between the observed value of the actual slave lateral body position and the target slave lateral body position may be used to determine the virtual external force moment. Alternatively, instead of the body posture deviation, for example, the virtual moment of external force may be determined according to the deviation between the observed value and the target value of the lateral position of the slave's center of gravity.

Further, the upper body support part 65' provided in the seventh to twelfth embodiments rotates the backrest member 65b in the roll direction and the pitch direction by the first backrest drive actuator 68 and the second backrest drive actuator 69, respectively. configured to obtain However, for example, it may be configured to be rotationally driven in either one of the roll direction and the pitch direction according to the inclination of the body 2 of the slave device 1 .

Further, the upper body support portions 65 and 65' may be configured to be rotationally driven in the yaw direction with respect to the slide member 62 of the master device 51 by an actuator such as an electric motor. In this case, for example, according to the observed value or target value of the orientation of the upper body 2 of the slave device 1, the upper body support section 65 is rotationally driven in the yaw direction so that the operator P A rotational force in the yaw direction may be applied to the upper body.

For example, the following control processing can be adopted. That is, for example, the moment in the yaw direction of the floor reaction force acting on the operator P is estimated based on the observed values of the operator foot floor reaction force of each foot of the operator P. Then, the desired moment in the yaw direction of the desired slave floor reaction force is determined according to the moment in the yaw direction.

Furthermore, the target motion of the slave device 1 is determined using a dynamic model so that this target yaw direction moment can be dynamically realized. Then, a predetermined relationship ( A predetermined relationship between the orientation of the upper body support portion 65 and the orientation of the body 2 (for example, the relationship represented by the above equation (26)) is determined to satisfy the desired orientation of the upper body support portion, and the target A rotational driving force is applied to the upper body support portions 65, 65' so as to realize the orientation of the upper body support portions.

As a result, when the relationship between the actual orientation of the upper body of the operator P and the actual orientation of the upper body 2 of the slave device 1 deviates from the predetermined relationship, the operator P can support the upper body. It can be recognized from the rotational force in the yaw direction received from the portion 65 .
It is also possible to control the orientation of the upper body support parts 65 and 65' as described above by controlling the movement of the movement mechanism 52 of the master device 51. FIG.

Further, the master device 51 of the first to sixth embodiments may be provided with an upper body support section 65' such as that of the seventh embodiment instead of the upper body support section 65. In this case, the first backrest drive actuator 68 and the second backrest drive actuator 69 of the body support section 65' are controlled so that the moment in the roll direction and the moment in the pitch direction of the body support reaction force become zero. You may

Alternatively, as the upper body support portion provided in the master device 51 of the first to sixth embodiments, the upper body support portion 65' has a backrest member 65b in the same manner as the upper body support portion 65', and the backrest member 65b is attached to the slide member 62. An upper body support that is configured to be freely rotatable in roll and pitch directions (for example, an upper body support that omits the first backrest drive actuator 68 and the second backrest drive actuator 69 of the upper body support 65') part) may be adopted.

Further, in each of the above-described embodiments, as the upper body support section drive mechanism of the present invention, the moving mechanism 52 capable of moving on the floor surface together with the operator P and the elevating mechanism 60 are provided. However, the upper body support part drive mechanism in the present invention may be configured as illustrated in FIG. 34, for example.

In this example, the master device 51' includes an elevating mechanism 60 and an upper body support section 65 having the same configuration as those of the first embodiment, and the ceiling section (not shown) of the building in the environment where the operator P (not shown) moves. The moving mechanism 100 is installed in the moving mechanism 100 (not shown), and the supporting column 61 of the lifting mechanism 60 is supported by the moving mechanism 100 (in other words, the upper body support portion 65 moves to the moving mechanism 100 via the lifting mechanism 60). ).

In this case, the moving mechanism 100 moves in the front-rear direction (the Xm-axis direction of the master coordinate system Cm shown) along a pair of parallel rail mechanisms 101, 101 fixed to the ceiling by the driving force of an actuator (not shown). a pair of first movable parts 102, 102, a rail mechanism 103 bridged between the first movable parts 102, 102 so as to move integrally with the first movable parts 102, 102, and a rail mechanism 103, a second movable portion 104 that can move in the left-right direction (Ym-axis direction of the master coordinate system Cm) by the driving force of an actuator (not shown), and an upper plate extending forward from the second movable portion 104. and an actuator 106 arranged below the upper plate-like member 105 around the axis in the vertical direction with respect to the upper plate-like member 105 (in the direction around the Zm axis of the master coordinate system Cm). a lower plate-like member 107 attached to the plate-like member 105 via the actuator 106 so as to be rotatable by (for example, an electric motor).

The upper end of the pillar 61 of the lifting mechanism 60 is fixed to the lower plate-like member 107 so that the center of the body support portion 65 is positioned substantially directly below the axis of rotation of the actuator 106 . In this embodiment, the moving mechanism 100 and the lifting mechanism 60 realize the upper body support portion driving mechanism of the present invention. Also, the upper body support portion may be the upper body support portion 65' described in the seventh embodiment.

In the master device 51', the pillar 61 of the lifting mechanism 60 can be moved in the horizontal direction (perpendicular to the vertical direction) by the operation of the moving mechanism 100, and can rotate about the axis of the actuator 106 (in the yaw direction). can. Therefore, the operator P can move on the floor similarly to the first embodiment. Then, by controlling the operation of the actuator (not shown) of the movement mechanism 100 and the actuator 106, the upper body support portion 65 ( or 65′) and the lateral translational force and yaw moment of the body support reaction forces.

Further, the upper body support portion drive mechanism in the present invention may employ, for example, the structure illustrated in FIG. 35 . In this example, the master device 51'' includes an upper body support portion 65 having the same configuration as that of the first embodiment, and an operator P wearing the upper body support portion 65 on the upper body (waist). A plurality of electric motors 110 are arranged. Each electric motor 110 is mounted, for example, on a pillar 111 erected on the floor.

A pulley 112 that can be driven to rotate by the electric motor 110 is attached to the rotating shaft of each electric motor 110 . Further, a wire 113 wound around a pulley 112 corresponding to each electric motor 110 is pulled out from the pulley 112 , and the tip of the pulled out wire 113 is connected to the upper body support portion 65 . The upper body support portion may be the upper body support portion 65' described in the seventh embodiment.

In such a master device 51'', a plurality of electric motors 110 and wires 113 realize driving of the upper body support portion of the present invention. In this master device 51'', the operator P can move on the floor surface by controlling the operation of each electric motor 110, and the operator P can move on the floor in the same manner as in the first to sixth embodiments (or in the seventh to twelfth embodiments). morphology), it is possible to control the movement of the body support 65 (or 65') and the lateral translational and yaw moments of the body support reaction forces.
An actuator such as a hydraulic motor may be used instead of the electric motor 110 .

In each of the above embodiments, the slave control unit 31 and the master control unit 81 are installed in the slave device 1 and the master device 51 respectively. Either one of them may be installed, or an external server or the like may be provided. Also, the master control unit 81 may have a part of the functions of the slave control unit 31 . Alternatively, the slave control unit 31 may have a part of the functions of the master control unit 81 .

Further, a set of the slave control unit 31 and the master control unit 81 is installed in both the slave device 1 and the master device 51, and the processing of the slave control unit 31 and the master control unit 81 in the slave device 1 and the processing of the master device 51 The processing of the slave control unit 31 and the processing of the master control unit 81 may be executed in parallel in synchronization with each other.

Also, the master device 51 may be equipped with a manipulator for manipulating each arm 10 of the slave device 1 by moving each arm of the operator P, for example.

Claims (20)

A maneuvering system capable of maneuvering to move a slave device, which is a legged mobile body having an upper body and two legs extending from the upper body, comprising:
an upper body support attached to the operator's upper body so as to move with the operator as the operator moves; an upper body support drive mechanism operably attached to the support; and
a slave-side motion target including a target slave leg motion that is a target motion of each leg of the slave device and a target slave body motion that is a target motion of the upper body of the slave device; and upper body support driving of the master device. a motion target determination unit that determines a master-side motion target that is the motion target of the mechanism;
a master-side control unit that controls the operation of the upper body support unit drive mechanism according to the determined master-side operation target;
a slave-side control unit that controls the operation of the slave device according to the determined slave-side operation target,
The lateral position of any one of the upper body support, the operator's upper body, and the operator's center of gravity is defined as the master-side reference part lateral position, and the slave device's upper body and center of gravity of the slave device. When the lateral position of any one of them is defined as the lateral position of the slave side reference part,
The motion goal determination unit
determining the target slave leg motion using observations of the state of motion of at least each leg of the operator, such that as the operator moves through the leg motion, the slave device is moved by the leg motion; A first processing unit that determines;
The relationship between the actual lateral position of the master-side reference part and the actual lateral position of the slave-side reference part approaches a state that satisfies a predetermined first target correspondence, and is defined by the slave-side operation target. The master-side reference portion lateral position and the slave-side reference portion lateral position are adjusted so that the target value of the slave-side reference portion lateral direction position to match or approach the actual slave side reference portion lateral position. and a second processing unit that determines the target slave body motion and the master motion target using respective observed values. The mobile body control system according to claim 1,
The master-side motion target determined by the motion target determination unit includes a target lateral position of the upper body support section, and the second processing section determines the target lateral position of the upper body support section. so that the relationship between the master side reference portion lateral position and the observed value or target value of the slave side reference portion lateral position satisfies the first target correspondence relationship, the slave side reference portion lateral direction configured to include a 201 processing unit for determining a target lateral position of the upper body support using an observed or target value of position;
The master-side control section is configured to control the operation of the upper body support drive mechanism so that the actual lateral position of the upper body support follows the determined target lateral position. A control system for a moving body characterized by: 3. The mobile body control system according to claim 1 or 2,
The second processing unit adjusts the target value of the lateral position of the reference part on the slave side to match the observed value of the lateral position of the reference part on the slave side. 202. A control system for a moving object, comprising a processing unit 202 that determines a target lateral position of the upper body using an observed value of the lateral position of the slave-side reference part. 3. In the mobile body control system according to claim 1 or 2,
The action target determination unit uses an observed value of the operator's floor reaction force, which is the floor reaction force acting on each leg of the operator, to determine the desired slave floor reaction force, which is the floor reaction force to be applied to the slave device. and a fourth processing unit for determining a first virtual external force acting on the slave device in a first dynamics model representing dynamics of the slave device,
The second processing unit determines that a resultant force of an inertial force generated by the slave device due to a desired motion of the slave device defined by the slave-side operation target and a gravitational force acting on the slave device is equal to the determined target slave device. determining a target lateral translational acceleration of the upper body of the slave device so as to balance the resultant force of the floor reaction force and the determined first virtual external force on the first dynamic model; a target lateral position of the slave device's body in the target slave body motion by integrating
deviation between a target value of the slave-side reference lateral position defined by the slave-side motion target and an observed value of the slave-side reference lateral position, and the slave device defined by the slave-side motion target When any one of the deviations between the target value of the inclination of the upper body and the observed value of the inclination is defined as the first deviation, the fourth processing unit causes the first deviation to approach zero (2) a control system for a moving object, wherein the calculated value of the first deviation is used to determine the first virtual external force; The mobile body control system according to claim 1,
The second processing unit performs the following steps so that the relationship between the target value of the lateral position of the slave-side reference portion and the actual lateral position of the master-side reference portion satisfies the first target correspondence relationship. a 204th processing unit for determining a target lateral position of the slave device's body in the target slave body motion using an observed value of the master side reference lateral position; A calculated value of the degree of deviation of the relationship between the observed value of the directional position and the observed value or target value of the lateral position of the slave-side reference portion from the first target correspondence relationship so as to approach zero. and a 205th processing unit that determines the master-side motion target using the . In the mobile body control system according to claim 5,
The 205th processing unit is configured to determine a desired upper body support lateral reaction force, which is a target value of lateral reaction force received by the operator from the upper body support part, as the master-side action target. ,
The master-side control section controls the upper body support section so that the actual lateral reaction force that the operator receives from the upper body support section follows the determined desired upper body support section lateral reaction force. A control system for a moving object, characterized in that it is configured to control the operation of a drive mechanism. In the mobile body control system according to any one of claims 1 to 6,
The second processing unit converts a target vertical position of the body of the slave device out of the target slave body motion to an actual vertical position of the upper body of the upper body support unit or the operator. determining a target vertical position of the body of the slave device using an observed value of the vertical position of the upper body support or the operator's upper body so as to satisfy a predetermined second target correspondence; or 206. A control system for a moving object, comprising: a 206th processing unit for determining a target vertical position of the upper body of said slave device to a predetermined value. In the mobile body control system according to any one of claims 1 to 7,
The second processing unit sets the target posture of the body of the slave device among the target slave body motions so as to satisfy a predetermined third target correspondence relationship with the actual posture of the body of the operator. and a 207th processing unit that determines a target posture of the body of the slave device using an observed value of the posture of the operator's body. . In the mobile body control system according to any one of claims 1 to 7,
The upper body support driving mechanism is configured to apply to the upper body support a translational force for laterally moving the upper body support and a rotational force for rotating the upper body support. In addition, the upper body support part is configured so that the rotational force applied from the upper body support part driving mechanism in a state of being attached to the upper body of the operator serves as a force for changing the posture of the upper body of the operator. It is configured so that it can be transmitted to the upper body of the operator,
The action target determination unit uses an observed value of the operator's floor reaction force, which is the floor reaction force acting on each leg of the operator, to determine the desired slave floor reaction force, which is the floor reaction force to be applied to the slave device. is configured to include a third processing unit that determines by
The second processing unit determines that a resultant force of an inertial force generated by the slave device due to a desired motion of the slave device defined by the slave-side operation target and a gravitational force acting on the slave device is equal to the determined target slave device. Determining a target angular acceleration of the upper body of the slave device so as to balance the floor reaction force on a second dynamic model representing the dynamics of the slave device, and integrating the target angular acceleration A 208th processing unit that determines a target posture of the body of the slave device among the target slave body motions, an actual posture of the body of the slave device, and an actual posture of the body of the operator. The upper body support portion rotational motion target, which is the motion target of the upper body support portion drive mechanism related to the rotational motion of the upper body support portion, is set so that the relationship between the a 210th processing unit that determines using the observed value of the body posture of the slave device or the determined target posture,
The control system for a moving object, wherein the master-side control unit is configured to generate a rotational force for rotating the upper body support according to the determined target for rotating the upper body support. . In the mobile body control system according to any one of claims 1 to 7,
The upper body support driving mechanism is configured to apply to the upper body support a translational force for laterally moving the upper body support and a rotational force for rotating the upper body support. In addition, the upper body support part is configured so that the rotational force applied from the upper body support part driving mechanism in a state of being attached to the upper body of the operator serves as a force for changing the posture of the upper body of the operator. It is configured so that it can be transmitted to the upper body of the operator,
The action target determination unit uses an observed value of the operator's floor reaction force, which is the floor reaction force acting on each leg of the operator, to determine the desired slave floor reaction force, which is the floor reaction force to be applied to the slave device. and a fifth processing unit for determining a second virtual external force acting on the slave device in a third dynamics model representing the dynamics of the slave device,
The second processing unit determines that a resultant force of an inertial force generated by the slave device due to a desired motion of the slave device defined by the slave-side operation target and a gravitational force acting on the slave device is equal to the determined target slave device. determining a target angular acceleration of the upper body of the slave device so as to balance the resultant force of the floor reaction force and the determined second virtual external force on the third dynamic model; and integrating the target angular acceleration. A 209th processing unit for determining a target posture of the body of the slave device out of the target slave body motion by performing a combination of the posture of the body of the slave device and the posture of the operator's body The upper body support part rotational movement target, which is the movement target of the upper body support part drive mechanism related to the rotational movement of the upper body support part, is set to the slave so that the relationship between the a 210th processing unit for determining using the observed value of the actual posture of the upper body of the device or the determined target posture,
The master-side control section is configured to generate a rotational force for rotating the upper body support section in accordance with the determined upper body support section rotational movement target,
deviation between a target value of the slave-side reference lateral position defined by the slave-side motion target and an observed value of the slave-side reference lateral position, and the slave device defined by the slave-side motion target When any one of the deviations between the target value of the inclination of the upper body and the observed value of the inclination is defined as the first deviation, the fifth processing unit causes the first deviation to approach zero. (2) a control system for a moving object, wherein the calculated value of the first deviation is used to determine the second virtual external force; In the mobile body control system according to any one of claims 1 to 10,
The action target determination unit determines a desired slave floor reaction force, which is a floor reaction force to be applied to the slave device, using an observed value of the operator's floor reaction force, which is a floor reaction force acting on each leg of the operator. configured to include a third processing unit that determines,
The first processing unit determines a basic desired slave leg position/posture, which is a basic desired value of the position and posture of the tip of each leg of the slave device, from the observed values of the motion state of each leg of the operator. A processing unit 101 and a floor reaction force acting on each leg of the slave device for adjusting the basic desired slave leg position/orientation so that the floor reaction force actually acting on the slave device approaches the determined desired slave floor reaction force. a 102nd processing unit that corrects using the observed force values, and calculates a desired slave leg position/orientation, which is a target value of the position and orientation of the tip of each leg of the slave device after correction by the 102nd processing unit. A vehicle steering system configured to determine the target slave leg motion. In the mobile body control system according to any one of claims 1 to 10,
The action target determination unit uses an observed value of the operator's floor reaction force, which is the floor reaction force acting on each leg of the operator, to determine the desired slave floor reaction force, which is the floor reaction force to be applied to the slave device. a degree of deviation of the relationship between the observed value of the master-side reference portion lateral position and the observed value of the slave-side reference portion lateral position from the first target correspondence relationship; adding to the determined target slave floor reaction force so that at least one of the reaction force received by the operator from the upper body support portion and the lateral translational force of the upper body support portion reaction force approaches zero; A sixth processing unit that determines a compensating floor reaction force, which is the floor reaction force to be applied, using at least one of the calculated value of the degree of deviation and the observed value of the lateral translational force of the reaction force of the upper body support part configured to include and
The first processing unit determines a basic target slave leg position/posture, which is a basic target value of the position and posture of the tip of each leg of the slave device, from observed values of the actual motion state of each leg of the operator. and a 101st processing unit that adjusts the basic desired slave leg position/orientation so that the floor reaction force actually acting on the slave device approaches a desired value obtained by adding the compensating floor reaction force to the desired slave floor reaction force. and a 103rd processing unit that corrects using the observed value of the floor reaction force acting on each leg of the slave device, wherein the positions and postures of the tips of the legs of the slave device after correction by the 103rd processing unit. as the desired slave leg motion. In the mobile body control system according to any one of claims 1 to 10,
The action target determination unit uses an observed value of the operator's floor reaction force, which is the floor reaction force acting on each leg of the operator, to determine the desired slave floor reaction force, which is the floor reaction force to be applied to the slave device. a deviation between a target value of the slave side reference portion lateral position defined by the slave side motion target and an observed value of the slave side reference portion lateral position; When any one of the deviations between the target value of the inclination of the upper body of the slave device defined by the motion target and the observed value of the inclination is defined as a first deviation, the first deviation and the Add to the determined target slave floor reaction force so that at least one of the reaction force received by the operator from the upper body support portion and the lateral translational force of the upper body support portion reaction force approaches zero. A seventh processing unit that determines a compensating floor reaction force, which is the floor reaction force to be applied, using at least one of the calculated value of the first deviation and the observed value of the lateral translational force of the reaction force of the upper body support part. configured to include and
The first processing unit determines a basic target slave leg position/posture, which is a basic target value of the position and posture of the tip of each leg of the slave device, from observed values of the actual motion state of each leg of the operator. and a 101st processing unit that adjusts the basic desired slave leg position/orientation so that the floor reaction force actually acting on the slave device approaches a desired value obtained by adding the compensating floor reaction force to the desired slave floor reaction force. and a 103rd processing unit that corrects using the observed value of the floor reaction force acting on each leg of the slave device, wherein the positions and postures of the tips of the legs of the slave device after correction by the 103rd processing unit. as the desired slave leg motion. In the mobile body control system according to any one of claims 1 to 13 ,
The upper body support driving mechanism includes a moving mechanism configured to move on the floor on which the operator moves, and a first actuator capable of generating a driving force for moving the moving mechanism with respect to the floor. and
The upper body support part is mounted on the moving mechanism so as to move together with the moving mechanism,
The moving body, wherein the master-side control unit is configured to control the moving operation of the moving mechanism by controlling the operation of the first actuator according to the master-side operation target. steering system. The mobile body control system according to claim 9 or 10,
The upper body support driving mechanism includes a moving mechanism configured to move on the floor on which the operator moves, and a first actuator capable of generating a driving force for moving the moving mechanism with respect to the floor. , and a second actuator capable of generating a rotational force for rotating the upper body support,
The master-side control section is configured to perform operation control of the first actuator according to the master-side operation target, and to perform operation control of the second actuator according to the upper body support portion rotational operation target. A control system for a moving body, characterized by: The mobile body control system according to claim 14 or 15 ,
The upper body support drive mechanism includes an elevating mechanism that supports the upper body support so that it can be raised and lowered with respect to the moving mechanism, and a driving force that raises and lowers the upper body support with respect to the moving mechanism. and a possible third actuator,
The master-side control unit further controls the operation of the third actuator according to the observed value of the vertical translational force so that the vertical translational force that the operator receives from the upper body support unit becomes a predetermined value. A control system for a moving object, characterized in that it is configured to perform In the mobile body control system according to any one of claims 1 to 15 ,
A control system for a moving body, wherein the upper body support section is attached to the upper body support section drive mechanism so as to be vertically movable. In the mobile body control system according to any one of claims 1 to 15 ,
A control system for a moving object, wherein the upper body support is attached to the upper body support drive mechanism so as to be elastically movable in the vertical direction. In the mobile body control system according to any one of claims 1 to 8,
The upper body support is attached to the upper body support drive mechanism so as to be rotatable in the roll direction of the operator, in the pitch direction of the operator, or in both the roll and pitch directions. A control system for a moving object, characterized by: A maneuvering system capable of maneuvering to move a slave device, which is a legged mobile body having an upper body and two legs extending from the upper body, comprising:
an upper body support attached to the operator's upper body so as to move with the operator as the operator moves; an upper body support drive mechanism applicably attached to the support; and an upper body support drive mechanism;
a motion target determination unit that determines a slave-side motion target that is the motion target of the slave device and a master-side motion target that is the motion target of the upper body support portion driving mechanism of the master device;
a master-side control unit that controls the operation of the upper body support unit drive mechanism according to the determined master-side operation target;
a slave-side control unit that controls the operation of the slave device according to the determined slave-side operation target,
The lateral position of any one of the upper body support, the operator's upper body, and the operator's center of gravity is defined as the master-side reference part lateral position, and the slave device's upper body and center of gravity of the slave device. When the lateral position of any one of them is defined as the lateral position of the slave side reference part,
The motion target determination unit
The slave side motion target is determined so as to move the slave device along with the movement of the operator, and the actual lateral position of the master side reference portion and the actual lateral position of the slave side reference portion are determined. determining the master-side motion goal to cause the upper body support to exert a lateral translational force on the operator in a direction that reduces the deviation of the relationship between A control system for a moving object, comprising:

Images