JP2011014100A - Inverted pendulum type mobile body and control method for the same - Google Patents

Abstract

PROBLEM TO BE SOLVED: To provide an inverted pendulum type mobile body which travels with high accuracy on a target path.SOLUTION: This mobile body includes wheels and a vehicle body supported by the wheels and travels on the path while maintaining the vehicle body in an inverted state by allowing the wheels to be driven in rotation. This moving body includes means (40, 32) for calculating a first control command value for controlling the position of the mobile body in the proceeding direction while maintaining the inversion of the vehicle body by using a target track expressing a target position of the mobile body by a time function, means (50, 34) for calculating a second control command value for controlling the angle in the turning direction of the mobile body by using the information regarding the target track and the actually measured or estimated position of the mobile body in the proceeding direction, and means (22, 24) for driving the wheels based on the first control command value and the second control command value.

Description

The present invention relates to an inverted pendulum type moving body. In particular, the present invention relates to a technique for accurately traveling on a target route on which an inverted pendulum type moving body is set.
In this specification, the “traveling direction of the moving body” refers to a direction orthogonal to the rotation axis of the wheel projected on the plane on which the moving body moves. The “turning direction of the moving body” refers to a direction around the vertical axis of the moving body. The “target trajectory” refers to a target position with time of the moving body (that is, a target position (target path) of the moving body expressed as a function of time).

  The inverted pendulum type moving body includes a wheel and a vehicle body supported by the wheel. In this moving body, the center of gravity of the vehicle body is located above the center of rotation of the wheel, and the vehicle body cannot be maintained in an inverted state unless the wheel is driven to rotate. For this reason, this type of moving body travels on the road surface while maintaining the vehicle body in an inverted state by rotationally driving the wheels. As a conventional technique of this type of moving body, for example, those disclosed in Patent Documents 1 and 2 are known.

JP 2004-295430 A JP 2007-319991 A

  The inverted pendulum type moving body has a structural characteristic that the speed response in the moving direction of the moving body is slower than the speed response in the turning direction of the moving body. More specifically, as shown in FIG. 12, in the inverted pendulum type moving body, the moment around the contact point A between the wheel 100 and the road surface R must be zero in order to maintain the vehicle body in an inverted state. That is, the moment around the contact point A due to the inertial force ma in the traveling direction of the moving body and the moment around the contact point A due to gravity mg acting on the moving body must be zero. For this reason, the acceleration in the traveling direction of the moving body is a function of the tilt angle of the vehicle body and cannot be changed suddenly. On the other hand, the acceleration in the turning direction of the mobile body does not have the above-described problem, and it is possible to change the acceleration in a step-like manner. Therefore, the inverted pendulum type moving body has a characteristic that the speed response in the moving direction of the moving body is slower than the speed response in the turning direction of the moving body. If the target trajectory is set ignoring such characteristics, there may be a problem that the moving body deviates from the target trajectory due to the nonholonomic constraint of wheel movement and does not travel on the target route.

  The present invention has been made in view of the above circumstances, and an inverted pendulum that can travel on a target route with high accuracy even if a target trajectory is set without considering the characteristics of an inverted pendulum type moving body. Provide a moving body of the mold.

The inverted pendulum type moving body of the present invention includes a wheel and a vehicle body supported by the wheel, and travels on the road surface while maintaining the vehicle body in an inverted state by rotationally driving the wheel. The moving body includes first control command value calculation means, second control command value calculation means, and drive means. The first control command value calculating means uses a target trajectory representing the target position of the moving body as a function of time, and controls the position of the moving body in the traveling direction while maintaining the vehicle body inverted. Calculate the value. The second control command value calculating means calculates a second control command value for controlling the angle of the moving body in the turning direction, using the target trajectory and the information on the actually measured or estimated position of the moving body in the traveling direction. To do. The driving means drives the wheel based on the first control command value and the second control command value.
In this moving body, the first control command value calculating means calculates the first control command value for controlling the position in the traveling direction, and the second control command value calculating means is the second for controlling the angle in the turning direction. Control command values are calculated, and the wheels are driven based on these control command values. Here, the second control command value calculation means calculates the second control command value using information on the target trajectory and the position of the moving body in the traveling direction. For this reason, when the moving body deviates from the target trajectory due to a delay in response speed in the traveling direction of the moving body, the second control command value in the turning direction can be calculated by correcting the deviation. Thereby, the moving body can travel on the target route with high accuracy.
Here, the “information on the position of the moving body in the traveling direction” is information that can specify the position (moving distance) of the moving body in the traveling direction. Corresponds to the physical quantity.

In the moving body, it is preferable that the moving body travels on the road surface by repeatedly executing the first control command value calculating means, the second control command value calculating means, and the driving means at predetermined intervals.
According to this configuration, the control command value is calculated every predetermined cycle, and the deviation from the target route is corrected every predetermined cycle. For this reason, the moving body can travel on the target route with high accuracy.

In one aspect of the moving body, a sensor (for example, an encoder that detects a rotational angular velocity of a wheel) for measuring information related to a position in the traveling direction of the moving body can be further provided. The second control command value calculating means calculates “information about the position of the measured traveling direction” calculated from “target trajectory” and “information about the position of the moving body in the traveling direction obtained from the detection result of the sensor”. It is preferable to calculate a second control command value for controlling the angle of the turning direction of the moving body based on the “target position of the corresponding moving body”.
According to this configuration, the second control command value in the turning direction is calculated using information on the measured position in the traveling direction. By using the information about the actually measured position, it is possible to realize movement along the target path (close to the target path or along the shape of the target path), and the moving body can move on the target path with high accuracy. .

In another aspect of the above moving body, the second control command value calculating means includes a “target trajectory” and a “traveling direction of the moving body estimated from the first control command value calculated by the first control command value calculating means”. Second control command value for controlling the angle of the turning direction of the moving body based on “the target position of the moving body corresponding to the information on the position in the estimated traveling direction” calculated from the “information on the position” Is preferably calculated.
According to this configuration, the second control command value is calculated using information on the position of the moving body in the traveling direction estimated from the first control command value. By using the information related to the position in the traveling direction estimated from the first control command value, the control command value in the turning direction can be calculated in consideration of the deviation of the moving body from the target route. Also with this configuration, the moving body can move on the target route with high accuracy.

In another aspect of the moving body, a sensor that measures the position of the moving body and an angle in the turning direction, a detection result of the sensor and a target trajectory, (1) position deviation in the traveling direction of the moving body, and (2) movement Based on the positional deviation in the direction perpendicular to the moving direction of the body and (3) the angular deviation in the turning direction of the moving body, the target value related to the position of the moving body in the moving direction and the target value related to the angle of the moving body in the turning direction A tracking controller for calculating the value can be further provided. Then, the first control command value calculating means calculates the first control command value based on the target value related to the position of the moving body in the traveling direction calculated by the tracking controller, and the second control command value calculating means is the tracking controller. It is preferable to calculate the second control command value based on the target value regarding the angle of the turning direction of the moving body calculated in step 1 and the calculated first control command value.
According to this configuration, the position of the moving body and the angle in the turning direction (that is, the orientation (posture)) can be determined from the detection result of the sensor, and the deviation from the target trajectory is specified using these, and this specified deviation is used. Thus, each target value in the traveling direction and the turning direction of the moving body is calculated. Based on these target values, the first control command value and the second control command value are calculated. At this time, since the second control command value is calculated using the first control command value, the second control command value in the turning direction is calculated in consideration of the delay in the traveling direction. Also with this configuration, the moving body can move on the target route with high accuracy.

In addition, the present invention provides an inverted pendulum type moving body control method capable of accurately traveling on a target route. That is, this control method includes a wheel, a vehicle body supported by the wheel, a drive device that rotationally drives the wheel, and a control device that controls the drive device, and the vehicle body is driven by rotating the wheel by the drive device. This is a control method for an inverted pendulum type moving body that travels on the road surface while maintaining an inverted state.
In this control method, (1) a first control command value for controlling the position of the moving body in the traveling direction while maintaining the vehicle body inverted using a target trajectory representing the target position of the moving body as a function of time. And (2) calculating a second control command value for controlling the angle of the turning direction of the moving body using the target trajectory and the information about the position of the moving body that is actually measured or estimated. And (3) driving the wheel based on the first control command value and the second control command value. And a mobile body is controlled by performing repeatedly each said process.
Also with this control method, since the control command value in the turning direction of the moving body is calculated using the information related to the position of the moving body in the traveling direction, the moving body can accurately move on the target route.

The perspective view of a moving body. The figure for demonstrating the advancing direction and turning direction of a mobile body. The functional block diagram of the control apparatus 30. FIG. The functional block diagram of the advancing direction target value calculation part 40. FIG. The functional block diagram of the turning direction target value calculation part 50. FIG. 4 is a flowchart showing processing of the control device 30. The figure for demonstrating operation | movement of the moving body of 1st Example. The functional block diagram of the control apparatus 60 which concerns on 2nd Example. The flowchart which shows the process of the control apparatus 60. FIG. The functional block diagram of the control apparatus 88 which concerns on 3rd Example. The flowchart which shows the process of the control apparatus 88. FIG. The figure for demonstrating the structural characteristic of the movable body of an inverted pendulum type.

The main features of the embodiments described in detail below are listed first.
(Mode 1) The moving body is composed of two wheels arranged on the same axle and a vehicle body supported by the wheels.
(Mode 2) The vehicle body drives the motor according to the detection results of the motors that drive the wheels, the gyro sensor that detects the tilt angular velocity of the vehicle body, the encoder that detects the rotational angular velocity of each motor, and the sensors. A control device is provided.
(Mode 3) The control device calculates a control command value for controlling the position of the moving body in the traveling direction using the target trajectory (x (t), y (t)).
(Mode 4) The control device corrects the target trajectory (x (t), y (t)) using at least one of the position in the traveling direction of the vehicle body, the velocity, and the acceleration. Then, using the corrected target trajectory (x ′, y ′), a control command value for controlling the angle of the moving body in the turning direction is calculated.
(Mode 5) The control device calculates the distance traveled by the moving body from the immediately preceding control cycle, using the rotational angular velocity of the motor detected by the encoder. Next, using the calculated moving distance and the target trajectory (x (t), y (t)), a time t ′ at which the moving distance should have been realized is calculated. Then, using the target trajectory (x (t ′), y (t ′)) at time t ′, a control command value for controlling the angle of the moving body in the turning direction is calculated.
(Mode 6) The control device will move the moving body in the current control cycle using the “control command value for controlling the position of the moving body in the traveling direction” calculated in the current control cycle. Calculate the distance. Next, using the calculated moving distance and the target trajectory (x (t), y (t)), a time t ′ at which the moving distance is to be realized is calculated. Then, using the target trajectory (x (t ′), y (t ′)) at time t ′, a control command value for controlling the angle of the moving body in the turning direction is calculated.
(Mode 7) The moving body includes a sensor that measures the position and orientation (x, y, φ) of the moving body. As this type of sensor, a GPS sensor, a laser range sensor, a motion capture, or the like can be used.
(Mode 8) The control device, based on the measured position and orientation (x, y, φ) of the moving body and the target trajectory (x (t), y (t)), (1) the traveling direction of the moving body The position deviation, (2) the position deviation in the direction orthogonal to the traveling direction of the moving body, and (3) the posture deviation of the moving body are calculated. Next, using these calculated deviations, a target value related to the position of the moving body in the traveling direction (for example, speed in the traveling direction) and a target value related to the angle in the turning direction of the moving body (for example, angular velocity in the turning direction). ) Is calculated. And about the control command value for controlling the position of the advancing direction of a mobile body, it calculates using the target value regarding the position of the calculated advancing direction. Regarding the control command value for controlling the angle of the turning direction of the moving body, the target value related to the calculated position in the traveling direction, the target value related to the calculated angle of the turning direction of the moving body, and the calculated moving body It calculates using the control command value for controlling the position of the advancing direction.
(Mode 9) In mode 8, the control command value for controlling the angle of the moving body in the turning direction is “target value related to the angle of the moving body in the turning direction” and “target value related to the position of the moving body in the traveling direction”. ”And“ control command value for controlling the position of the moving body in the traveling direction ”(ie,“ control command value for controlling the position of the moving body in the traveling direction ”/“ position of the moving body in the traveling direction ” Is calculated based on “a new target value related to the angle of the moving body in the turning direction”.

First Embodiment An inverted pendulum type moving body according to an embodiment of the present invention will be described with reference to the drawings. As shown in FIG. 1, the moving body 10 includes two wheels 12 and 14 disposed on an axle 16 and a vehicle body 20 supported by the wheels 12 and 14 so as to be tiltable around the axle 16. The center of gravity of the vehicle body 20 is located above the axle 16. The moving body 10 maintains the inverted state of the vehicle body 20 by driving the wheels 12 and 14, and travels on the floor surface by driving the wheels 12 and 14.

  The wheel 12 is connected to the motor 22 and is driven by the motor 22. The wheel 14 is connected to a motor 24 and is driven by the motor 24. That is, the wheels 12 and 14 are driven independently. The inverted state of the vehicle body 20 is maintained by driving the wheels 12 and 14 in accordance with the inclination angle θ of the vehicle body 20 (the angle formed by the vertical line drawn from the center of gravity of the vehicle body 20 to the axle 16 and the vertical axis). Moreover, the mobile body 10 travels on the floor surface by driving the wheels 12 and 14. The angle φ in the turning direction (the direction around the vertical axis) of the moving body 10 can be changed by controlling the rotational angular velocities of the wheels 12 and 14. That is, the movement of the moving body 10 on the floor surface (xy plane) is caused by the non-holonomic constraint of the speed r in the traveling direction of the moving body 10 (direction perpendicular to the axle 16) and the movement of the moving body 10. It is represented by an angular velocity φ ′ in the turning direction (direction around the vertical axis) (see FIG. 2).

  The vehicle body 20 includes motors 22 and 24 that drive the wheels 12 and 14, a gyro sensor 28 that detects the inclination angular velocity θ ′ of the vehicle body 20, and a control device 30 that outputs a control command value to the motors 22 and 24. .

Motors 22 and 24 arranged in the vehicle body 20 are connected to the wheels 12 and 14. The motors 22 and 24 are electrically connected to the control device 30. The motors 22 and 24 drive the wheels 12 and 14 in accordance with a control command value input from the control device 30.
The motor 22 is provided with an encoder 22a. The encoder 22a is electrically connected to the control device 30. The encoder 22a detects the rotational angular velocity of the motor 22 (that is, the rotational angular velocity of the wheel 12). The rotational angular velocity of the wheel 12 detected by the encoder 22a is input to the control device 30. The motor 24 is also provided with an encoder 24a. The encoder 24 a detects the rotational angular velocity of the wheel 14. The rotational angular velocity of the wheel 14 detected by the encoder 24 a is also input to the control device 30.

  The gyro sensor 28 is disposed inside the vehicle body 20. The gyro sensor 28 is electrically connected to the control device 30. The gyro sensor 28 detects the inclination angular velocity θ ′ of the vehicle body 20. The detected tilt angular velocity θ ′ is input to the control device 30.

The control device 30 is constituted by a microprocessor having a CPU, a ROM, and a RAM. The control device 30 is disposed in the vehicle body 20. The control device 30 is electrically connected to the motors 22 and 24, the encoders 22a and 24a, and the gyro sensor 28.
FIG. 3 is a block diagram illustrating functions of the control device 30. As shown in FIG. 3, the control device 30 includes a target trajectory input unit 38, a traveling direction target value calculation unit 40, an inversion control controller 32, a turning direction target value calculation unit 50, a turning control controller 34, and a command value synthesis unit 36. It is configured. In FIG. 3, only one motor and encoder are illustrated in consideration of easy viewing, but actually, control command values are independently output to the two motors 22 and 24.

  The target trajectory input unit 38 stores the target trajectory (x (t), y (t)) of the moving body 10. The target trajectory (x (t), y (t)) stored in the target trajectory input unit 38 is input to the traveling direction target value calculation unit 40 and the turning direction target value calculation unit 50. The target trajectory (x (t), y (t)) represents the position (x, y) on the floor surface of the moving body 10 as a function of time t. The target trajectory (x (t), y (t)) is input to the target trajectory input unit 38 using an external device (for example, a remote control device or a device that generates a predetermined activation).

  The traveling direction target value calculation unit 40 uses the target trajectory (x (t), y (t)) for each control cycle, and the traveling target control target value (target tangential velocity s ′ (t)). , Target route length s (t)) is calculated. Details of the traveling direction target value calculation unit 40 will be described later.

The inversion control controller 32, for each control cycle, the inclination angular velocity θ ′ of the vehicle body 20 detected by the gyro sensor 28, the rotational angular velocities of the wheels 12 and 14 detected by the encoders 22a and 24a, and the traveling direction target value calculation unit. Based on the target path length s (t) and the target tangential speed s ′ (t) calculated at 40, the moving body 10 maintains an inverted position and controls the position of the moving body 10 in the traveling direction. Calculate the command value. Specifically, the path length s _r (t) and the target path length of the moving body calculated from the rotational angular velocities of the wheels 12 and 14 detected by the encoders 22a and 24a and the tilt angle of the vehicle body (detected by the gyro sensor 28). The deviation from s (t) is reduced, and the tangential speed s _r ′ (t) and the target tangential speed s ′ (t) of the moving body calculated from the rotational angular velocities of the wheels 12 and 14 detected by the encoders 22a and 24a. Torque command values for driving the motors 22 and 24 so that the mobile body 10 is inverted based on the inclination angular velocity θ ′ of the vehicle body 20 detected by the gyro sensor 28. Is calculated. As a specific configuration of the inverted control controller 32, for example, the configuration disclosed in Patent Document 1 can be used.

  The turning direction target value calculation unit 50 uses the target trajectory (x (t), y (t)) and the rotational angular velocities of the wheels 12 and 14 detected by the encoders 22a and 24a for each control cycle. 10 control target values (target angle φ, target angular velocity φ ′) in the turning direction are calculated.

The turning controller 34 is based on the rotational angular velocities of the wheels 12 and 14 detected by the encoders 22a and 24a, the target angle φ and the target angular velocity φ ′ calculated by the turning direction target value calculation unit 50 for each control cycle. Then, a control command value for controlling the position of the moving body 10 in the turning direction is calculated. Specifically, the deviation between the “current angle φ _r of the moving body 10 calculated from the rotational angular velocities of the wheels 12 and 14 detected by the encoders 22a and 24a” and the target angle φ, and “the encoders 22a and 24a Torque command values for driving the motors 22 and 24 so as to reduce the deviation between the angular velocity φ _r ′ of the moving body calculated from the detected rotational angular velocities of the wheels 12 and 14 and the target angular velocity φ ′. calculate. As a specific configuration of the turning control controller 34, for example, the configuration disclosed in Patent Document 1 can be used.

  The command value synthesis unit 36 synthesizes the torque command value output from the inverted control controller 32 and the torque command value output from the turning control controller 34, and outputs the synthesized torque command value to the motors 22 and 24. The motors 22 and 24 are driven based on the combined torque command value.

Next, a detailed configuration of the above-described traveling direction target value calculation unit 40 will be described. FIG. 4 is a block diagram illustrating functions of the traveling direction target value calculation unit 40. As shown in FIG. 4, the traveling direction target value calculation unit 40 includes a target speed calculation unit 42, a target tangential speed calculation unit 44, and a target path length calculation unit 46.
The target speed calculation unit 42 calculates the target speed of the moving body 10 from the target trajectory (x (t), y (t)) input from the target trajectory input unit 38. Specifically, the target velocity (x ′ (t), y ′ (t)) is calculated by differentiating the target trajectory (x (t), y (t)). The calculated target speed (x ′ (t), y ′ (t)) is input to the target tangential speed calculation unit 44.
The target tangential speed calculation unit 44 calculates the target tangential speed from the input target speed (x ′ (t), y ′ (t)). The target tangential speed is an absolute value (scalar) of a target speed (x ′ (t), y ′ (t)) given by a vector. Therefore, the target tangential velocity s ′ (t) is calculated by (x ′ (t) ² + y ′ (t) ² ) ^1/2 . The calculated target tangential speed s ′ (t) is input to the inversion control controller 32 and the target path length calculation unit 46.
The target path length calculation unit 46 calculates the target path length s (t) of the moving body 10 by integrating the input target tangential velocity (x ′ (t) ² + y ′ (t) ² ) ^1/2. . The calculated target path length s (t) is input to the inversion control controller 32 and the target path length calculation unit 46.
As is clear from the above, the target values (target tangential speed s ′, target path length s) of the moving body 10 are calculated based only on the target trajectory (x (t), y (t)). The

  Next, a detailed configuration of the above-described turning direction target value calculation unit 50 will be described. FIG. 5 is a block diagram illustrating functions of the turning direction target value calculation unit 50. As shown in FIG. 5, the turning direction target value calculation unit 50 includes a measured tangential speed calculation unit 59, a measurement path length calculation unit 58, a correction time calculation unit 57, a new target position calculation unit 51, and a new target speed. The calculation unit 52, the new target acceleration calculation unit 54, the target posture (angle) calculation unit 53, the new target curvature calculation unit 55, and the target angular velocity calculation unit 56 are configured.

The measurement tangential velocity calculation unit 59, the measurement path length calculation unit 58, the correction time calculation unit 57, and the new target position calculation unit 51 use the rotational angular velocities of the wheels 12 and 14 detected by the encoders 22a and 24a, respectively. A new target position (x, y) is calculated in consideration of the response delay.
That is, the measured tangential velocity calculation unit 59 calculates the tangential velocity s _r ′ of the moving body 10 from the rotational angular velocities of the wheels 12 and 14 detected by the encoders 22a and 24a. That is, the tangential velocity s _r ′ of the moving body 10 is calculated from the rotational angular velocity of the wheels 12 and 14 and the geometrical relationship (the diameter of the wheels 12 and 14 and the distance between the wheels 12 and 14). To do. The calculated tangential velocity s _r ′ is input to the target angle calculation unit 56 and the measurement path length calculation unit 58.
The measurement path length calculation unit 58 calculates the path length s _r of the moving body 10 by integrating the tangential speed s _r ′ calculated by the measurement tangential speed calculation unit 59. The calculated route length s _r is input to the correction time calculation unit 57.
The correction time calculation unit 57 calculates a time t ′ at which the calculated route length s _r should be realized from the calculated route length s _r and the target trajectory (x (t), y (t)). calculate. That is, the relationship between the time t and the target path length s (t) is obtained from the target trajectory (x (t), y (t)). Next, based on the relationship between the time t and the target route length s (t), a time t ′ when the route length s _r calculated by the measured route length calculation unit 58 is obtained is calculated. The calculated time t ′ is input to the new target position calculation unit 51.
The new target position calculation unit 51 uses the time t ′ calculated by the correction time calculation unit 57 to calculate a new target position (x (t ′), y (t ′)). Therefore, when the actual path length s _r does not become the target path length s (t) obtained from the target trajectory due to the delay in speed response in the traveling direction, the time t when the path length s _r should have been realized. 'Is obtained, and the target position at the time t' is adopted as the new target position (x (t '), y (t')).

The new target velocity calculation unit 52, the target posture calculation unit 53, the new target acceleration calculation unit 54, the new target curvature calculation unit 55, and the target angular velocity calculation unit 56 are calculated using the new target position (x (t ′)) calculated as described above. , Y (t ′)), a control target value (target angle φ, target angular velocity φ ′) in the turning direction of the moving body 10 is calculated.
That is, the new target speed calculation unit 52 calculates the new target speed of the moving body 10 using the newly calculated new target position (x (t ′), y (t ′)). Specifically, the new target speed (x ′ (t ′), y ′ (t ′)) is calculated by differentiating the new target position (x (t ′), y (t ′)). The calculated new target speed (x ′ (t ′), y ′ (t ′)) is input to the target posture calculation unit 53, the new target acceleration calculation unit 44, and the new target curvature calculation unit 55.
The target posture calculation unit 53 calculates the target posture of the moving body 10 (that is, the target angle φ in the turning direction) using the calculated new target speed (x ′ (t ′), y ′ (t ′)). To do. That is, since the relationship of tan φ = [y ′ (t ′)] / [x ′ (t ′)] is established, the new target speeds (x ′ (t ′), y ′ (t ′) calculated by this equation are satisfied. )) Is substituted to calculate the target angle φ. The calculated target angle φ is input to the turning control controller 34.
The new target acceleration calculation unit 54 calculates the new target acceleration of the moving body 10 using the calculated new target speed (x ′ (t ′), y ′ (t ′)). Specifically, the new target acceleration (x ″ (t ′), y ″ (t ′)) is obtained by differentiating the new target speed (x ′ (t ′), y ′ (t ′)). calculate. The calculated new target acceleration (x ″ (t ′), y ″ (t ′)) is input to the new target curvature calculation unit 55.
The new target curvature calculation unit 55 uses the new target speed (x ′ (t ′), y ′ (t ′)) and the new target acceleration (x ″ (t ′), y ″ (t ′)). Thus, the new target curvature 1 / R of the moving body 10 is calculated. Specifically, the target tangential speed v ′ of the moving body is calculated from the new target speed (x ′ (t ′), y ′ (t ′)), and the new target acceleration (x ″ (t ′), y ′ It calculates the target acceleration a _n of the normal direction of the moving body from the '(t')). Here, the target tangential velocity v 'between the acceleration a _n and the curvature radius R of the normal direction, a n ₌ v' relationship ^{2 /} R is satisfied. Therefore, when an acceleration a _n of the target tangential velocity v 'and the normal direction is known, it is possible to calculate the radius of curvature R (i.e., it is possible to calculate the curvature 1 / R). The calculated curvature 1 / R is input to the target angle calculation unit 56.
The target angular velocity calculation unit 56 calculates the target angular velocity φ ′ in the turning direction of the moving body 10 using the actual tangential velocity s _r ′ calculated by the measured tangential velocity calculation unit 59 and the calculated curvature 1 / R. To do. Specifically, the target angular velocity φ ′ is calculated by multiplying the tangential velocity s _r ′ by the curvature (1 / R). The calculated target angular velocity φ ′ is input to the turning controller 34.

  In the above-described embodiment, the target angle φ and the target angular velocity φ ′ of the turning direction of the moving body 10 are used to control the position of the moving body 10 in the turning direction. However, it is not always necessary to use both the target angle φ and the target angular velocity φ ′. For example, the position of the moving body 10 in the turning direction is controlled based only on the target angle φ in the turning direction of the moving body 10. Also good. In this case, the new target acceleration calculator 54, the new target curvature calculator 55, and the target angular velocity calculator 56 described above can be eliminated, and the configuration of the turning direction target value calculator 50 can be simplified.

Next, processing performed by the control device 30 configured as described above will be described. FIG. 6 is a flowchart showing a processing procedure of the control device 30.
As shown in FIG. 6, the control device 30 first reads a time t after the control device 30 is activated (that is, an elapsed time since the start of the control of the moving body 10) (S2). The time t is obtained, for example, by installing a timer in the control device 30 and measuring the elapsed time from the start of control using this timer.
Next, the values of the encoders 22a and 24a of the motors 22 and 24 (that is, the rotational angular velocities of the wheels 12 and 14) are read (S4).
Next, from the values of the encoders 22a and 24a read in step S4, the current tangential speed s _r ′ of the moving body 10, the path length s _r , the turning direction angle (posture) φ _r, and the turning direction An angular velocity φ _r ′ is calculated (S6). That is, the rotational speeds of the wheels 12 and 14 are calculated from the values of the encoders 22a and 24a, and the x-direction, y-direction, and φ-direction of the moving body 10 are calculated from the calculated rotational speed and the geometric relationship between the wheels 12 and 14. The speed is calculated (for example, it can be calculated using the Jacobian matrix disclosed in Patent Document 1). Next, the current tangential speed s _r ′, path length s _r , turning direction angle (posture) φ _r , and turning direction angular speed φ _r ′ of the moving body 10 are calculated from the calculated speed components. .
In step S8, the tilt angular velocity θ ′ detected by the gyro sensor 28 is read.
In step S10, the target tangential velocity s' (t) and the target are obtained from the time t read in step S2 and the target trajectory data (x (t), y (t)) stored in the target trajectory input unit 38. The path length s (t) is calculated (see the description of the traveling direction target value calculation unit 40).
Next, the current tangential velocity s _r ′ and path length s _r calculated in step S6, the tilt angular velocity θ ′ read in step S8, and the target tangential velocity s ′ (t) calculated in step S10. Using the target path length s (t), torque command values of the motors 22 and 24 relating to the traveling direction of the moving body 10 are calculated (S12).
Next, using the current route length s _r calculated in step S6, a time t ′ at which the route length s _r should have been obtained is obtained (S14), and the moving object 10 is obtained from the obtained correction time t ′. The target angle (target posture) φ and the target angular velocity φ ′ in the turning direction are calculated (s16). (Refer to the description of the turning direction target value calculation unit 50).
Next, the current turning direction angle (posture) φ _r and angular velocity φ _r ′ calculated in step S6 and the turning direction target angle (target posture) φ and target angular velocity φ ′ calculated in step S14 are obtained. The torque command values of the motors 22 and 24 relating to the turning direction of the moving body 10 are respectively calculated (S18).
In step S20, the torque command value calculated in step S12 and the torque command value calculated in step S18 are added, and these values are added to the motors 22 and 24 (specifically, the motor drive for driving the motors 22 and 24). Circuit). As a result, the wheels 12 and 14 are driven. When step S20 ends, the process returns to step 2 to start processing at the next control timing.
Note that the processing from step S2 to step S20 is performed at a predetermined time interval (for example, 10 ms). Thereby, the mobile body 10 will correct | amend the deviation | shift from a target track | orbit for every control period.

The operation of the moving body 10 when the moving body 10 described above deviates from the target trajectory (x (t), y (t)) will be described with reference to FIG. Note that the control cycle of the moving body 10 is Δt. In FIG. 7, a target trajectory is set on the xy plane, a travel route on which the moving body 10 of the present embodiment travels according to the target trajectory, and a travel on which the mobile body according to the related art travels according to the target trajectory. A route is schematically shown. As shown in FIG. 7, the target position _P 1 _(t 1) at time _{t 1} as the target trajectory, the target position _P 2 _{(t 1} + _Δt) at time _{t 1} + Delta] t, the target at time _t 1 + 2? T The target position P ₄ (t ₁ + 3Δt) at the time of position P ₃ (t ₁ + 2Δt) and time t ₁ + 3Δt is set. The moving body is assumed to be at the target position P ₁ (t ₁ ) at time t ₁ and moves from this position P ₁ (t ₁ ) to the target trajectory (P ₁ → P ₂ → P ₃ → P ₄ →). And

First, the case of an inverted pendulum type moving body according to the prior art will be described. Mobile first calculates the control command value for moving to a position _P 1 at time _{t 1 (t 1)} from the time _{t 1} + Delta] t of the target position _{P 2 (t 1 + Δt)} , based on the control command value To drive the wheels 12 and 14. At this time, since there is a delay in speed response in the traveling direction, the moving body cannot move the distance (path length) necessary to reach the target position P ₂ from the target position P ₁ . On the other hand, the turning direction is controlled as set in the target trajectory without any delay in speed response. Therefore, the moving body moves to a position P ₂ ′ that deviates from the target position P _{2 at} time t ₁ + Δt.
Next, the mobile calculates the control command value for moving from the target position P ₂ to the target position P _3, to drive the wheels 12, 14 on the basis of the control command value. Also in this case, the delay of the speed responsive for the traveling direction, the mobile shall not be able to move a distance (path length) required to reach the target position P ₂ to the target position P _3. On the other hand, the turning direction is controlled as set in the target trajectory without any delay in speed response. Therefore, the moving body moves to a position P ₃ ″ further deviated from the target position P _{3 at} time t ₁ + 2Δt.
Hereinafter, similarly calculates a control command value for moving from the target position P ₃ to the target position P _4, to drive the wheels 12, 14 on the basis of the control command value. Also in this case, the moving body moves to a position P ₄ ″ further deviated from the target trajectory due to a delay in speed response in the traveling direction.
As described above, when the moving body deviates from the target trajectory due to the delay in the speed response in the traveling direction, the control command value is calculated without correcting the deviation. For this reason, the deviation from the target trajectory increases.

Next, the case of the moving body 10 of the present embodiment will be described. Moving body 10, similarly to the case described above, first, it calculates a control command value for moving to a position _{P 1 (t} 1) from the time _{t 2} the target position _P 2 at time _{t 1 (t 1} + _Δt) The wheels 12 and 14 are driven based on the control command value. Again, the speed response of the delay for the travel direction, the moving body can not move the distance (path length) required to reach the target position P ₁ to the target position P _2. On the other hand, since there is no speed response delay in the turning direction, the turning direction is controlled as set in the target trajectory. Therefore, the moving body moves to a position P ₂ ′ that deviates from the target position P ₂ , as in the prior art.
Next, the moving body 10 calculates a control command value for moving from the target position P ₂ (t ₁ + Δt) to the target position P ₃ (t ₁ + 2Δt) in the traveling direction. On the other hand, for the turning direction, the control target value is calculated with reference to the point P _2n on the target trajectory corresponding to the distance between the point P ₁ and the point P ₂ ′. Here, when the moving body moves according to the target trajectory, the moving body 10 passes through the point _P2n on the target trajectory at time t ′. The control target value in the turning direction moves from point P (t ′) (a point on the target trajectory at time t ′) to point P (t ′ + Δt) (a point on the target trajectory at time t ′ + Δt). The control command value is calculated. Next, the wheels 12 and 14 are driven based on these control target values. For this reason, the moving body moves from the point P ₂ ′ to the point P ₃ ′. As a result, the moving body 10 approaches the target trajectory.
Hereinafter, similarly, in the traveling direction, a control command value for moving from the target position P ₃ (t ₁ + 2Δt) to the target position P ₄ (t ₁ + 3Δt) is calculated. For the turning direction, a control command value is calculated based on the current position P ₃ ′ of the moving body 10. For this reason, the moving body 10 moves from the point P ₃ ′ to the point P ₄ ′.

  As is clear from the above description, in the moving body 10 of the present embodiment, when the moving body 10 deviates from the target trajectory due to a delay in speed response in the traveling direction, the turning direction is controlled based on the deviated position. A command value is calculated. That is, the control command value in the turning direction is calculated according to the moving distance in the traveling direction (the actually traveled path length). Thereby, the deviation of the moving body 10 from the target trajectory is corrected, and the moving body 10 can travel on the target route with high accuracy.

Second Example Next, a moving body according to a second example of the present invention will be described. Note that the moving body of the second embodiment has substantially the same configuration as the moving body of the first embodiment, and only the configuration of the control device 60 is different. Below, it demonstrates centering on a different point from 1st Example.

  FIG. 8 is a block diagram showing functions of the control device 60 of the second embodiment. As shown in FIG. 8, the control device 60 includes a target trajectory input unit 38, a traveling direction target value calculation unit 62 for calculating a control command value in the traveling direction of the moving body, an inverted control controller 64, and a target tangential velocity calculation. A turning direction control command value calculation unit 70 that calculates a control command value for the turning direction of the moving body, and a wheel angular velocity controller 68.

  The target trajectory input unit 38 stores the target trajectory (x (t), y (t)) of the moving body, as in the first embodiment. Similarly to the first embodiment, the traveling direction target value calculation unit 62 also uses the target trajectory (x (t), y (t)) of the moving body to control the traveling direction control target value (target tangential velocity). s ′ (t), target route length s (t)) is calculated. The calculated control target values (target tangential speed s ′ (t), target path length s (t)) are input to the inversion control controller 64.

Similarly to the first embodiment, the inversion control controller 64 also includes the path length s _r (t) of the moving body calculated from the rotational angular velocities of the wheels 12 and 14 detected by the encoders 22a and 24a and the target path length s ( t), and the difference between the tangential speed s _r '(t) and the target tangential speed s' (t) of the moving body calculated from the rotational angular velocities of the wheels 12 and 14 detected by the encoders 22a and 24a. Based on the inclination angular velocity θ ′ of the vehicle body 20 detected by the gyro sensor 28, the control command value for driving the motors 22 and 24 is calculated so that the moving body 10 is kept inverted. To do. However, unlike the first embodiment, the inversion control controller 64 of the second embodiment outputs a target tangential acceleration (hereinafter referred to as a new target tangential acceleration) of the moving body as a control command value. As a design method of the inversion control controller 64, a method similar to the design method disclosed in Patent Document 1 can be used. That is, from the relationship of τ (torque) = I (moment of inertia) × ω ′ (angular acceleration), an equation of motion as shown in Patent Document 1 is expressed using the angular acceleration ω ′, and the patent literature describes the equation of motion. 1 can be applied by applying the same method as in FIG.

  The target tangential speed calculation unit 66 integrates the new target tangential acceleration calculated by the inversion control controller 64 to calculate a new target tangential speed. The new target tangential velocity is output to the turning direction control command value calculation unit 70 and the wheel angular velocity controller 68.

The turning direction control command value calculation unit 70 includes the target trajectory (x (t), y (t)) and the new target tangential velocity calculated by the target tangential velocity calculation unit 66 (that is, a delay in the traveling direction). The control command value (target angular velocity φ ′) in the turning direction of the moving body 10 is calculated using the above. The turning direction control command value calculation unit 70 includes a plurality of calculation units 72, 74, 76, 78, 80, 82, and 84, which perform predetermined calculations in order, thereby controlling the turning direction control command value (target angular velocity). φ ′) is calculated.
Specifically, the turning direction control command value calculation unit 70 first calculates the estimated path length of the moving body using the new target tangential speed calculated by the target tangential speed calculation unit 66 (new path length calculation unit). 76). Next, a time t ′ at which the path length is to be realized is calculated from the estimated path length of the mobile body (corrected time calculation unit 74). Next, a new target position (x (t ′), y (t ′)) is calculated from the calculated time t ′ and the target trajectory (x (t), y (t)) (new target position calculation unit 72). . When a new target position (x (t ′), y (t ′)) is calculated, a new target speed is calculated from the target position (x (t ′), y (t ′)) (new target speed calculation). Part 78). Next, a new target acceleration is calculated from the calculated new target speed (new target acceleration calculation unit 80), and a curvature 1 / R is calculated from the new target acceleration and the new target speed (target curvature calculation unit 82). Finally, the target angular velocity φ ′ is calculated by multiplying the new target velocity calculated by the new target velocity calculating unit 78 and the curvature 1 / R calculated by the target curvature calculating unit 82 (target angular velocity calculating unit 84). The calculated target angular velocity φ ′ is input to the wheel angular velocity controller 68.

  The wheel angular velocity controller 68 combines the new target tangential velocity calculated by the target tangential velocity calculation unit 66 and the target angular velocity φ ′ calculated by the target angle calculation unit 84, and distributes them to the motors 22 and 24. The target angular velocity of each motor 22, 24 (each wheel 12, 14) is calculated. Then, the motors 22 and 24 are driven so that the deviation between the calculated target angular velocity and the target angular velocity detected by the encoders 22a and 24a becomes zero.

  As is clear from the above description, in the second embodiment, the route travels in this cycle from the new target tangential speed (tangential speed including a delay in the response speed in the traveling direction) that is the control command value in the traveling direction. The length is calculated, and the control command value in the turning direction is calculated based on the target position (x (t ′), y (t ′)) at the time t ′ at which the path length is to be realized. That is, in the second embodiment, the control command value in the turning direction is calculated in consideration of the deviation predicted from the control command value in the traveling direction in the current cycle.

Next, processing performed by the control device 60 configured as described above will be described. FIG. 9 is a flowchart showing a processing procedure of the control device 60.
As shown in FIG. 9, the control device 60 first reads a time t (elapsed time from the start of control) since the control device 60 was activated (S22). Next, the values of the encoders 22a and 24a of the motors 22 and 24 (that is, the rotational angular velocities of the wheels 12 and 13) and the value of the gyro sensor 28 (the inclination angular velocity of the vehicle body) are read (S23). Further, the current tangential velocity s _r ′ and path length s _r of the moving body 10 are calculated from the read values of the encoders 22a and 24a.
Next, the target tangential speed and target path length of the moving body are calculated from the time t read in step S22 (S24), and obtained from the calculated target tangential speed and target path length and the values of the encoders 22a and 24a. A control command value (target tangential acceleration) for inversion control is calculated from the current tangential speed and path length and the inclination angular velocity of the gyro sensor 28 (S26).
In step S28, the control command value (acceleration) calculated in step S26 is integrated to calculate a new target tangential velocity. Next, a new path length is calculated from the new target tangential velocity calculated in step S28 (S30), and a correction time t ′ is calculated from the new path length (S32). Next, the target angular velocity φ ′ in the turning direction of the moving body 10 is calculated from the correction time t ′ obtained in step S32 (S34).
Next, the new target tangential velocity calculated in step S28 and the target angular velocity φ ′ calculated in step S34 are combined, and the combined result is distributed to the motors 12, 14, and the target angular velocity of each motor 12, 14 is determined. Calculate (S36).
Next, the motors 22 and 24 are controlled so that the deviation between the target angular velocity calculated in step S36 and the wheel rotational speed measured by the encoders 22a and 24a becomes zero (S38). As a result, the wheels 12 and 14 are driven. When step S38 ends, the process returns to step 22, and processing at the next control timing is started.

As is apparent from the above description, in the moving body of the second embodiment, the target position of the moving body (target position including the delay in the traveling direction) is determined using the new target tangential velocity that is the control command value in the traveling direction. Based on this target position, a control command value (target angular velocity) in the turning direction is calculated. For this reason, the motors 22 and 24 are driven in anticipation of a deviation from the target trajectory due to a speed response delay in the traveling direction, and the deviation of the moving body from the target trajectory is prevented in advance.
In the second embodiment, the detection results of the encoders 22 a and 24 a are fed back to the wheel angular velocity controller 68. For this reason, the feedback loop becomes small, and the motors 22 and 24 can be accurately controlled to a desired angle. Thereby, the moving body can travel on the target route defined by the target track with high accuracy.

(Third embodiment) Next, a moving body according to a third embodiment of the present invention will be described. The moving body of the third embodiment includes a sensor that detects the position (x, y) and posture (φ) of the moving body, and uses the detection result of the sensor in the first and second embodiments. It is different from the moving body in the example. In the following description, the points different from the above-described embodiment will be mainly described.

FIG. 10 is a block diagram showing functions of the moving body control device 88 of the third embodiment. As shown in FIG. 10, a position / posture sensor 86, a gyro sensor 28, and encoders 22a and 24a are connected to the control device 88.
The position / posture sensor 86 detects the position (x, y) and posture (φ) of the moving body. As the position / posture sensor 86, for example, a laser range sensor can be used. The laser range sensor detects the distance to the object irradiated with the laser by irradiating the laser. For this reason, the position and posture (x) of the moving body is checked by comparing the map storing the shape of the wall of the room where the moving body travels with the shape of the wall when the laser is irradiated in all directions (360 degrees). , Y, φ) can be obtained. The detection result (x, y, φ) of the position / posture sensor 86 is input to the control device 88. The position / orientation sensor 86 may be a GPS sensor, motion capture, or the like.

The control device 88 controls the motors 22 and 24 based on the detection result (x, y, φ) of the position / posture sensor 86, the detection result θ ′ of the gyro sensor 28, and the detection results of the encoders 22a and 24a. To do. The control device 88 includes various functions 38 and 90 to 112, and these perform predetermined calculations to calculate control command values (target angular velocities) for controlling the motors 22 and 24.
Specifically, the control device 88 differentiates the target trajectory (x (t), y (t)) stored in the target trajectory input unit 38 to obtain the target speed (x ′ (t)) of the moving object. , Y ′ (t)). Then, x ′ (t) and y ′ (t) are substituted into the equation φ = Arctan (y ′ (t) / x ′ (t)) to calculate the target posture φ of the moving body (target posture calculation unit 90).
When the target posture is calculated, a posture deviation Δφ is calculated from the target posture and the posture (φ) of the moving body detected by the position / posture sensor (deviation calculation unit 92), and the posture deviation Δφ is calculated as the trajectory tracking controller 100. To enter.
Further, using the position of the moving body calculated from the target trajectory (x (t), y (t)) and the current position of the moving body calculated from the detection result of the position / posture sensor 86, the moving body Is calculated (deviation calculation unit 94), and the positional deviation is input to the trajectory tracking controller 100.
Further, using the position of the moving body calculated from the target trajectory (x (t), y (t)) and the current position of the moving body calculated from the detection result of the position / posture sensor 86, the moving body The position deviation in the direction orthogonal to the traveling direction is calculated (deviation calculation unit 96), and the deviation is input to the trajectory tracking controller 100.

  The trajectory tracking controller 100 receives each of the above deviations and calculates a target tangential velocity and a target angular velocity such that these deviations are zero. A specific configuration example of the trajectory tracking controller 100 is disclosed in, for example, Japanese Patent Laid-Open No. 3-24605.

The control device 88 inputs the target tangential speed calculated by the trajectory tracking controller 100 to the inversion control controller 110. Further, the target path length is calculated by integrating the calculated target tangential velocity (path length calculation unit 102), and the target path length is input to the inversion control controller 110. Based on the detection result of the gyro sensor 28, the detection results of the encoders 22a and 24a, and the input target tangential speed and target path length, the inversion control controller 110 controls the moving body control command value (new tangential acceleration). ) Is calculated. The inversion control controller 110 is the same as the inversion control controller 64 of the second embodiment.
Next, a new tangential velocity is calculated by integrating the new tangential acceleration calculated by the inversion control controller 110 (new tangential velocity calculation unit 108). A ratio (new tangential speed / target tangential speed) between the calculated new tangential speed and the target tangential speed calculated by the trajectory tracking controller 100 is calculated (speed ratio calculation unit 104). In other words, the new tangential speed processed by the inversion control controller 110 takes into account the response delay in the traveling direction of the moving body. On the other hand, the target tangential velocity calculated by the trajectory tracking controller 100 does not take into account the response delay in the traveling direction of the moving body. For this reason, the degree of delay of the speed response in the traveling direction of the moving body is calculated by obtaining the ratio between the new tangential speed and the target tangential speed.
Next, the target angular velocity calculated by the trajectory tracking controller 100 is multiplied by the above ratio (new tangential velocity / target tangential velocity) to calculate a control command value (target angular velocity) of the turning direction of the moving body (in the turning direction). New target calculation unit 106).
The wheel angular velocity controller 112 combines the control command value (new tangential velocity) calculated by the new tangential velocity calculation unit 108 and the control command value (target angular velocity φ ′) calculated by the target angle calculation unit 106, By distributing to each motor 22, 24, the target angular velocity of each motor 22, 24 (each wheel 12, 14) is calculated. Then, the motors 22 and 24 are driven so that the deviation between the calculated target angular velocity and the target angular velocity detected by the encoders 22a and 24a becomes zero.

Next, processing performed by the control device 88 configured as described above will be described. FIG. 11 is a flowchart showing the processing procedure of the control device 88.
As shown in FIG. 11, the control device 88 first reads a time t (that is, an elapsed time from the start of control) after the control device 88 is activated (S42). Next, the output of the position / posture sensor 86 is read (S43). Further, the output of the gyro sensor 28 and the outputs of the encoders 22a and 24a are read (S44).
Next, a target tangential speed is calculated from the target trajectory, and a target posture (φ) is calculated from the target tangential speed (S46). Then, each deviation (Δx, Δy, Δφ) is calculated from the output of the position / posture sensor 86 and the target trajectory (S48). Next, the trajectory tracking controller 110 calculates the target tangential velocity and target angular velocity (target angular velocity in the turning direction) of the moving body using the deviation calculated in step S48 (S50). When the target tangential speed is calculated in step S50, the target path length is calculated by integrating the target tangential speed (S52). Next, a new tangential acceleration for inversion control is calculated using the detection result detected in step S44, the target tangential velocity calculated in step S50, and the target path length calculated in step S52 (S54). ).
The new tangential acceleration calculated in step S52 is integrated to calculate a new tangential velocity (S56). Next, a speed ratio (new tangential speed / target tangential speed) between the target tangential speed calculated in step S50 and the new tangential speed calculated in step S56 is calculated (S58), and this speed ratio (new target tangential speed / target tangent). (Speed) is multiplied by the target angular velocity calculated in step S50 to correct the target angular velocity in the turning direction (S60).
Next, the new tangential velocity calculated in step S56 and the target angular velocity φ ′ calculated in step S60 are combined, and the combined result is distributed to the motors 12 and 14, and the target angular velocity of each motor 12 and 14 is calculated. (S62).
Next, the motors 22 and 24 are controlled so that the deviation between the target acceleration calculated in step S62 and the wheel rotation speed measured by the encoders 22a and 24a becomes zero (S64). As a result, the wheels 12 and 14 are driven. When step S64 ends, the process returns to step 42 and processing at the next control timing is started.

  As is apparent from the above description, the moving body of the third embodiment is controlled so that the moving body moves on the target route using the detection result of the position / posture sensor 86. Further, the target angular velocity in the turning direction is corrected in consideration of a delay in velocity response in the traveling direction. As a result, the mobile body can travel on the target route with high accuracy.

Specific examples of the present invention have been described in detail above, but these are merely examples and do not limit the scope of the claims. The technology described in the claims includes various modifications and changes of the specific examples illustrated above.
The technical elements described in this specification or the drawings exhibit technical usefulness alone or in various combinations, and are not limited to the combinations described in the claims at the time of filing. In addition, the technology illustrated in the present specification or the drawings achieves a plurality of objects at the same time, and has technical utility by achieving one of the objects.

10: moving body 12, 14: wheel 16: axle 20: vehicle body 22, 24: motor 22a, 24a: encoder 28: gyro sensor 30, 60, 88: control device

Claims (6)

An inverted pendulum type moving body that includes a wheel and a vehicle body supported by the wheel, and that runs on the road surface while rotating the wheel to maintain the vehicle body in an inverted state,
Means for calculating a first control command value for controlling the position of the moving body in the traveling direction while maintaining the vehicle body inversion using the target trajectory representing the target position of the moving body as a function of time;
Means for calculating a second control command value for controlling the angle of the turning direction of the moving body using the target trajectory and the information about the position in the traveling direction of the moving body that is actually measured or estimated;
An inverted pendulum type moving body comprising: means for driving a wheel based on a first control command value and a second control command value.   2. The inverted pendulum type according to claim 1, wherein the movable body travels on the road surface by repeatedly executing the first control command value calculating means, the second control command value calculating means, and the driving means at predetermined intervals. Moving body. A sensor for measuring information about the position of the moving body in the direction of travel;
The second control command value calculating means calculates “information about the position of the measured traveling direction” calculated from “target trajectory” and “information about the position of the moving body in the traveling direction obtained from the detection result of the sensor”. The inverted pendulum type moving body according to claim 2, wherein a second control command value for controlling an angle in a turning direction of the moving body is calculated based on a "target position of the corresponding moving body".   The second control command value calculating means is calculated from “target trajectory” and “information on the position of the moving body in the traveling direction estimated from the first control command value calculated by the first control command value calculating means”. The second control command value for controlling the angle of the turning direction of the moving body is calculated based on "the target position of the moving body corresponding to the information on the estimated position in the traveling direction". The inverted pendulum type moving body according to Item 2. A sensor for measuring the position of the moving body and the angle of the turning direction;
(1) Position deviation in the traveling direction of the moving body, (2) Position deviation in the direction perpendicular to the traveling direction of the moving body, and (3) Angular deviation in the turning direction of the moving body. And a tracking controller that calculates a target value related to the position of the moving body in the traveling direction and a target value related to the angle of the turning direction of the moving body,
The first control command value calculating means calculates a first control command value based on a target value related to the position of the moving body in the traveling direction calculated by the tracking controller,
The second control command value calculating means calculates the second control command value based on the target value calculated in the turning direction of the moving body calculated by the tracking controller and the calculated first control command value. The inverted pendulum type moving body according to claim 2. A wheel, a vehicle body supported by the wheel, a drive device that rotationally drives the wheel, and a control device that controls the drive device are provided, and the vehicle body is maintained in an inverted state by rotationally driving the wheel by the drive device. A method for controlling an inverted pendulum type moving body traveling on a road surface,
Calculating a first control command value for controlling the position of the moving body in the advancing direction while maintaining the vehicle body inverted using a target trajectory representing the target position of the moving body as a function of time;
A step of calculating a second control command value for controlling the angle of the turning direction of the moving body using the target trajectory and the information on the position of the moving body that is actually measured or estimated;
And driving the wheel based on the first control command value and the second control command value,
A control method for an inverted pendulum type moving body, characterized in that the moving body is controlled by repeatedly executing the above steps.

Images