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

Abstract

This control device controls driving of a two-wheel inverted pendulum robot comprising a pair of wheels, a wheel motor that outputs torque for rotating the wheels to an axle, and a body that is attached to the pair of wheels and that is provided so as to be swingable around the axle. The control device comprises a determination unit that determines whether or not the state of the two-wheel inverted pendulum robot satisfies a prescribed condition, and a drive control unit that performs, in accordance with the result of the determination performed by the determination unit, orientation stability control to stabilize an inverted-pendulum orientation of the body. Thus, provided is a control device with which it is possible for a two-wheel inverted pendulum robot to maintain an inverted-pendulum orientation even when subjected to external force.

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 two-wheeled inverted robot comprising a drive wheel, a body coupled to the drive wheel, and an arm configured to move relative to the body. In this robot, the body includes an inverted pendulum body and a counterbalance body, and a rear joint rotatably connecting the inverted pendulum body and the counterbalance body allows the robot's pose to be changed by shifting the robot's center of mass relative to the vertical gravity axis.

[0003] Special Publication No. 2022-524973

[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 a two-wheeled inverted robot is subjected to an external force, its body tilts due to the force. Therefore, the control system for a two-wheeled inverted robot requires control to maintain the robot's inverted posture when subjected to an external force.

[0006] 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 maintain an inverted posture even when subjected to external forces.

[0007] 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 to rotate the wheels, and a body attached to the pair of wheels and provided to be swingable around the axle, comprising a determination unit that determines whether the state of the two-wheeled inverted robot satisfies predetermined conditions, and a drive control unit that performs attitude stabilization control to stabilize the inverted posture of the body according to the determination result of the determination unit.

[0008] Furthermore, in one aspect of the present invention, the control device includes a drive control unit which, as attitude stabilization control, stops controlling the position or speed of the two-wheeled inverted robot and performs feedback control of the pitch angle, which is the angle between the vertical axis of the body and the vertical direction.

[0009] Furthermore, in one aspect of the present invention, the control device includes a drive control unit which, as part of the attitude stabilization control, sets the speed command value for controlling the speed of the two-wheeled inverted robot to zero and performs feedback control of the pitch angle, which is the angle between the body and the vertical direction.

[0010] Furthermore, in one aspect of the present invention, the control device is characterized in that the predetermined condition is that the two-wheeled inverted robot is subjected to an external force exceeding a threshold.

[0011] Furthermore, a control device according to one aspect of the present invention includes an external force estimation unit that calculates an estimated value of the external force applied to the two-wheeled inverted robot, and the predetermined condition is whether or not the estimated value of the external force is above a threshold.

[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 to rotate the wheels, and a body attached to the pair of wheels and provided to be swingable around the axle, wherein a processor determines whether the state of the two-wheeled inverted robot satisfies predetermined conditions, reads the determination result from memory, and performs attitude stabilization control to stabilize the inverted posture of the body according to the determination result.

[0016] Furthermore, a control program according to one aspect of the present invention is a control program for controlling 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 swingable around the axle, wherein the program causes a processor to determine whether the state of the two-wheeled inverted robot satisfies predetermined conditions, and to execute a process to perform attitude stabilization control to stabilize the inverted posture of the body according to the determination result.

[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 a determination unit that determines whether the state of the two-wheeled inverted robot satisfies predetermined conditions, and a drive control unit that performs attitude stabilization control to stabilize the inverted posture of the body according to the determination result of the determination unit.

[0018] 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 the two-wheeled inverted robot to maintain an inverted posture even when subjected to external forces.

[0019] 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 block diagram showing the control of a two-wheeled inverted robot including a control device according to Embodiment 4.

[0020] 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.

[0021] 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.

[0022] (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.

[0023] 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.

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

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

[0026] 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.

[0027] 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.

[0028] 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.

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

[0030] 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.

[0031] 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.

[0032] 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.

[0033] [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).

[0034] The control device 20 may be built into the two-wheeled inverted robot 1. Also, 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, a mobile phone network, WiFi (registered trademark, Wireless Fidelity), BLE (Bluetooth (registered trademark) Low Energy) or the like.

[0035] The control device 20 includes a position command value generating unit 21, a robot position control controller 22, a robot pitch angle control controller 23, a pitch disturbance estimating unit 24, an external force estimating unit 25, a determining unit 26, and a drive control unit 27.

[0036] The position command value generating unit 21 generates and outputs a position command value θ that indicates a position of the two-wheeled inverted robot 1 ｗ ｃｍｄ .

[0037] The robot position control controller 22 calculates and outputs a pitch angle command value θ for controlling the position of the body 4, on the basis of the position command value θ generated by the position command value generating unit 21 ｗ ｃｍｄ , and a wheel angle θ detected by a wheel angle detecting unit 12 ｗ . ｐ ｃｍｄ

[0038] The robot pitch angle control controller 23 calculates and outputs a torque command value τ that a wheel motor 3 outputs to a wheel 2 for maintaining the inverted posture of the body 4, on the basis of the pitch angle command value θ output from the robot position control controller 22 ｐ ｃｍｄ , and a pitch angle θ detected by a pitch angle detecting unit 11 ｐ . When the influence of a load held by a fork 6 is to be considered, for example, a pitch angle command value θ for maintaining the inverted posture with the load held is calculated, and the robot pitch angle control controller 23 also calculates the torque command value τ in consideration of the pitch angle command value θ ｗ ｒｅｆ . ｐ ｃｍｄ_ｌоａｄ , the robot pitch angle control controller 23, the pitch angle command value θ ｐ ｃｍｄ_ｌоａｄ , and the torque command value τ ｗｒｅｆ You may calculate this.

[0039] The pitch disturbance estimation unit 24 estimates the pitch disturbance τ, which is the force that rotates the body 4 in the pitch angle direction. ｐ ｄｉｓ (Hat) (Hereafter, the estimated value will be referred to as "hat" in the specification and as an umbrella-shaped symbol above the word in the drawings.) is calculated. Specifically, the pitch disturbance estimation unit 24 first calculates the pitch angle θ ｐ The pitch angular acceleration θ was calculated by performing a second derivative calculation with respect to time from the detected value. ｐ (Two-dot) (Hereafter, the derivative with respect to time will be referred to as "dot" in the specification and as a dot above the letter in the drawings. In the case of the second derivative with respect to time, it will be referred to as "two-dot" in the specification and as two dots 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 θ ｗ Based on (two dots), the estimated pitch disturbance τ ｐ ｄｉｓ The (hat) is calculated. Note that Figure 1 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.

[0040] 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 ｈｕｍａｎ ｄｉｓ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 the (hat). Estimated 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 estimated 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 in Figure 1, the wheel angle θ is located outside the external force estimation unit 25. ｗThe configuration shown includes a calculation circuit that performs a first-order derivative operation with respect to time from the detected value to calculate the wheel angular velocity, but this calculation may also be performed by the external force estimation unit 25.

[0041] The determination unit 26 determines whether the state of the two-wheeled inverted robot 1 satisfies predetermined conditions. The predetermined conditions are that the two-wheeled inverted robot 1 is subjected to an external force exceeding a threshold. Specifically, the determination unit 26 uses the estimated value τ of the external force estimated by the external force estimation unit 25 as the predetermined condition. ｈｕｍａｎ ｄｉｓ Determine whether the (hat) value is above the threshold.

[0042] The drive control unit 27 performs attitude stabilization control to stabilize the inverted posture of the body 4 according to the determination result of the determination unit 26. As part of the attitude stabilization control, the drive control unit 27 stops controlling the position of the two-wheeled inverted robot 1 and the pitch angle θ ｐ Feedback control is performed. Specifically, the drive control unit 27 switches the command value input to the robot pitch angle control controller 23 according to the determination result of the determination unit 26. In the example in Figure 4, the drive control unit 27 determines the external force estimate τ when the determination unit 26 ｈｕｍａｎ ｄｉｓ If it is determined that the (hat) value is above the threshold, the connection between contact C1 and contact C2 is disconnected from the state in which they were connected.

[0043] [Control Method for a Two-Wheeled Inverted Robot] Next, the control method for the two-wheeled inverted robot 1 executed by the control device 20 will be described. Figure 3 is a flowchart showing an overview of the process executed by the control device according to Embodiment 1. When an external force is applied to the two-wheeled inverted robot 1, as shown in Figure 3, the pitch disturbance estimation unit 24 estimates the pitch disturbance τ ｐ ｄｉｓ Calculate the hat (step S1).

[0044] 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).

[0045] Then, the determination unit 26 determines whether the two-wheeled inverted robot 1 has received an external force exceeding a threshold (step S3). Specifically, the determination unit 26 determines the estimated external force τ calculated by the external force estimation unit 25. ｈｕｍａｎ ｄｉｓ Determine whether the (hat) value is above the threshold.

[0046] If the determination unit 26 determines that the two-wheeled inverted robot 1 has received an external force exceeding a threshold (step S3: Yes), the drive control unit 27 stops controlling the position of the two-wheeled inverted robot 1 as attitude stabilization control and controls the pitch angle θ ｐ Feedback control is performed (step S4).

[0047] In the control device 20 that performs attitude stabilization control, the drive control unit 27 disconnects the connection between contact C1 and contact C2, and the control of the position of the two-wheeled inverted robot 1, which is input from the robot position control controller 22 to the robot pitch angle control controller 23, is stopped. Furthermore, in this control device 20, the pitch angle θ detected by the pitch angle detection unit 11 is stopped. ｐ The pitch angle θ is directly input to the robot pitch angle control controller 23. ｐ Feedback control is then performed. This limits the effective control range to region A1, stabilizing the inverted posture of the two-wheeled inverted robot 1. After that, the series of processes is completed.

[0048] In step S3, if the determination unit 26 determines that the body 4 is not subjected to an external force exceeding a threshold (step S3: No), the process returns to step S1, and the control device 20 continues the normal processing that occurs when no external force is applied to the two-wheeled inverted robot 1.

[0049] 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, the determination unit 26, and the drive control unit 27 execute the processes of steps S1 to S4. As a result, when an external force is applied to the two-wheeled inverted robot 1, the estimated value τ of the external force calculated by the pitch disturbance estimation unit 24 and the external force estimation unit 25 ｈｕｍａｎ ｄｉｓthe determining unit 26 determines whether (hat) is equal to or greater than a threshold value. The determining unit 26 determines that the estimated external force τ ｈｕｍａｎ ｄｉｓ (hat) is equal to or greater than the threshold value, the drive control unit 27 performs, as posture stabilization control, pitch angle θ ｐ feedback control is performed. Accordingly, the two-wheeled inverted robot 1 can maintain an inverted posture even when subjected to an external force.

[0050] Further, when the external force applied to the two-wheeled inverted robot 1 disappears, the determining unit 26 determines that the estimated external force τ ｈｕｍａｎ ｄｉｓ (hat) is not equal to or greater than the threshold value. As a result, the control automatically returns to the original control for driving the two-wheeled inverted robot 1 based on the position command value θ ｗ ｃｍｄ generated by the position command value generating unit 21. Further, the drive control unit 27 is configured such that, when the estimated external force τ ｈｕｍａｎ ｄｉｓ (hat) is not equal to or greater than the threshold value and a deviation of the body 4 from an inverted equilibrium angle is equal to or less than a threshold value, the posture stabilization control may be ended. Here, the inverted equilibrium angle is a pitch angle θp at which the posture of the body 4 is stabilized when the wheels 2 and the body 4 are regarded as an inverted pendulum, and is a pitch angle θp at which all forces acting in the pitch angle θp direction are balanced when the wheel motor 3 outputs no torque. Further, the drive control unit 27 is configured such that, when the estimated external force τ ｈｕｍａｎ ｄｉｓ (hat) is not equal to or greater than the threshold value and a predetermined time has elapsed after receiving the external force, the posture stabilization control may be ended. Further, the drive control unit 27 may lengthen the time until ending the posture stabilization control as the estimated external force τ ｈｕｍａｎ ｄｉｓ (hat) is larger.

[0051] According to the first embodiment described above, when the determining unit 26 determines that the two-wheeled inverted robot 1 has received an external force equal to or greater than the threshold value, the drive control unit 27 executes posture stabilization control, so that the two-wheeled inverted robot 1 can maintain an inverted posture even when subjected to an external force.

[0052] In the first embodiment, the determining unit 26 uses the estimated external force τ as a predetermined condition ｈｕｍａｎ ｄｉｓAn example has been described in which the drive control unit 27 performs attitude stabilization control according to the determination result after determining whether the (hat) value is above a threshold, but the predetermined conditions used for switching the control are not limited to this.

[0053] If the two-wheeled inverted robot 1 is equipped with a pressure-sensitive or microswitch-type contact sensor such as a bumper, the predetermined condition may be whether or not the input value from the contact sensor is above a threshold. When the determination unit 26 determines that the input value from the contact sensor is above a threshold, the drive control unit 27 considers that the two-wheeled inverted robot 1 has been subjected to an external force and performs attitude stabilization control. However, when detecting an external force using a contact sensor, it is not possible to detect an external force applied to a part other than the sensor. Also, when using a contact sensor, the force is applied directly to the contact sensor, making it prone to failure. In contrast, with the configuration of Embodiment 1, it is possible to detect an external force regardless of which part of the two-wheeled inverted robot 1 is subjected to the external force, and it is also less prone to failure.

[0054] If the two-wheeled inverted robot 1 can detect the distance between the two-wheeled inverted robot 1 and surrounding objects using a distance measuring sensor or camera, the predetermined condition may be whether or not the distance to the surrounding objects is below a threshold. When the determination unit 26 determines that the distance to the surrounding objects is below a threshold, the drive control unit 27 assumes that the two-wheeled inverted robot 1 has been subjected to an external force and performs attitude stabilization control.

[0055] The specified conditions are: pitch angular acceleration θ ｐ (Two dots) or pitch angular velocity θ ｐ The condition may be whether (dot) is above a threshold or deviates from the command value by a certain value or more. The determination unit 26 determines the pitch angular acceleration θ ｐ (Two dots) or pitch angular velocity θ ｐ If the (dot) is determined to be above a threshold or deviates from the command value by a certain amount, the drive control unit 27 assumes that the two-wheeled inverted robot 1 has been subjected to an external force and performs attitude stabilization control.

[0056] The specified conditions are: pitch angle θ ｐ The conditions may be whether the value is above a threshold or deviates from the command value by a certain amount. The determination unit 26 determines the pitch angle θ ｐIf the drive control unit 27 determines that the value is above a threshold or deviates from the command value by a certain amount, it assumes that the two-wheeled inverted robot 1 has been subjected to an external force and performs attitude stabilization control.

[0057] The predetermined conditions may be whether the acceleration or velocity of the two-wheeled inverted robot 1 as it translates is above a threshold, or whether it deviates from the command value by a certain value or more. When the determination unit 26 determines that the acceleration or velocity of the two-wheeled inverted robot 1 as it translates is above a threshold, or whether it deviates from the command value by a certain value or more, the drive control unit 27 assumes that the two-wheeled inverted robot 1 has been subjected to an external force and performs attitude stabilization control.

[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 θ ｗ While examples of detection have been described, the method is not limited to these. For example, the three-dimensional acceleration and angular velocity may be directly measured using an IMU (Internal Measurement Unit). Alternatively, the acceleration between the upper and lower parts of the body 4 can be measured using two acceleration sensors, and the pitch angular acceleration θ can be directly measured. ｐ (Two dots) may be measured. Alternatively, the wheel angular acceleration θ can be indirectly measured by measuring the position of wheel 2 with a distance sensor or camera. ｗ (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] 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.

[0060] 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.

[0061] 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.

[0062] (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 includes a speed command value generation unit 21A, a robot speed control controller 22A, and a drive control unit 27A. Other configurations may be the same as in Embodiment 1, so their descriptions will be omitted as appropriate.

[0063] The speed command value generation unit 21A generates a speed command value θ that instructs the speed of the two-wheeled inverted robot 1. ｗ ｃｍｄ It generates and outputs a (dot). In the control device 20A, the speed command value generation unit 21A always sets the speed command value θ as a command value that sets the speed to zero. ｗ ｃｍｄ (Dot) = Generates and outputs 0.

[0064] The robot speed control controller 22A receives the speed command value θ generated by the speed command value generation unit 21A. ｗ ｃｍｄ (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.

[0065] The drive control unit 27A controls the speed of the two-wheeled inverted robot 1 using a speed command value θ as part of attitude stabilization control. ｗ ｃｍｄ Set (dot) to zero, and pitch angle θ ｐ Feedback control is performed. Specifically, the drive control unit 27A determines the external force τ when the determination unit 26 ｈｕｍａｎ ｄｉｓ If it is determined that the (hat) value is above the threshold, the connection is switched from a state where contact CA1 and contact CA3 are connected to a state where contact CA2 and contact CA3 are connected.

[0066] In other words, in Embodiment 2, under normal conditions when no external force is applied, the position of the two-wheeled inverted robot 1 is controlled by the position command value generation unit 21 and the robot position control controller 22. On the other hand, in attitude stabilization control when an external force is applied, the speed of the two-wheeled inverted robot 1 is controlled by the speed command value generation unit 21A and the robot speed control controller 22A. The speed command value generation unit 21A always controls the speed command value θ ｗ ｃｍｄ (Dot) = 0 is generated and output. As a result, the pitch angle θ ｐ Feedback control is performed, which stabilizes the inverted posture of the two-wheeled inverted robot 1.

[0067] According to the embodiment 2 described above, when the determination unit 26 determines that the two-wheeled inverted robot 1 has received an external force exceeding a threshold, the drive control unit 27A performs attitude stabilization control, so that the two-wheeled inverted robot 1 can maintain an inverted posture even when subjected to an external force.

[0068] According to Embodiment 2, when an external force is applied, the velocity command value θ ｗ ｃｍｄSince (dot) = 0 is input to the robot pitch angle control controller 23, the response time until the speed of the two-wheeled inverted robot 1 becomes zero is faster than in Embodiment 1, where position control is stopped.

[0069] (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 comprises a speed command value generation unit 21B, a robot speed control controller 22B, and a drive control unit 27B. Other configurations may be the same as in Embodiment 1, so their descriptions will be omitted as appropriate.

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

[0071] The robot speed control controller 22B may have the same configuration as the robot speed control controller 22A in Embodiment 2, so its description will be omitted.

[0072] The drive control unit 27B, as part of attitude stabilization control, stops controlling the speed of the two-wheeled inverted robot 1 and the pitch angle θ ｐ Feedback control is performed. Specifically, the drive control unit 27B determines the external force τ when the determination unit 26 ｈｕｍａｎ ｄｉｓ If it is determined that the (hat) value is above the threshold, the connection between contact CB1 and contact CB2 is disconnected from the state in which they were connected.

[0073] In the control device 20B that performs attitude stabilization control, the drive control unit 27B disconnects the connection between contact CB1 and contact CB2, and the control of the speed of the two-wheeled inverted robot 1, which is input from the robot speed control controller 22B to the robot pitch angle control controller 23, is stopped. Furthermore, in the control device 20B, the pitch angle θ detected by the pitch angle detection unit 11 is stopped. ｐ The pitch angle θ is directly input to the robot pitch angle control controller 23. ｐ Feedback control is then performed. This limits the effective control range to region A2, thereby stabilizing the inverted posture of the two-wheeled inverted robot 1.

[0074] According to the embodiment 3 described above, when the determination unit 26 determines that the two-wheeled inverted robot 1 has received an external force exceeding a threshold, the drive control unit 27B performs attitude stabilization control, so that the two-wheeled inverted robot 1 can maintain an inverted posture even when subjected to an external force.

[0075] (Embodiment 4) Figure 6 is a block diagram showing the control of a two-wheeled inverted robot including a control device according to Embodiment 4. As shown in Figure 6, the control device 20C comprises a speed command value generation unit 21C, a robot speed control controller 22C, and a drive control unit 27C. Other configurations may be the same as in Embodiment 1, so their descriptions will be omitted as appropriate.

[0076] The speed command value generation unit 21C and the robot speed control controller 22C may have the same configuration as the speed command value generation unit 21B and the robot speed control controller 22B in Embodiment 3, so their description will be omitted.

[0077] The drive control unit 27C controls the speed of the two-wheeled inverted robot 1 using a speed command value θ as part of attitude stabilization control. ｗ ｃｍｄ Set (dot) to zero, and pitch angle θ ｐ Feedback control is performed. Specifically, the drive control unit 27C determines the external force τ when the determination unit 26 ｈｕｍａｎ ｄｉｓ If it is determined that (hat) is above the threshold, the speed command value generation unit 21C will issue a speed command value θ as a command value to set the speed to zero. ｗ ｃｍｄ (Dot) = 0 is generated and output.

[0078] According to Embodiment 4 described above, when the determination unit 26 determines that the two-wheeled inverted robot 1 has received an external force exceeding a threshold, the drive control unit 27C performs attitude stabilization control, so that the two-wheeled inverted robot 1 can maintain an inverted posture even when subjected to an external force.

[0079] According to Embodiment 4, similar to Embodiment 2, when an external force is applied, the velocity command value θ ｗ ｃｍｄSince (dot) = 0 is input to the robot pitch angle control controller 23, the response time until the speed of the two-wheeled inverted robot 1 becomes zero is faster than in Embodiment 1, where position control is stopped.

[0080] 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.

[0081] 1 Two-wheeled inverted robot 2 Wheels 3 Wheel motors 4 Body 5 Arms 6 Forks 7 Fork motors 11 Pitch angle detection unit 12 Wheel angle detection unit 20, 20A, 20B, 20C, 20D, 20E, 20F, 20G Control device 21 Position command value generation unit 21B, 21C, 21G Speed ​​command value generation unit 22B, 22C, 22G Robot speed control controller 22 Robot position control controller 23 Robot pitch angle control controller 24 Pitch disturbance estimation unit 25 External force estimation unit 26 Judgment unit 27, 27B, 27C, 27G Drive control unit 28E Deflection behavior generation unit

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 swingable around the axle, the control device comprising: a determination unit that determines whether the state of the two-wheeled inverted robot satisfies predetermined conditions; and a drive control unit that performs posture stabilization control to stabilize the inverted posture of the body according to the determination result of the determination unit.

2. The control device according to claim 1, wherein the drive control unit stops controlling the position or speed of the two-wheeled inverted robot and performs feedback control of the pitch angle, which is the angle between the vertical axis of the body and the vertical direction, as the attitude stabilization control.

3. The control device according to claim 1, wherein the drive control unit sets the speed command value for controlling the speed of the two-wheeled inverted robot to zero as attitude stabilization control, and performs feedback control of the pitch angle, which is the angle between the body and the vertical direction.

4. The control device according to claim 1, wherein the predetermined condition is that the two-wheeled inverted robot receives an external force exceeding a threshold.

5. The control device according to claim 1, comprising an external force estimation unit that calculates an estimated value of the external force applied to the two-wheeled inverted robot, wherein the predetermined condition is whether or not the estimated value of the external force is equal to or greater than a threshold.

6. The control device according to claim 5, 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.

7. The control device according to claim 6, 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.

8. The control device according to claim 6, 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.

9. 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 to rotate the wheels; and a body attached to the pair of wheels and provided to be swingable around the axle, wherein a processor determines whether the state of the two-wheeled inverted robot satisfies predetermined conditions, reads the determination result from memory, and performs attitude stabilization control to stabilize the inverted posture of the body according to the determination result.

10. A control program for controlling 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 determine whether the state of the two-wheeled inverted robot satisfies predetermined conditions, and to execute a process to perform posture stabilization control to stabilize the inverted posture of the body according to the determination result.

11. 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: a determination unit that determines whether the state of the two-wheeled inverted robot satisfies predetermined conditions; and a drive control unit that performs attitude stabilization control to stabilize the inverted posture of the body according to the determination result of the determination unit.

Images