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

Abstract

This control device controls the driving of a two-wheel inverted robot comprising a pair of wheels, a wheel motor that outputs torque for rotating the wheels to an axle, a body which is attached to the pair of wheels and can swing around the axle, and a placement part on which a load is placed. The control device comprises a load disturbance compensation unit that calculates and outputs a compensated pitch angle command value obtained by compensating gravity-dependent variation on the basis of a dynamic model, the compensated pitch angle command value being calculated on the basis of: a pitch angle command value for controlling the behavior of pitch angle, which is the angle between the vertical axis of the body and the vertical direction, so as to maintain an inverted orientation of the two-wheel inverted robot; information about the load weight applied to the placement part by the load; and the translational velocity of the wheels obtained by first-order differentiation of wheel position, calculated from a detection value of wheel angle, with respect to time. As a result, provided is a control device that does not require tuning for control for stabilizing orientation when placing a load.

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 an inverted wheel type cargo handling mobile vehicle. In this cargo handling mobile vehicle, the load is supported on the upper surface of the forks. When the center of gravity changes due to the support of the load, the attitude control unit uses a drive unit (motor) to swing the vehicle body and move the center of gravity of the cargo handling mobile vehicle in the front-rear direction in order to invert the cargo handling mobile vehicle.

[0003] Repulsive compliance control (RCC) is a known method for controlling the attitude of a vehicle when it is loaded with cargo. In repulsive compliance control, a pitch angle command value, which represents the tilt of the vehicle body, is calculated using a virtual model based on the gravitational moment value measured by the forks when the cargo is loaded, and the attitude of the vehicle body is controlled by the calculated pitch angle command value.

[0004] Japanese Patent Publication No. 2024-88228

[0005] However, repulsive compliance control has the challenge that the parameters of the virtual model used to generate the pitch angle command value are not uniquely determined, and tuning and adjusting the parameters is time-consuming in order to obtain the desired response.

[0006] The present invention has been made in view of the above, and aims to provide a control device, control method, control program, and two-wheeled inverted robot for a two-wheeled inverted robot that do not require tuning for control to stabilize the posture when a load is placed on it.

[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 the axle for rotating the wheels, a body attached to the pair of wheels and provided to be pivotable around the axle, and a loading section for loading a load, and includes a load disturbance compensation unit that calculates and outputs a compensated pitch angle command value, which compensates for gravity-dependent fluctuations based on a dynamic model, based on a pitch angle command value for controlling the behavior of the pitch angle, which is the angle between the vertical axis of the body and the vertical direction, in order to maintain the inverted posture of the two-wheeled inverted robot, load load information applied to the loading section by the load, and the translational velocity of the wheel calculated by first derivative calculation with respect to time from the wheel position calculated from the detected wheel angle.

[0008] Furthermore, in one aspect of the present invention, the control device includes a model-based compensator in the cargo disturbance compensation unit that calculates and outputs a pitch angle compensation value that compensates the pitch angle command value so that the gravity term applied in the pitch angle direction by the cargo load information becomes zero.

[0009] Furthermore, in one aspect of the present invention, the control device includes a model error suppression compensator in which the cargo disturbance compensation unit calculates and outputs a pitch angle compensation value that compensates the pitch angle command value for fluctuations dependent on the weight of the cargo based on the error between the actual response and the reference model.

[0010] Furthermore, a control device according to one aspect of the present invention includes a virtual point translation velocity calculation unit that calculates the translational velocity of the wheel by performing a first derivative calculation with respect to time from the wheel position, calculates the pitch angular velocity by performing a first derivative calculation with respect to time from the detected pitch angle, and calculates the translational velocity of a virtual point located on the center of gravity axis of the two-wheeled inverted robot based on the translational velocity of the wheel and the pitch angular velocity.

[0011] Furthermore, in one aspect of the present invention, the control device includes a cargo disturbance compensation unit which calculates and outputs the compensated pitch angle compensation value based on the translational velocity of the virtual point.

[0012] Furthermore, in one aspect of the present invention, the control device is configured such that the distance from the axle to the virtual point eliminates unstable zeros.

[0013] Furthermore, 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, which comprises 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 loading section for loading a load, and comprises a virtual point translation velocity calculation unit that calculates the translational velocity of the wheels by performing a first derivative calculation with respect to time from the wheel position calculated from a detected value of the wheel angle, calculates the pitch angular velocity by performing a first derivative calculation with respect to time from a detected value of the pitch angle, which is the angle between the vertical axis of the body and the vertical direction, and calculates the translational velocity of a virtual point located on the center of gravity axis of the two-wheeled inverted robot based on the translational velocity of the wheels and the pitch angular velocity, and a robot speed control controller that calculates and outputs a speed command value that instructs the translational velocity of the two-wheeled inverted robot, and a pitch angle command value for controlling the behavior of the pitch angle to maintain the inverted posture of the two-wheeled inverted robot based on the translational velocity of the virtual point.

[0014] 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, a body attached to the pair of wheels and provided to be pivotable around the axle, and a loading section for loading a load, wherein a processor calculates and outputs a pitch angle compensation value that compensates for gravity-dependent fluctuations based on a dynamic model, based on a pitch angle command value for controlling the behavior of the pitch angle, which is the angle between the vertical axis of the body and the vertical direction, to maintain the inverted posture of the two-wheeled inverted robot, load information applied to the loading section by the load, and the translational velocity of the wheels calculated by first-order derivative calculation with respect to time from the wheel position calculated from the detected wheel angle value.

[0015] 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 to rotate the wheels, a body attached to the pair of wheels and provided to be pivotable around the axle, and a loading section for loading a load, wherein the program causes the processor to perform a process of calculating and outputting a pitch angle compensation value that compensates for gravity-dependent fluctuations based on a dynamic model, based on a pitch angle command value for controlling the behavior of the pitch angle, which is the angle between the vertical axis of the body and the vertical direction, in order to maintain the inverted posture of the two-wheeled inverted robot, load information applied to the loading section by the load, and the translational velocity of the wheels calculated by performing a first derivative with respect to time from the wheel position calculated from the detected wheel angle value.

[0016] 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 pivotable around the axle, a loading section for loading a load, and a control device for controlling the drive of the two-wheeled inverted robot. The control device includes a load disturbance compensation unit that calculates and outputs a pitch angle compensation value, which compensates for gravity-dependent fluctuations based on a dynamic model, using a pitch angle command value for controlling the behavior of the pitch angle, which is the angle between the vertical axis of the body and the vertical direction, to maintain the inverted posture of the two-wheeled inverted robot, load load information applied to the loading section by the load, and the translational velocity of the wheels calculated by first-order derivative calculation with respect to time from the wheel position calculated from the detected wheel angle.

[0017] According to the present invention, a control device, control method, control program, and two-wheeled inverted robot can be realized that do not require tuning for control to stabilize the posture when a load is placed on them.

[0018] 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 the control device according to Embodiment 1. Figure 3 is a block diagram showing the processing in the load disturbance compensation unit and the virtual point translation speed calculation unit. Figure 4 is a block diagram showing the control of a two-wheeled inverted robot including the control device according to Embodiment 1. Figure 5 is a block diagram showing the control of a two-wheeled inverted robot including the control device according to Embodiment 1.

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

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

[0021] (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 wheels 2, wheel motors 3, a body 4, an arm 5 as a loading section for placing loads, and an arm motor 6.

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

[0023] The pair of wheels 2 are provided at both ends of the axle AX1.

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

[0025] Body 4 is attached to the axle AX1 of wheel 2 and is provided to be able to pivot around axle AX1. In the following description, the longitudinal direction of body 4 perpendicular to axle AX1 will be referred to as the vertical axis L of body 4, and the direction perpendicular to axle AX1 and the vertical axis L of body 4 will be referred to as the horizontal axis S of body 4.

[0026] The arm 5 is attached to the body 4 and has a load-bearing surface 5a. Under the control of the arm motor 6, the direction in which the arm 5 extends relative to the body 4 is changed, thereby changing the orientation of the load-bearing surface 5a.

[0027] The arm motor 6 outputs a torque that changes the orientation of the mounting surface 5a of the arm 5 by changing the direction in which the arm 5 extends relative to the body 4. In this embodiment, the rotation axis of the arm motor 6 is located on the vertical axis L.

[0028] Furthermore, the pitch angle θ of the body 4, described later, passes through the axle AX1. ｐ The straight line on the body 4 that serves as the reference point is defined as coordinate axis AX2. In this embodiment, when no load is placed on the arm 5, the center of gravity G is located on the vertical axis L of the body 4, as shown in Figure 1, and the vertical axis L of the body 4 coincides with coordinate axis AX2. When a load is placed on the arm 5, the center of gravity G shifts from the vertical axis L of the body 4 due to the load of the load.

[0029] Furthermore, a virtual point Vp used for controlling the two-wheeled inverted robot 1 is set on the coordinate axis AX2. In the control of the two-wheeled inverted robot 1, there are unstable zeros in the transfer function from the command value of the pitch angle to the translational acceleration. The virtual point Vp is an arbitrary point located on the coordinate axis AX2, but the distance l from the axle to the virtual point Vp is... ｖｐ It is preferable that the settings are configured to eliminate unstable zeros.

[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, a wheel angle detection unit 12, and an arm angle detection unit 13.

[0031] The pitch angle detection unit 11 is, for example, a gyro sensor attached to the body 4 of a two-wheeled inverted robot 1, and the pitch angle θ ｐ (See Figure 1) is detected. Pitch angle θ ｐ This is the angle in the plane that includes the direction of travel by the wheels 2 and the vertical direction V, and is the angle between the vertical axis L of the body 4 and the vertical direction V.

[0032] The wheel angle detection unit 12 is, for example, an encoder attached to the wheel motor 3, and the wheel angle θ is the rotation angle from a predetermined position of the wheel motor 3. ｗ It detects.

[0033] The arm angle detection unit 13 is, for example, an encoder attached to the arm motor 6, and the arm angle θ is the rotation angle of the arm motor 6 from a predetermined position. ａ (See Figure 1) is detected. Arm angle θ ａ This is an angle in a plane that includes the direction of travel by the wheels 2 and the vertical direction V, and is the angle between the horizontal axis S of the body 4 and the mounting surface 5a of the arm 5.

[0034] [Configuration of the control device] Next, the configuration of the control device 20 will be described. The control device 20 shown in Figure 2 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 speed command value generation unit 21, a robot speed controller 22, a robot pitch angle controller 23, an arm angle command value generation unit 24, a robot arm angle controller 25, a load disturbance compensation unit 26, and a virtual point translational speed calculation unit 27.

[0037] The speed command value generation unit 21 generates and outputs a speed command value v that indicates the speed of the virtual point Vp of the two-wheeled inverted robot 1. ｖｐ ｃｍｄ

[0038] Based on the speed command value v generated by the speed command value generation unit 21 and the translational speed v of the virtual point Vp output by the virtual point translational speed calculation unit 27, the robot speed controller 22 calculates and outputs a pitch angle command value θ for controlling the behavior of the pitch angle θ so as to maintain the inverted posture of the two-wheeled inverted robot 1. ｖｐ ｃｍｄ ｖｐ ｒｅｓ ｐ ｐ ｃｍｄ

[0039] Based on the pitch angle command value θ output by the robot speed controller 22, the pitch angle command value θ compensated by the load disturbance compensation unit 26, and the detected value θ of the pitch angle detected by the pitch angle detection unit 11, the robot pitch angle controller 23 calculates and outputs a torque command value τ that the wheel motor 3 outputs to the wheel 2 in order to maintain the inverted posture of the body 4. ｐ ｃｍｄ ｐ ｒｅｆ ｐ ｒｅｓ ｗ ｒｅｆ

[0040] The arm angle command value generation unit 24 generates and outputs an arm angle command value θ that indicates the arm angle θ of the two-wheeled inverted robot 1. ａ ａ ｃｍｄ

[0041] Based on the arm angle command value θ output by the arm angle command value generation unit 24, the robot arm angle controller 25... ａ ｃｍｄ ​​​​​​​​​​​, and the detected value θ of the arm angle detected by the arm angle detection unit 13 ａ ｒｅｓ Based on this, the arm angle θ of the two-wheeled inverted robot 1 ａ To control this, the torque command value τ output by the arm motor 6 ａ ｒｅｆ Calculate and output the result.

[0042] The cargo disturbance compensation unit 26 controls the pitch angle command value θ. ｐ ｃｍｄ The estimated value of the disturbance torque τ applied to arm 5 by the load. ａ ｌｏａｄ (Hat) (Load information) (Hereinafter, the estimated value will be referred to as "Hat" in the specification and an umbrella-shaped symbol will be placed above the letter in the drawings.), and the translational velocity v of the virtual point Vp ｖｐ ｒｅｓ Based on this, the compensated pitch angle command value θ compensates for gravity-dependent variations based on a dynamics model. ｐ ｒｅｆ The unit calculates and outputs the following. The cargo disturbance compensation unit 26 calculates the pitch angle command value θ based on the input information on the load from the cargo and the measurement values ​​from the sensor. ｐ ｒｅｆ The following can be calculated and output: In other words, the load information may be the input value of the load due to the load, or the measurement value of the sensor. The dynamics model will be described later. The load information may be the estimated value of the disturbance torque τ. ａ ｌｏａｄ Instead of (HAT), information on the weight of the cargo obtained in advance, values ​​measured by sensors, or their torque conversion values ​​may be used.

[0043] Figure 3 is a block diagram showing the processing in the cargo disturbance compensation unit and the virtual point translation velocity calculation unit. As shown in Figure 3, the cargo disturbance compensation unit 26 includes a model-based compensator (MBC) 261 and a model error suppression compensator (MEC) 262.

[0044] The model-based compensator 261 estimates the disturbance torque τ ａ ｌｏａｄEstimated value g of the horizontal component due to the weight of the load calculated from (Hat) ２ｈ The pitch angle command value θ is set such that the gravitational term acting in the pitch angle direction (horizontal direction) becomes zero due to the (hat) symbol. ｐ ｃｍｄ Pitch angle compensation value θ to compensate for ｐ ｍｂｃ Calculate and output the result.

[0045] Next, the model error suppression compensator 262 receives the pitch angle command value θ. ｐ ｃｍｄ The pitch angle compensation value θ ｐ ｍｂｃ The compensated pitch angle command value θ ｐ ｒｅｆ’ The following is entered.

[0046] The model error suppression compensator 262 compensates for vertical fluctuations dependent on the weight of the load by reducing the pitch angle command value θ by the error between the actual response and the reference model. ｐ ｒｅｆ’ Pitch angle compensation value θ to compensate for ｐ ｍｅｃ The following is calculated and output. Alternatively, instead of the model error suppression compensator 262, a disturbance observer (DOB) may be used to calculate the pitch angle compensation value.

[0047] The specific processing of the cargo disturbance compensation unit 26 will be described later. Subsequently, the cargo disturbance compensation unit 26 processes the pitch angle command value θ. ｐ ｒｅｆ’ The pitch angle compensation value θ ｐ ｍｅｃ The compensated pitch angle command value θ ｐ ｒｅｆ Outputs.

[0048] The virtual point translation speed calculation unit 27 uses the detected wheel angle θ ｗ ｒｅｓ The wheel position calculated from x ｒｅｓ The first derivative of wheel 2 with respect to time is obtained from the translational velocity v of wheel 2. ｒｅｓ The pitch angle is calculated, and the detected value θ is used. ｐ ｒｅｓ The pitch angular velocity θ obtained by first-order differentiation with respect to time. ｐ ｒｅｓ(Dot) is calculated, and the translational speed v of wheel 2 is calculated. ｒｅｓ and pitch angular velocity and θ ｐ ｒｅｓ Using (dots), the translational velocity v of an arbitrarily set virtual point Vp is defined. ｖｐ ｒｅｓ Calculate.

[0049] [Dynamic Model] Next, the operation of the two-wheeled inverted robot 1 will be explained in more detail using the dynamic model of the two-wheeled inverted robot 1. The two-wheeled inverted robot 1 can be represented by the following equation (1) using the dynamic model. In the following equation and explanation, the direction in which the two-wheeled inverted robot 1 moves forward and backward (left and right direction in Figure 1) is the X-axis, the direction along the axle AX1 (direction perpendicular to the plane of the paper in Figure 1) is the Y-axis, and the height direction of the two-wheeled inverted robot 1 (up and down direction in Figure 1) is the Z-axis. In equation (1), M, H, and G represent the inertia matrix, centrifugal force and Coriolis force terms, and gravity term, respectively, and Eτ represents the thrust force, body rotation torque, and arm rotation torque. In the specification, the first derivative with respect to time is indicated by a "dot," and in the drawings, it is indicated by a dot above the letter. Similarly, the second derivative with respect to time is indicated by two dots in the specification, and in the drawings, the number of dots is two.

[0050] Furthermore, M, H, G, E, and τ in equation (1) can be expressed by the following equations (2) to (6). n ｗ The torque gear ratio of the wheel motor 3 is n. ａ r is the torque gear ratio of the arm motor 6. ｗ The angle of wheel 2 is τ ｗ The torque of the wheel motor 3, τ ａ This is the torque of the arm motor 6.

[0051] m in equation (2) １１ ~m ３３ This can be expressed by the following equations (7) to (15). m ｂ is the weight of the body 4, m ｗ is the weight of the wheel 2, m ａ is the weight of the arm 5. I ｗｙ is the inertia of the wheel 2 about the y-axis, I ｂｙ is the inertia of the body 4 about the y-axis, I ａｙ is the inertia of the arm 5 about the y-axis, I ｗｒｙ is the inertia of the wheel motor 3 about the y-axis, I ａｒｙ is the inertia of the arm motor 6 about the y-axis. l ｂ is the distance from the axle AX1 to the center of gravity of the body 4, l ｈ is the distance from the axle AX1 to the rotation axis of the arm motor 6, l ａ is the distance from the rotation axis of the arm motor 6 to the center of gravity of the arm 5.

[0052] The h in Equation (3) １ ~h ３ can be expressed by the following Equations (16) to (18).

[0053] The g in Equation (4) ２ , g ３ can be expressed by the following Equations (19) and (20).

[0054] Here, in the state where the two-wheeled inverted robot 1 is inverted and stationary, g ２ becomes zero. When a load is applied to the two-wheeled inverted robot 1 in the inverted stationary state, and the center of gravity of the body 4 rotates by θ Δｐ around the axle AX1, and the center of gravity of the arm 5 rotates by θ Δａ around the rotation axis of the arm motor 6, respectively, then g ２ in Equation (19) can be modified as in the following Equation (21).

[0055] And, assuming that the arm 5 is controlled to be horizontal with the ground, θ ｐ +θ ａUsing the fact that when the angle θ is small, sinθ = θ and cosθ = 1, equation (21) becomes equation (22).

[0056] This equation (22) is θ ｐ By separating the terms that depend on and the other terms, we can divide them into the following equations (23) to (25).

[0057] g ２ｖ This is the vertical component due to the weight of the load, g ２ｈ m is the horizontal component due to the weight of the load. ｌ l ｌ g can be estimated by the reaction torque estimation observer (RTOB) of the robot arm angle control controller 25. The estimated value τ of the disturbance torque estimated by the reaction torque estimation observer is ａ ｌｏａｄ Using (Hat), estimate the horizontal component g due to the weight of the load. ２ｈ The (hat) can be expressed by the following equation (26).

[0058] Estimated value of disturbance torque τ ａ ｌｏａｄ (Hat) is estimated by the load applied to arm 5 and can be expressed by the following equation (27). ω in equation (27) ｒ m is the cutoff frequency of the low-pass filter (LPF) of the reaction force torque estimation observer. ３３ , g ３ , τ ａ ｆｒｉｃ These terms represent inertia, gravity, and friction, respectively, and can be determined in advance. In the following explanation, the subscript n indicates a nominal value.

[0059] [Processing of the Cargo Disturbance Compensation Unit] Next, the processing of the cargo disturbance compensation unit 26 will be explained in detail. As shown in Figure 2, the cargo disturbance compensation unit 26 receives an estimated value of the disturbance torque τ ａ ｌｏａｄ (Hat) and pitch angle command value θｐ ｃｍｄ The following is entered.

[0060] Estimated value of disturbance torque τ ａ ｌｏａｄ Using (Hat), the estimated value of the horizontal component due to the load weight g is obtained by equation (26). ２ｈ (Hat) can be calculated. As shown in Figure 3, the cargo disturbance compensation unit 26 has an estimated value g of the horizontal component due to the weight of the cargo. ２ｈ (Hat) and pitch angle command value θ ｐ ｃｍｄ The following is entered.

[0061] First, let's estimate the horizontal component g due to the weight of the load. ２ｈ The pitch angle required to compensate for the (hat) can be expressed by the following equation (28).

[0062] Furthermore, a low-pass filter compensates the pitch angle value θ. ｐ ｍｂｃ This can be expressed by the following equation (29). ω in equation (29) ｍｂｃ This is the cutoff frequency of the low-pass filter (LPF) of the model-based compensator 261, and K ｍｂｃ This is the gain of the model-based compensator 261.

[0063] The model-based compensator 261 does not compensate for the vertical effects of the load. This effect is compensated for by the model error suppression compensator 262. The model error suppression compensator 262 compensates for model errors by feeding back the error between the actual response and the model.

[0064] Reference model 2621 includes an estimated value g for the horizontal component due to the load. ２ｈ (Hat) and pitch angle command value θ ｐ ｒｅｆ’ The following is input. Then, the reference model 2621 is the translational acceleration v of the virtual point Vp according to the model represented by the following equation (30). ｖｐ Ｍ The (dot) is calculated, and the translational velocity v of the virtual point Vp according to the model is obtained by the first integral with respect to time. ｖｐＭ Outputs.

[0065] The translational velocity v of the virtual point Vp according to this model ｖｐ Ｍ and the actual translational velocity v of the virtual point Vp ｖｐ ｒｅｓ The error is calculated, and the pitch angle compensation value θ is calculated by the compensator 2622. ｐ ｍｅｃ This can be expressed by the following equation (31). ω in equation (31) ｍｅｃ This is the cutoff frequency of the low-pass filter (LPF) of the model error suppression compensator 262, and K ｍｅｃ This is the gain of the model error suppression compensator 262.

[0066] Subsequently, the cargo disturbance compensation unit 26 uses the pitch angle compensation value θ output by the model-based compensator 261. ｐ ｒｅｆ’ The pitch angle compensation value θ output by the model error suppression compensator 262 was... ｐ ｍｅｃ Adding these together, we get the pitch angle compensation value θ ｐ ｒｅｆ Outputs.

[0067] [Processing of the virtual point translation velocity calculation unit] Next, the processing of the virtual point translation velocity calculation unit 27 will be explained in detail. The pitch angle command value θ is derived from the equation of motion between the translational position and the pitch angle. ｐ ｒｅｆ Translational acceleration v ｒｅｓ The transfer function to (dot) can be expressed by the following equation (32).

[0068] Since the transfer function in equation (32) has unstable zeros, a virtual point Vp is introduced to eliminate the unstable zeros. As shown in Figure 1, when the virtual point Vp is set on the coordinate axis AX2, the x-coordinate of the virtual point Vp is x ｖｐ This can be expressed by the following equation (33).

[0069] Pitch angle θ ｐ If sinθ is sufficiently small, ｐ = θ ｐSince it can be approximated as such, equation (32) becomes equation (34) by equation (33).

[0070] In equation (34), if the following condition in equation (35) is met, the unstable zeros can be eliminated and the inverse response in the pitch angle direction can be prevented.

[0071] According to Embodiment 1 described above, since the cargo disturbance compensation unit 26 compensates for gravity-dependent fluctuations based on a dynamic model, there is no need to tune the parameters of the virtual model, and therefore no tuning is required for control to stabilize the attitude when a load is placed on it.

[0072] Furthermore, according to Embodiment 1, the virtual point translation velocity calculation unit 27 calculates the translation velocity v of the virtual point Vp, which is set to remove unstable zeros. ｖｐ ｒｅｓ Calculate the translational velocity v of this virtual point Vp. ｖｐ ｒｅｓ The robot speed control controller 22 uses this to control the speed of the two-wheeled inverted robot 1. As a result, the translational speed of the two-wheeled inverted robot 1 can be controlled without causing reverse vibration in response to the movement of the pitch angle, and the translational responsiveness can be improved while stabilizing the pitch angle.

[0073] Furthermore, according to Embodiment 1, since the cargo disturbance compensation unit 26 compensates for gravity-dependent fluctuations based on a dynamic model, the tracking ability of the translational velocity system can be improved even when the weight or loading position of the cargo fluctuates.

[0074] The cargo disturbance compensation unit 26 may also be configured to have only one of either the model-based compensator 261 or the model error suppression compensator 262. In the case of only the model-based compensator 261, the pitch angle command value θ ｐ ｒｅｆ’ The pitch angle command value θ ｐ ｒｅｆ It outputs as follows. Also, in the case of only the model error suppression compensator 262, the pitch angle command value θ ｐ ｃｍｄ The pitch angle command value θ ｐ ｒｅｆ’This is input into reference model 2621.

[0075] (Embodiment 2) Figure 4 is a block diagram showing the control of a two-wheeled inverted robot including the control device according to Embodiment 1. As shown in Figure 4, the control device 20A does not have a virtual point translation speed calculation unit 27.

[0076] The speed command value generation unit 21 generates a speed command value v that instructs the translational speed of the two-wheeled inverted robot 1. ｃｍｄ Generate and output the following.

[0077] The robot speed control controller 22 receives the speed command value v generated by the speed command value generation unit 21. ｃｍｄ , and the translational speed v of wheel 2 ｒｅｓ Based on this, pitch angle θ ｐ The pitch angle command value θ controls the behavior of the two-wheeled inverted robot 1 to maintain its inverted posture. ｐ ｃｍｄ Calculate and output the result.

[0078] The cargo disturbance compensation unit 26 controls the pitch angle command value θ. ｐ ｃｍｄ The estimated value of the disturbance torque τ applied to arm 5 by the load. ａ ｌｏａｄ (Hat) and the translational velocity v of wheel 2 ｒｅｓ The compensated pitch angle command value θ, which is compensated using [the specified method]. ｐ ｒｅｆ The following is calculated and output. In the first embodiment, the cargo disturbance compensation unit 26 receives the translational velocity v of the virtual point Vp. ｖｐ ｒｅｓ Instead of the input previously being v, in Embodiment 2, the translational speed v of the two-wheeled inverted robot 1 is input to the cargo disturbance compensation unit 26. ｒｅｓ The input is used and the calculation is performed.

[0079] According to Embodiment 2 described above, since the cargo disturbance compensation unit 26 compensates for gravity-dependent fluctuations based on a dynamic model, there is no need to tune the parameters of the virtual model, and therefore no need to tune the control to stabilize the attitude when a load is placed on it.

[0080] As in Embodiment 2, the system may have a cargo disturbance compensation unit 26 but no virtual point translation speed calculation unit 27. In this configuration as well, the cargo disturbance compensation unit 26 may have only one of either a model-based compensator 261 or a model error suppression compensator 262. Alternatively, the system may have a disturbance observer instead of the model error suppression compensator 262.

[0081] (Embodiment 3) Figure 5 is a block diagram showing the control of a two-wheeled inverted robot including the control device according to Embodiment 1. As shown in Figure 5, the control device 20B does not have a cargo disturbance compensation unit 26.

[0082] The robot pitch angle control controller 23 receives the pitch angle command value θ output by the robot speed control controller 22. ｐ ｃｍｄ 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 following is calculated and output. Furthermore, when considering the effect of the load on the arm motor 6, for example, the pitch angle command value θ required to maintain an inverted posture while holding a load is calculated. ｐ ｃｍｄ_ｌоａｄ The robot pitch angle control controller 23 calculates the pitch angle command value θ and then... ｐ ｃｍｄ_ｌоａｄ Also take into consideration the torque command value τ ｗ ｒｅｆ You may calculate this.

[0083] According to the embodiment 3 described above, the virtual point translation velocity calculation unit 27 calculates the translation velocity v of the virtual point Vp, which is set to remove unstable zeros. ｖｐ ｒｅｓ Calculate the translational velocity v of this virtual point Vp. ｖｐ ｒｅｓ By using this, the robot speed control controller 22 controls the speed of the two-wheeled inverted robot 1, making it possible to control the translational speed of the two-wheeled inverted robot 1 without causing reverse vibration in response to the movement of the pitch angle, thereby improving the translational responsiveness while stabilizing the pitch angle.

[0084] As in Embodiment 3, the system may have a virtual point translation speed calculation unit 27 and omit a cargo disturbance compensation unit 26.

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

[0086] 1 Two-wheeled inverted robot 2 Wheels 3 Wheel motors 4 Body 5 Arm 5a Mounting surface 6 Arm motor 11 Pitch angle detection unit 12 Wheel angle detection unit 13 Arm angle detection unit 20, 20A, 20B Control device 21 Speed ​​command value generation unit 22 Robot speed control controller 23 Robot pitch angle control controller 24 Arm angle command value generation unit 25 Robot arm angle control controller 26 Cargo disturbance compensation unit 27 Virtual point translation speed calculation unit 261 MBC 262 MEC 2621 Reference model 2622 Compensator

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 the axle to rotate the wheels; a body attached to the pair of wheels and provided to be pivotable around the axle; and a loading section for loading a load, the control device comprising a load disturbance compensation unit that calculates and outputs a compensated pitch angle command value, which compensates for gravity-dependent fluctuations based on a dynamic model, based on a pitch angle command value for controlling the behavior of the pitch angle, which is the angle between the vertical axis of the body and the vertical direction, to maintain the inverted posture of the two-wheeled inverted robot; load load information applied to the loading section by the load; and the translational velocity of the wheels calculated by first derivative calculation with respect to time from the wheel position calculated from the detected wheel angle.

2. The control device according to claim 1, wherein the cargo disturbance compensation unit has a model-based compensator that calculates and outputs a pitch angle compensation value that compensates the pitch angle command value so that the gravity term applied in the pitch angle direction by the cargo load information becomes zero.

3. The control device according to claim 1, wherein the cargo disturbance compensation unit has a model error suppression compensator that calculates and outputs a pitch angle compensation value that compensates the pitch angle command value for fluctuations dependent on the weight of the cargo based on the error between the actual response and the reference model.

4. The control device according to claim 1, further comprising a virtual point translation velocity calculation unit that calculates the translational velocity of the wheel by performing a first derivative calculation with respect to time from the wheel position, calculates the pitch angular velocity by performing a first derivative calculation with respect to time from the detected pitch angle, and calculates the translational velocity of an arbitrarily set virtual point based on the translational velocity of the wheel and the pitch angular velocity.

5. The control device according to claim 4, wherein the cargo disturbance compensation unit calculates and outputs the compensated pitch angle compensation value based on the translational velocity of the virtual point.

6. The control device according to claim 4, wherein the distance from the axle to the virtual point is set to eliminate unstable zeros.

7. 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; a body attached to the pair of wheels and provided to be pivotable around the axle; and a loading section for loading a load, the control device comprising: a virtual point translation velocity calculation unit that calculates the translational velocity of the wheels by performing a first derivative calculation with respect to time from the wheel position calculated from a detected value of the wheel angle; a pitch angular velocity by performing a first derivative calculation with respect to time from a detected value of the pitch angle, which is the angle between the vertical axis of the body and the vertical direction; and a robot speed control controller that calculates and outputs a speed command value that instructs the translational velocity of the two-wheeled inverted robot, and a pitch angle command value for controlling the behavior of the pitch angle to maintain the inverted posture of the two-wheeled inverted robot, based on the translational velocity of the virtual point.

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 to rotate the wheels; a body attached to the pair of wheels and provided to be pivotable around the axle; and a loading section for loading a load, wherein the processor calculates and outputs a pitch angle compensation value that compensates for gravity-dependent fluctuations based on a dynamic model, based on a pitch angle command value for controlling the behavior of the pitch angle, which is the angle between the vertical axis of the body and the vertical direction, to maintain the inverted posture of the two-wheeled inverted robot; load information applied to the loading section by the load; and the translational velocity of the wheels calculated by performing a first-order derivative with respect to time from the wheel position calculated from the detected wheel angle.

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 the axle to rotate the wheels; a body attached to the pair of wheels and provided to be pivotable around the axle; and a loading section for loading a load, wherein the program causes a processor to perform the following process: calculate and output a pitch angle compensation value that compensates for gravity-dependent fluctuations based on a dynamic model, based on a pitch angle command value for controlling the behavior of the pitch angle, which is the angle between the vertical axis of the body and the vertical direction, to maintain the inverted posture of the two-wheeled inverted robot; load information applied to the loading section by the load; and the translational velocity of the wheels calculated by first-order derivative calculation with respect to time from the wheel position calculated from the detected wheel angle value.

10. A two-wheeled inverted robot comprising: 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 pivotable around the axle; a loading section for loading a load; and a control device for controlling the drive of the two-wheeled inverted robot, wherein the control device includes a load disturbance compensation unit that calculates and outputs a pitch angle compensation value based on a dynamic model, compensating for gravity-dependent fluctuations based on a dynamic model, using a pitch angle command value for controlling the behavior of the pitch angle, which is the angle between the vertical axis of the body and the vertical direction, to maintain the inverted posture of the two-wheeled inverted robot; load load information applied to the loading section by the load; and the translational velocity of the wheels calculated by first-order derivative calculation with respect to time from the wheel position calculated from the detected wheel angle.

Images