Control device, control method, control program, and two-wheeled inverted pendulum robot

Abstract

This control device controls driving of a two-wheeled inverted pendulum robot that includes a pair of wheels, a wheel motor that outputs torque for rotating the wheels to an axle, and a body attached to the pair of wheels and provided so as to be capable of pivoting about the axle, wherein the control device includes: an external force estimating unit that calculates an estimated value of an external force applied to the two-wheeled inverted pendulum robot; and a deflection behavior generating unit that generates a command value for executing a deflection behavior to return the body to an inverted equilibrium angle in response to the estimated value of the external force such that a pushing-back force is equal to or less than a threshold. The present invention thus provides a control device capable of allowing inverted pendulum robots to collaborate safely with humans.

Description

Control device, control method, control program, and two-wheeled inverted robot

[0001] The present invention relates to a control device, a control method, a control program, and a two-wheeled inverted robot.

[0002] Patent Document 1 describes a technology that uses an extension sensor to detect contact between an automated guided vehicle (AGV) and an object, thereby protecting the AGV. This allows for the protection of the transported object.

[0003] Japanese Patent Publication No. 2021-37776

[0004] Takuro Kawamura and Toshiyuki Murakami, Multifunctional Control of a Two-Wheel Drive Wheelchair Considering Passenger Comfort, Transactions of the Institute of Electrical Engineers of Japan (Industrial Applications Division), Vol. 133, No. 4, 2013, pp. 404-413.

[0005] When humans and transport robots work together in the same space, it is necessary to consider not only the safety of the goods being transported, but also the safety of the people working on the task.

[0006] When an object comes into contact with a robot, an external force is applied to the robot. In the case of a two-wheeled inverted robot, where the body is pivotably mounted around an axle, when an external force is applied, it is necessary to control the robot to rotate the body in the opposite direction to the external force in order to maintain the inverted posture. This control generates a force that pushes back against the external force from the object it comes into contact with. If this force is strong, a strong force will be applied to the object that comes into contact with the two-wheeled inverted robot. Therefore, there is a need to prevent contact between humans and two-wheeled inverted robots while enabling cooperation between humans and two-wheeled inverted robots, which presents a challenge as it significantly restricts the types of work that can be done and the workspace available.

[0007] The present invention has been made in view of the above, and aims to provide a control device, a control method, a control program, and a two-wheeled inverted robot that can safely cooperate with a person using an inverted robot.

[0008] To solve the above-mentioned problems and achieve the objective, a control device according to one aspect of the present invention is a control device for controlling the drive of a two-wheeled inverted robot comprising a pair of wheels, a wheel motor that outputs torque to an axle for rotating the wheels, and a body attached to the pair of wheels and provided to be swingable around the axle, comprising: an external force estimation unit that calculates an estimated value of an external force applied to the two-wheeled inverted robot, and a deflection behavior generation unit that generates a command value for executing a deflection behavior that returns the body to an inverted balance angle in response to the estimated value of the external force such that the pushing force is below a threshold.

[0009] Furthermore, in one aspect of the present invention, the control device generates the command value by compliance control that returns the body to the inverted equilibrium angle, using the equation of motion of a spring in which coefficients are set such that the force pushing back in response to the estimated value of the external force is below a threshold value.

[0010] Furthermore, in one aspect of the present invention, the control device is configured such that the command value is a position command value indicating the position of the two-wheeled inverted robot.

[0011] Furthermore, in one aspect of the present invention, the control device is configured such that the command value is a speed command value that instructs the speed of the two-wheeled inverted robot.

[0012] Furthermore, a control device according to one aspect of the present invention includes a pitch disturbance estimation unit that calculates pitch angular acceleration from a detected value of the pitch angle, which is the angle between the vertical axis of the body and the vertical direction; calculates wheel angular acceleration from a detected value of the wheel angle, which is the rotation angle of the wheel from a predetermined position; and calculates an estimated value of pitch disturbance, which is the force that rotates the body in the pitch angle direction, based on the torque command value input to the wheel motor, the pitch angular acceleration, and the wheel angular acceleration.

[0013] Furthermore, in one aspect of the present invention, the control device includes an external force estimation unit which calculates an estimated value of the external force applied to the two-wheeled inverted robot based on the estimated value of the pitch disturbance.

[0014] Furthermore, in a control device according to one aspect of the present invention, the external force estimation unit calculates an estimated value of the external force applied to the two-wheeled inverted robot by subtracting the gravitational force applied to the body and the force applied to the wheels from the estimated value of the pitch disturbance.

[0015] Furthermore, a control method according to one aspect of the present invention is a control method for controlling the drive of a two-wheeled inverted robot comprising a pair of wheels, a wheel motor that outputs torque to an axle for rotating the wheels, and a body attached to the pair of wheels and provided to be swingable around the axle, wherein a processor calculates an estimated value of the external force applied to the two-wheeled inverted robot, reads the estimated value of the external force from memory, and generates a command value for executing a deflection behavior that returns the body to an inverted equilibrium angle in response to the estimated value of the external force such that the force pushing back is less than or equal to a threshold.

[0016] Furthermore, a control program according to one aspect of the present invention is a control program for controlling the drive of a two-wheeled inverted robot comprising a pair of wheels, a wheel motor that outputs torque to an axle for rotating the wheels, and a body attached to the pair of wheels and provided to be swingable around the axle, wherein the program causes a processor to calculate an estimated value of the external force applied to the two-wheeled inverted robot and to generate a command value for executing a deflection behavior that returns the body to an inverted equilibrium angle such that the force pushing back in response to the estimated value of the external force is below a threshold.

[0017] Furthermore, a two-wheeled inverted robot according to one aspect of the present invention comprises a pair of wheels, a wheel motor that outputs torque to the axle to rotate the wheels, a body attached to the pair of wheels and provided to be swingable around the axle, and a control device that controls the driving of the two-wheeled inverted robot, wherein the control device comprises an external force estimation unit that calculates an estimated value of the external force applied to the two-wheeled inverted robot, and a deflection behavior generation unit that generates a command value for executing a deflection behavior that returns the body to an inverted equilibrium angle in response to the estimated value of the external force such that the pushing force is below a threshold.

[0018] Furthermore, a control device according to one aspect of the present invention is a control device for controlling the drive of an inverted robot comprising a wheel, a wheel motor that outputs torque to an axle for rotating the wheel, a connecting part located on the wheel, and a body attached to the connecting part and provided to be swingable around the connecting part, the control device comprising: an external force estimation unit that calculates an estimated value of an external force applied to the body, and a deflection behavior generation unit that generates a command value for executing a deflection behavior to return the body to an inverted equilibrium angle in response to the estimated value of the external force such that the force pushing back is below a threshold.

[0019] According to the present invention, a control device, a control method, a control program, and a two-wheeled inverted robot can be realized that enable an inverted robot to safely cooperate with a human.

[0020] Figure 1 is a schematic diagram of the configuration of a two-wheeled inverted robot. Figure 2 is a block diagram showing the control of a two-wheeled inverted robot including a control device according to Embodiment 1. Figure 3 is a flowchart showing an overview of the processing performed by the control device according to Embodiment 1. Figure 4 is a block diagram showing the control of a two-wheeled inverted robot including a control device according to Embodiment 2. Figure 5 is a block diagram showing the control of a two-wheeled inverted robot including a control device according to Embodiment 3. Figure 6 is a flowchart showing an overview of the processing performed by the control device according to Embodiment 3. Figure 7 is a block diagram showing the control of a two-wheeled inverted robot including a control device according to Embodiment 4. Figure 8 is a schematic diagram of the configuration of an inverted robot according to Embodiment 5. Figure 9 is a schematic diagram of the configuration of a two-wheeled inverted robot according to Embodiment 6.

[0021] The following describes embodiments of the control device, control method, control program, and two-wheeled inverted robot according to the present invention with reference to the drawings. However, the present invention is not limited to these embodiments. The present invention can be applied to control devices, control methods, control programs, and two-wheeled inverted robots in general for controlling the drive of a two-wheeled inverted robot.

[0022] Furthermore, in the drawings, identical or corresponding elements are appropriately denoted by the same reference numeral. It should also be noted that the drawings are schematic, and the dimensional relationships and proportions of each element may differ from reality. Even between drawings, there may be differences in dimensional relationships and proportions.

[0023] (Embodiment 1) [Configuration of a two-wheeled inverted robot] Figure 1 is a schematic configuration diagram of a two-wheeled inverted robot. As shown in Figure 1, the two-wheeled inverted robot 1 comprises a pair of wheels 2, wheel motors 3, a body 4, an arm 5, a fork 6, and a fork motor 7.

[0024] The two-wheeled inverted robot 1 is a two-wheeled inverted robot in which a pair of wheels 2 are in contact with the ground and a body 4 attached to the wheels 2 maintains an inverted posture. The two-wheeled inverted robot 1 can transport goods in the same space as people, for example, in logistics centers or production plants.

[0025] The pair of wheels 2 are located at both ends of the axle.

[0026] The wheel motor 3 outputs torque to the axle that rotates the wheel 2.

[0027] The body 4 is attached to a pair of wheels 2 and is configured to swing around the axle. In the following description, the longitudinal direction of the body 4 perpendicular to the axle (the vertical direction in Figure 1) will be referred to as the vertical axis of the body 4, and the direction along the axle (the direction perpendicular to the plane of the paper in Figure 1) will be referred to as the horizontal axis of the body 4.

[0028] Arm 5 extends from the body 4 to the side opposite the wheel 2. The length of arm 5 may be variable, and the direction in which it extends from the body 4 may also be variable.

[0029] The fork 6 is attached to an arm 5 extending from the body 4 and is capable of carrying loads. Under the control of the fork motor 7, the direction in which the fork 6 extends from the arm 5 is changed to prevent the loaded load from falling.

[0030] The fork motor 7 outputs torque to control the angle of the fork 6.

[0031] Figure 2 is a block diagram showing the control of a two-wheeled inverted robot including a control device according to Embodiment 1. As shown in Figure 2, the two-wheeled inverted robot 1 includes a pitch angle detection unit 11 and a wheel angle detection unit 12.

[0032] The pitch angle detection unit 11 is, for example, a gyro sensor attached to the body 4, and the pitch angle θ is the angle between the vertical axis of the body 4 and the vertical direction. ｐ Detects.

[0033] The wheel angle detection unit 12 is, for example, an encoder attached to the wheel 2, and the wheel angle θ is the rotation angle from a predetermined position of the wheel 2. ｗ Detects.

[0034] [Configuration of the control device] The control device 20 controls the drive of the two-wheeled inverted robot 1. The control device 20 is implemented by a processor, such as a CPU (Central Processing Unit), and a memory (main memory unit), such as RAM (Random Access Memory) or ROM (Read Only Memory).

[0035] The control device 20 may be built into the two-wheeled inverted robot 1. Alternatively, the control device 20 may be a computer or server installed outside the two-wheeled inverted robot 1, and may control the two-wheeled inverted robot 1 via a network such as the Internet, mobile phone network, Wi-Fi (registered trademark, Wireless Fidelity), or BLE (Bluetooth® Low Energy).

[0036] The control device 20 includes a position command value generation unit 21, a robot position control controller 22, a robot pitch angle control controller 23, a pitch disturbance estimation unit 24, an external force estimation unit 25, and a deflection behavior generation unit 26.

[0037] The position command value generation unit 21 generates a position command value θ that indicates the position of the two-wheeled inverted robot 1. ｗ ｃｍｄ Generate and output the following.

[0038] The robot position controller 22 receives the position command value θ generated by the position command value generation unit 21 ｗ ｃｍｄ , the position command value θ generated by the compliance behavior generation unit 26 ｐоｓ ｃｍｄ , and the wheel angle θ detected by the wheel angle detection unit 12 ｗ calculates and outputs a pitch angle command value θ for controlling the position of the body 4 based on the above ｐ ｃｍｄ .

[0039] The robot pitch angle controller 23 receives the pitch angle command value θ output by the robot position controller 22 ｐ ｃｍｄ , and the pitch angle θ detected by the pitch angle detection unit 11 ｐ calculates and outputs a torque command value τ output by the wheel motor 3 to the wheels 2 for maintaining the inverted posture of the body 4 based on the above ｗ ｒｅｆ . When the influence of the load carried by the fork 6 is to be considered, for example, a pitch angle command value θ for maintaining an inverted posture with the fork carrying the load is ｐ ｃｍｄ_ｌоａｄ calculated, and the robot pitch angle controller 23 also takes into account the pitch angle command value θ ｐ ｃｍｄ_ｌоａｄ to calculate the torque command value τ ｗ ｒｅｆ .

[0040] The pitch disturbance estimation unit 24 obtains an estimated value τ of pitch disturbance, which is a force that rotates the body 4 in the pitch angle direction ｐ ｄｉｓ with hat (hereafter, an estimated value is described as "hat" in the specification, and is represented as a character with an umbrella-shaped symbol above it in the drawings) and calculates this estimated value. Specifically, the pitch disturbance estimation unit 24 first receives the pitch angle θ ｐ , which is a pitch angular acceleration θ obtained by performing second-order differentiation with respect to time on the detected value of ｐ with two dots (hereafter, differentiation with respect to time is described as "dot" in the specification, and is represented as a character with a dot above it in the drawings. In the case of second-order differentiation with respect to time, it is described as "two dots" in the specification, and is represented as a character with two dots above it in the drawings) and the wheel angle θ ｗThe wheel angular acceleration θ was calculated by performing a second derivative calculation with respect to time from the detected value. ｗ (Two dots) is input. Next, the pitch disturbance estimation unit 24 inputs the torque command value τ to the wheel motor 3. ｗ ｒｅｆ and pitch angular acceleration θ ｐ (Two dots) and wheel angular acceleration θ ｗ Estimated pitch disturbance τ based on (two dots) ｐ ｄｉｓ The (hat) is calculated and output. Note that Figure 2 shows the pitch angle θ outside the pitch disturbance estimation unit 24. ｐ The second derivative with respect to time is calculated from the detected value to determine the pitch angular acceleration θ. ｐ A calculation circuit that calculates (two dots), and the wheel angle θ ｗ The second derivative with respect to time is calculated from the detected value to determine the wheel angular acceleration θ. ｗ Although a configuration is shown in which an arithmetic circuit for calculating (two dots) is provided, a configuration in which the pitch disturbance estimation unit 24 performs these calculations may also be used.

[0041] The external force estimation unit 25 processes the estimated pitch disturbance τ output by the pitch disturbance estimation unit 24. ｐ ｄｉｓ Based on (Hatt), estimate of the external force τ from an object, including a person, acting on the two-wheeled inverted robot 1. ｈｕｍａｎ ｄｉｓ The (hat) is calculated. Specifically, the external force estimation unit 25 first calculates the wheel angle θ. ｗ The wheel angular velocity θ was calculated by first-order differentiation with respect to time from the detected value. ｗ A (dot) is entered. Next, the external force estimation unit 25 calculates the pitch angle θ ｐ Based on the detected values, the estimated value of gravity (vehicle weight force term) acting on body 4 is calculated. Furthermore, the external force estimation unit 25 calculates the pitch angle θ ｐ Detected values ​​and wheel angular velocity θ ｗ (Dot) and torque command value τ ｗ ｒｅｆ Based on this, for example, by using the Quinn model, an estimated value of the force applied to wheel 2 (bearing friction term) is calculated. In addition, the external force estimation unit 25 calculates the wheel angle θ ｗThe detected value may be used to calculate an estimated value of the force applied to the wheel 2. The external force estimation unit 25 then calculates the estimated value of the pitch disturbance τ ｐ ｄｉｓ By subtracting the estimated gravitational force acting on body 4 and the estimated force acting on wheel 2 from (hat), the estimated external force τ acting on the two-wheeled inverted robot 1 is obtained. ｈｕｍａｎ ｄｉｓ Calculate and output the (hat). Estimated value of pitch disturbance τ ｐ ｄｉｓ (Hat) is the pitch angle θ ｐ Includes all directional disturbances. Estimated pitch disturbance τ ｐ ｄｉｓ By subtracting the estimable terms (such as gravity acting on body 4 and the force acting on wheel 2) from (Hat), we obtain the estimated value τ of the external force acting from an object including a person. ｈｕｍａｎ ｄｉｓ (Hats) can be calculated more accurately (see Non-Patent Document 1, etc. for details). However, the estimated value of pitch disturbance τ ｐ ｄｉｓ The term to be subtracted from (hat) is just an example and is not particularly limited. Furthermore, the external force estimation unit 25 estimates the pitch disturbance τ ｐ ｄｉｓ (Hat) is the estimated value of the external force τ ｈｕｍａｎ ｄｉｓ It may also be used as a (hat). Note that Figure 2 shows the wheel angle θ outside the external force estimation unit 25. ｗ The wheel angular velocity θ is calculated by performing a first-order derivative operation with respect to time from the detected value. ｗ Although a configuration is shown in which a calculation circuit for calculating (dots) is provided, this calculation may also be performed by the external force estimation unit 25.

[0042] The deflection behavior generation unit 26 generates an estimated value of the external force τ ｈｕｍａｎ ｄｉｓ The position command value θ is used to execute a deflection behavior that returns body 4 to the inverted equilibrium angle so that the force pushing back in response to (hat) is below a threshold. ｐоｓ ｃｍｄSpecifically, the deflection behavior generation unit 26 uses equations of motion for the wheels 2 and body 4, with coefficients set so that the force pushing back against external forces is below a threshold, to perform compliance control to return the body 4 to the inverted balance angle, thereby generating a position command value θ. ｐоｓ ｃｍｄ This generates the following. Here, the inverted equilibrium angle is the pitch angle θ at which the posture of the body 4 stabilizes when the wheel 2 and body 4 are considered as an inverted pendulum. ｐ Therefore, when the wheel motor 3 is not outputting torque, the pitch angle θ ｐ The pitch angle θ at which all forces acting in a particular direction are balanced. ｐ This inverted balance angle changes depending on the weight of the load placed on the fork 6, so the deflection behavior generation unit 26 adjusts the inverted balance angle according to the weight of the load and the pitch angle θ of the body 4. ｐ Put it back.

[0043] [Deflection Behavior] Deflection behavior is a control that enables the body 4 to deflect external forces. Specifically, the deflection behavior is a control that, for example, tilts the body 4 in the direction of the external force estimated by the external force estimation unit 25, and then returns the body 4 to the inverted equilibrium angle. Also, if the external force estimated by the external force estimation unit 25 is small, the tilt of the body 4 in the direction of the estimated external force may be small enough that it cannot be visually confirmed. Furthermore, if the body 4 is tilted to one side when no external force is applied, such as when the forks 6 are not carrying a load, then an external force applied to the opposite side may cancel out the original tilt and bring it into equilibrium. Moreover, if the body 4 is tilted to one side when no external force is applied, such as when the forks 6 are not carrying a load, then even if an external force is applied to the opposite side, the body 4 may still be tilted to the side opposite to the direction of the external force.

[0044] First, if we consider the wheels 2 and body 4 as an inverted pendulum, the pitch angle θ ｐ The motion of body 4 in the direction can be expressed using the equation of motion for a spring. Specifically, the equation of motion is expressed by the coefficients M of the pitch angular acceleration, pitch angular velocity, and pitch angle. ｃ , D ｃ _K ｃ, where τ is the external force from an object including a person ｈｕｍａｎ and is expressed by the following formula (1). In formula (1), as in the drawings, the pitch angle θ ｐ time derivative is indicated by a dot.

[0045] Then, the compliant behavior generation unit 26 adjusts the pitch angular acceleration θ ｐ double dot, pitch angular velocity θ ｐ dot, and pitch angle θ ｐ to satisfy formula (1), and generates a position command value θ ｐоｓ ｃｍｄ This enables implementation of compliance control that compensates for positional deviation of the pitch angle direction from the inverted balance angle caused by external force, while having a restoring force like a spring against the external force.

[0046] Here, coefficients M ｃ , D ｃ , K ｃ are preset in the compliant behavior generation unit 26 such that the force pushing back against external force is equal to or less than a threshold value. Then, when the estimated external force value τ ｈｕｍａｎ ｄｉｓ hat estimated by the external force estimation unit 25 is input to formula (1) as τ ｈｕｍａｎ , the compliant behavior generation unit 26 can calculate the pitch angular acceleration θ ｐоｓ ｃｍｄ double dot, pitch angular velocity θ ｐ dot, and pitch angle θ ｐ for calculating the position command value θ ｐ of compliant behavior.

[0047] [Control Method for Two-Wheel Inverted Robot] Next, a control method for the two-wheel inverted robot 1 executed by the control device 20 will be described. FIG. 3 is a flowchart showing an outline of processing executed by the control device according to Embodiment 1. When an external force is applied to the two-wheel inverted robot 1, as shown in FIG. 3, the pitch disturbance estimation unit 24 calculates an estimated pitch disturbance value τ ｐ ｄｉｓ hat (step S1).

[0048] Next, the external force estimation unit 25 processes the estimated pitch disturbance τ output by the pitch disturbance estimation unit 24. ｐ ｄｉｓ Based on the (Hat) diagram, the estimated value of the external force τ applied to the two-wheeled inverted robot 1 is ｈｕｍａｎ ｄｉｓ Calculate the (hat) (step S2).

[0049] Then, the deflection behavior generation unit 26 generates an estimated value of the external force τ ｈｕｍａｎ ｄｉｓ Based on the (hat), the position command value θ for executing the evasive behavior. ｐоｓ ｃｍｄ Generate (step S3).

[0050] The position command value θ generated by the position command value generation unit 21 ｗ ｃｍｄ In response, the pitch disturbance estimation unit 24, the external force estimation unit 25, and the deflection behavior generation unit 26 execute the processes in steps S1 to S3, and these are input to the robot position control controller 22. Furthermore, the robot position control controller 22 receives the pitch angle command value θ. ｐ ｃｍｄ The torque command value τ is input to the robot pitch angle control controller 23, and the torque command value τ is output by the robot pitch angle control controller 23. ｗ ｒｅｆ The two-wheeled inverted robot 1 is driven accordingly. This reduces the force with which the two-wheeled inverted robot 1 pushes back against external forces to below a threshold, ensuring the safety of the collaborating person while maintaining the inverted posture of the body 4.

[0051] Furthermore, when the external force applied to the two-wheeled inverted robot 1 is removed, the position command value θ is generated by the deflection behavior generation unit 26. ｐоｓ ｃｍｄ This becomes zero. As a result, the position command value θ generated by the position command value generation unit 21 ｗ ｃｍｄ This automatically returns the two-wheeled inverted robot 1 to its original control system.

[0052] According to Embodiment 1 described above, the deflection behavior generation unit 26 generates an estimated value of the external force τ ｈｕｍａｎ ｄｉｓThe position command value θ is used to execute a deflection behavior that returns body 4 to the inverted equilibrium angle so that the force pushing back in response to (hat) is below a threshold. ｐоｓ ｃｍｄ This generates a force that the two-wheeled inverted robot 1 exerts to push back against external forces, reducing the force below a threshold and enabling the two-wheeled inverted robot 1 to safely collaborate with humans.

[0053] Furthermore, according to Embodiment 1, regardless of which part of the two-wheeled inverted robot 1 is subjected to external force, control can be performed that reduces the force pushing back against the external force to below a threshold. In contrast, when detecting external force using a contact sensor such as the extension sensor described in Patent Document 1, it is not possible to detect external forces applied to parts other than the sensor.

[0054] Furthermore, according to Embodiment 1, the torque command value τ input to the wheel motor 3 ｗ ｒｅｆ , pitch angle θ ｐ The detected values ​​and the wheel angle θ ｗ Because it detects external force based on the detected value, it is less prone to failure. In contrast, when using a contact sensor such as the extension sensor in Patent Document 1, force is applied directly to the contact sensor, making it prone to failure.

[0055] Furthermore, according to Embodiment 1, because the two-wheeled inverted robot 1 is a two-wheeled inverted type, it occupies less floor space than a configuration with three or more wheels, and is less likely to get in the way when working with people.

[0056] Furthermore, in the above-described embodiment 1, the pitch disturbance estimation unit 24 uses the torque command value τ input to the wheel motor 3. ｗ ｒｅｆ , pitch angle θ ｐ The detected values ​​and the wheel angle θ ｗ Using the detected values, the estimated pitch disturbance τ ｐ ｄｉｓ An example of calculating the hat has been explained, but it is not limited to this. The pitch disturbance estimation unit 24 calculates the torque command value τ input to the wheel motor 3. ｗ ｒｅｆ Estimated pitch disturbance τ without using ｐ ｄｉｓ You may calculate the hat.

[0057] Furthermore, in the above-described embodiment 1, the pitch angle detection unit 11 and the wheel angle detection unit 12 detect the pitch angle θ ｐ and wheel angle θ ｗ An example of detecting the pitch angle θ has been described, but it is not limited to this. The pitch angle detection unit 11 or the wheel angle detection unit 12 detects the pitch angle θ ｐ or wheel angle θ ｗ Detect either one of the following and estimate the pitch disturbance τ ｐ ｄｉｓ (Hats) may be calculated. For example, the pitch disturbance estimation unit 24 calculates the pitch angle θ detected by the pitch angle detection unit 11. ｐ From pitch angular acceleration θ ｐ (Two dots) is calculated, and the wheel disturbance is used as the error to estimate the pitch disturbance τ ｐ ｄｉｓ (The hat) can be calculated.

[0058] Furthermore, in the embodiment 1 described above, the pitch angle detection unit 11 and the wheel angle detection unit 12 each detect the pitch angle θ ｐ and wheel angle θ ｗ An example of detecting this has been described, but it is not limited to this. For example, by measuring the acceleration between the upper and lower parts of the body 4 using two acceleration sensors, the pitch angular acceleration θ can be directly detected. ｐ (Two dots) may be measured. Also, by measuring the position of wheel 2 with a distance sensor, the wheel angular acceleration θ can be measured indirectly. ｗ (Two dots) may also be measured. Furthermore, by measuring the axle acceleration, the wheel angular acceleration θ can be indirectly determined. ｗ You may measure (two dots).

[0059] (Embodiment 2) Figure 4 is a block diagram showing the control of a two-wheeled inverted robot including a control device according to Embodiment 2. As shown in Figure 4, the control device 20A comprises a pitch angle command value generation unit 27A and a robot pitch angle control controller 23A. Other configurations may be the same as in Embodiment 1, so their descriptions will be omitted as appropriate.

[0060] The pitch angle command value generation unit 27A generates the pitch angle θ ｐ The pitch angle command value θ that indicates ｐ ｃｍｄ Generate and output the following.

[0061] The robot pitch angle control controller 23A receives the pitch angle command value θ generated by the pitch angle command value generation unit 27A. ｐ ｃｍｄ , and the pitch angle θ detected by the pitch angle detection unit 11 ｐ Based on this, the torque command value τ that the wheel motor 3 outputs to the wheel 2 in order to maintain the inverted position of the body 4. ｗ２ ｒｅｆ The torque command value τ output by the robot position control controller 22 is calculated. The wheel motor 3 receives the torque command value τ output by the robot position control controller 22. ｗ１ ｒｅｆ The torque command value τ output by the robot pitch angle control controller 23A ｗ２ ｒｅｆ The torque command value τ is the sum of the two. ｗ ｒｅｆ The following is entered.

[0062] As in Embodiment 2, the position command value θ ｗ ｃｍｄ and pitch angle command value θ ｐ ｃｍｄ When inputting and performing feedback control, the deflection behavior generation unit 26 executes a deflection behavior, thereby reducing the force with which the two-wheeled inverted robot 1 pushes back against external forces, allowing the two-wheeled inverted robot 1 to safely cooperate with humans.

[0063] According to Embodiment 2, the position command value θ ｗ ｃｍｄ and pitch angle command value θ ｐ ｃｍｄ To generate the position command value θ, ｗ ｃｍｄ The reaction is faster compared to when only the product is produced (Embodiment 1).

[0064] (Embodiment 3) Figure 5 is a block diagram showing the control of a two-wheeled inverted robot including a control device according to Embodiment 3. As shown in Figure 5, the control device 20B includes a speed command value generation unit 28B, a robot speed control controller 29B, and a dodge behavior generation unit 26B. Other configurations may be the same as in Embodiment 1, so their descriptions will be omitted as appropriate.

[0065] The speed command value generation unit 28B generates a speed command value θ that instructs the speed of the two-wheeled inverted robot 1. ｗ ｃｍｄ Generate and output a (dot).

[0066] The robot speed control controller 29B receives the speed command value θ generated by the speed command value generation unit 28B. ｗ ｃｍｄ (Dot), Speed ​​command value θ generated by the evasive behavior generation unit 26B ｐｏｓ ｃｍｄ (Dot), and the wheel angle θ detected by the wheel angle detection unit 12 ｗ The wheel angular velocity θ was calculated by first-order differentiation with respect to time from the detected value. ｗ Based on (dot), the pitch angle command value θ is used to control the speed of the two-wheeled inverted robot 1. ｐ ｃｍｄ Calculate and output the result.

[0067] The deflection behavior generation unit 26B generates an estimated value of the external force τ ｈｕｍａｎ ｄｉｓ The velocity command value θ is used to execute a deflection behavior that returns body 4 to the inverted equilibrium angle so that the force pushing back in response to (hat) is below a threshold. ｐｏｓ ｃｍｄ Generate a (dot).

[0068] Figure 6 is a flowchart showing an overview of the process performed by the control device according to Embodiment 3. When an external force is applied to the two-wheeled inverted robot 1, as shown in Figure 6, the pitch disturbance estimation unit 24 estimates the pitch disturbance τ ｐ ｄｉｓ Calculate the (hat) (step S11).

[0069] Next, the external force estimation unit 25 processes the estimated pitch disturbance τ output by the pitch disturbance estimation unit 24. ｐ ｄｉｓ Based on the (Hat) diagram, the estimated value of the external force τ applied to the two-wheeled inverted robot 1 is ｈｕｍａｎ ｄｉｓ Calculate the (hat) (step S12).

[0070] Then, the deflection behavior generation unit 26B generates an estimated value of the external force τ ｈｕｍａｎ ｄｉｓ Based on (Hat), the velocity command value θ for performing the deflection behavior.ｐｏｓ ｃｍｄ (A dot) is generated (step S13).

[0071] As in Embodiment 3, the speed command value θ ｗ ｃｍｄ Even in a configuration where feedback control is performed by inputting (dots), the evasive behavior generation unit 26B performs the evasive behavior, thereby reducing the force with which the two-wheeled inverted robot 1 pushes back against external forces, and enabling the two-wheeled inverted robot 1 to safely cooperate with humans.

[0072] Furthermore, according to Embodiment 3, the deflection behavior generation unit 26 generates a speed command value θ ｐｏｓ ｃｍｄ Because it generates (dots), the response is faster compared to when generating position command values ​​(Embodiment 1).

[0073] (Embodiment 4) Figure 7 is a block diagram showing the control of a two-wheeled inverted robot including the control device according to Embodiment 4. As shown in Figure 7, the control device 20C includes a speed command value generation unit 28C, a robot speed control controller 29C, a pitch angle command value generation unit 27C, a robot pitch angle control controller 23C, and a dodge behavior generation unit 26C. Other configurations may be the same as in Embodiment 1, so their descriptions will be omitted as appropriate.

[0074] The speed command value generation unit 28C generates a speed command value θ that instructs the speed of the two-wheeled inverted robot 1. ｗ ｃｍｄ Generate and output a (dot).

[0075] The robot speed control controller 29C receives the speed command value θ generated by the speed command value generation unit 28C. ｗ ｃｍｄ (Dot), Speed ​​command value θ generated by the evasive behavior generation unit 26C ｐｏｓ ｃｍｄ (Dot), and the wheel angle θ detected by the wheel angle detection unit 12 ｗ The wheel angular velocity θ was calculated by first-order differentiation with respect to time from the detected value. ｗ Based on (dot), the torque command value τ that the wheel motor 3 outputs to the wheel 2 controls the speed of the two-wheeled inverted robot 1. ｗ１ ｒｅｆ Calculate.

[0076] The pitch angle command value generation unit 27C generates the pitch angle θ ｐ The pitch angle command value θ that indicates ｐ ｃｍｄ Generate and output the following.

[0077] The robot pitch angle control controller 23C receives the pitch angle command value θ generated by the pitch angle command value generation unit 27C. ｐ ｃｍｄ , and the pitch angle θ detected by the pitch angle detection unit 11 ｐ Based on this, the torque command value τ that the wheel motor 3 outputs to the wheel 2 in order to maintain the inverted position of the body 4. ｗ２ ｒｅｆ The torque command value τ output by the robot speed control controller 29C is calculated. The wheel motor 3 receives the torque command value τ. ｗ１ ｒｅｆ The torque command value τ output by the robot pitch angle control controller 23C ｗ２ ｒｅｆ The torque command value τ is the sum of the two. ｗ ｒｅｆ The following is entered.

[0078] The deflection behavior generation unit 26C generates a velocity command value θ for executing a deflection behavior that returns the body 4 to the inverted balance angle so that the force pushing back in response to the estimated external force is below a threshold. ｐｏｓ ｃｍｄ Generate a (dot).

[0079] As in Embodiment 4, the speed command value θ ｗ ｃｍｄ (Dot) and pitch angle command value θ ｐ ｃｍｄ When inputting and performing feedback control, the deflection behavior generation unit 26C executes a deflection behavior, thereby reducing the force with which the two-wheeled inverted robot 1 pushes back against external forces, allowing the two-wheeled inverted robot 1 to safely cooperate with humans.

[0080] Furthermore, according to Embodiment 4, the speed command value θ w ｃｍｄ (Dot) and pitch angle command value θ ｐ ｃｍｄ The behavior generation unit 26 generates a speed command value θ. ｐｏｓｃｍｄ Because it generates (dots), the reaction is even faster compared to embodiments 1 to 3.

[0081] (Embodiment 5) Figure 8 is a schematic diagram of the inverted robot according to Embodiment 5. As shown in Figure 8, the inverted robot 101 includes wheels 102, wheel motors 103, a base 104, a connecting part 105, and a body 106. The inverted robot 101 may have a control device built in to control the drive of the inverted robot 101, or the control device may be installed outside the inverted robot 101.

[0082] There are four wheels 102 in total, one pair at the front and one at the rear. Torque from the wheel motors 103 moves the inverted robot 101 forward and backward (left and right directions in Figure 8).

[0083] The wheel motor 103 outputs torque to rotate the rotation axis of the wheel 102. The wheel motor 103 may rotate either the front or rear wheel 102, or both.

[0084] The connecting portion 105 is provided on a base portion 104 located on the wheel 102.

[0085] The body 106 is attached to the connecting portion 105 and is provided to be able to swing around the connecting portion 105.

[0086] In this inverted robot 101, the connection part 105 and the body 106 can be considered as an inverted pendulum. The control device then estimates the external force τ applied to the body 106 of the inverted robot 101. ｈｕｍａｎ ｄｉｓ (Hat) is calculated, and the estimated value of the external force τ ｈｕｍａｎ ｄｉｓ In response to the hat signal, a command value is generated to execute a deflection behavior that returns the body 106 to the inverted balance angle so that the force pushing back is below a threshold, thereby controlling the drive of the wheel motor 103. As a result, the force with which the body 106 pushes back against external forces is reduced to below a threshold, allowing the inverted robot 101 to safely cooperate with humans.

[0087] As shown in Embodiment 5, not limited to two-wheeled robots, any inverted robot can perform a deflection behavior in response to external forces applied to a part that can be considered an inverted pendulum. This reduces the force with which this part pushes back against the external force to below a threshold, allowing the inverted robot 101 to safely cooperate with humans.

[0088] (Embodiment 6) Figure 9 is a schematic diagram of a two-wheeled inverted robot according to Embodiment 6. As shown in Figure 9, the two-wheeled inverted robot 111 comprises wheels 112, wheel motors 113, a body 114, two movable parts 115, two arms 116, and a gripping part 117.

[0089] The pair of wheels 112 are located at both ends of the axle.

[0090] The wheel motor 113 outputs torque to the axle that rotates the wheel 112.

[0091] The body 114 is attached to a pair of wheels 112 and is configured to swing around the axle.

[0092] In this two-wheeled inverted robot 111, movable parts 115 to gripping parts 117 are provided on the upper part of the body 114. The control device then estimates the external force τ applied to the two-wheeled inverted robot 111. ｈｕｍａｎ ｄｉｓ (Hat) is calculated, and the estimated value of the external force τ ｈｕｍａｎ ｄｉｓ The system controls the drive of the wheel motor 113 by generating command values ​​to perform a deflection behavior that returns the body 114 to the inverted balance angle so that the force pushing back in response to the (hat) is below a threshold. As a result, the force that the two-wheeled inverted robot 111 exerts in response to external forces is reduced to below a threshold, allowing the two-wheeled inverted robot 111 to safely cooperate with humans.

[0093] As shown in Embodiment 6, various components may be arranged on the upper part of the body 114, and the configuration of the upper part of the body 114 is not particularly limited. In this configuration, no matter which part of the upper part of the body 114 is subjected to an external force, the force pushing back against the external force is reduced to below a threshold, allowing the two-wheeled inverted robot 111 to safely cooperate with a person.

[0094] Further effects and modifications can be readily derived by those skilled in the art. Therefore, broader aspects of the present invention are not limited to the specific details and representative embodiments expressed and described above. Accordingly, various modifications are possible without departing from the spirit or scope of the overall concept of the invention as defined by the appended claims and their equivalents.

[0095] 1, 111 Two-wheeled inverted robot 2, 102, 112 Wheels 3, 103, 113 Wheel motors 4, 106, 114 Body 5, 116 Arm 6 Fork 7 Fork motor 11 Pitch angle detection unit 12 Wheel angle detection unit 20, 20A, 20B, 20C Control device 21 Position command value generation unit 22 Robot position control controller 23, 23A, 23C Robot pitch angle control controller 24 Pitch disturbance estimation unit 25 External force estimation unit 26, 26B, 26C Deflection behavior generation unit 27A, 27C Pitch angle command value generation unit 28B, 28C Speed ​​command value generation unit 29B, 29C Robot speed control controller 101 Inverted robot 104 Base unit 105 Connection unit 115 Movable unit 117 Gripping part

Claims

1. A control device for controlling the drive of a two-wheeled inverted robot comprising: a pair of wheels; a wheel motor that outputs torque to an axle to rotate the wheels; and a body attached to the pair of wheels and provided to be pivotable around the axle, the control device comprising: an external force estimation unit that calculates an estimated value of an external force applied to the two-wheeled inverted robot; and a deflection behavior generation unit that generates a command value for executing a deflection behavior that returns the body to an inverted balance angle in response to the estimated value of the external force such that the force pushing back is below a threshold.

2. The control device according to claim 1, wherein the deflection behavior generation unit generates the command value by compliance control that returns the body to the inverted equilibrium angle, using the equation of motion of a spring in which coefficients are set such that the force pushing back in response to the estimated value of the external force is below a threshold value.

3. The control device according to claim 1, wherein the command value is a position command value that indicates the position of the two-wheeled inverted robot.

4. The control device according to claim 1, wherein the command value is a speed command value that instructs the speed of the two-wheeled inverted robot.

5. The control device according to claim 1, comprising a pitch disturbance estimation unit that calculates pitch angular acceleration from a detected value of the pitch angle, which is the angle between the vertical axis of the body and the vertical direction; calculates wheel angular acceleration from a detected value of the wheel angle, which is the rotation angle of the wheel from a predetermined position; and calculates an estimated value of pitch disturbance, which is the force that rotates the body in the direction of the pitch angle, based on the torque command value input to the wheel motor, the pitch angular acceleration, and the wheel angular acceleration.

6. The control device according to claim 5, wherein the external force estimation unit calculates an estimated value of the external force applied to the two-wheeled inverted robot based on the estimated value of the pitch disturbance.

7. The control device according to claim 5, wherein the external force estimation unit calculates an estimated value of the external force applied to the two-wheeled inverted robot by subtracting the gravitational force applied to the body and the force applied to the wheels from the estimated value of the pitch disturbance.

8. A control method for controlling the drive of a two-wheeled inverted robot comprising: a pair of wheels; a wheel motor that outputs torque to an axle for rotating the wheels; and a body attached to the pair of wheels and provided to be pivotable around the axle, wherein a processor calculates an estimated value of an external force applied to the two-wheeled inverted robot, reads the estimated value of the external force from memory, and generates a command value for executing a deflection behavior that returns the body to an inverted balance angle in response to the estimated value of the external force such that the force pushing back is less than or equal to a threshold.

9. A control program for controlling the drive of a two-wheeled inverted robot comprising: a pair of wheels; a wheel motor that outputs torque to an axle to rotate the wheels; and a body attached to the pair of wheels and provided to be pivotable around the axle, wherein the control program causes a processor to calculate an estimated value of the external force applied to the two-wheeled inverted robot, and to generate a command value for executing a deflection behavior that returns the body to an inverted equilibrium angle such that the force pushing back in response to the estimated value of the external force is below a threshold.

10. A two-wheeled inverted robot comprising: a pair of wheels; a wheel motor that outputs torque to an axle to rotate the wheels; a body attached to the pair of wheels and provided to be pivotable around the axle; and a control device that controls the drive of the two-wheeled inverted robot, wherein the control device comprises: an external force estimation unit that calculates an estimated value of an external force applied to the two-wheeled inverted robot; and a deflection behavior generation unit that generates a command value for executing a deflection behavior that returns the body to an inverted balance angle in response to the estimated value of the external force such that the force pushing back is below a threshold.

11. A control device for controlling the drive of an inverted robot comprising: a wheel; a wheel motor that outputs torque to an axle to rotate the wheel; a connecting part located on the wheel; and a body attached to the connecting part and provided to be swingable around the connecting part, the control device comprising: an external force estimation unit that calculates an estimated value of an external force applied to the body; and a deflection behavior generation unit that generates a command value for executing a deflection behavior to return the body to an inverted equilibrium angle in response to the estimated value of the external force such that the force pushing back is below a threshold.

Images