JP5560234B2 - Center of gravity angle estimation method and inverted wheel type traveling body controlled by the method - Google Patents

Description

  The present invention relates to a method for estimating a composite barycentric angle of an occupant or a load and an inverted wheel type traveling body, and an inverted wheel type traveling body controlled by the method. More specifically, the present invention relates to a method for estimating a composite barycentric angle between a passenger or a load and an inverted wheel type traveling body in both an inverted state and an auxiliary wheel grounded state, and an inverted wheel type controlled by the method. Regarding the traveling body.

  Inverted wheel-type traveling bodies, particularly inverted wheel-type two-wheeled bodies, have been actively developed in recent years, for example, onboard-type inverted two-wheeled mobile robots, for example, because they occupy a small area and are easy to turn. (See, for example, Patent Document 1 and Patent Document 2). Such an inverted wheel type traveling body is controlled so as to move while maintaining a stable state by correcting the combined center of gravity of the occupant or the mounted object and the inverted wheel type traveling body by driving the driving wheel. In order to correct the combined center of gravity position of the occupant or the mounted object and the inverted wheel type traveling body, it is necessary to detect the combined center of gravity angle of the occupant or the mounted object and the inverted wheel type traveling body.

  For example, the position of the center of gravity and the angle of center of gravity of the traveling body vary greatly depending on the weight, physique, bag, and obstacles of the occupant, and the weight and shape of the load (for example, luggage). The center-of-gravity angle of the vehicle also varies greatly depending on the characteristics of the passenger and the load. Therefore, as described above, in order to correct the position of the center of gravity of the occupant or the load and the inverted wheel type traveling body and to correct the position of the center of gravity of the occupant or the load and the inverted wheel type traveling body, It is possible to measure the weight and center of gravity of a vehicle by attaching a sensor to a person or a mounted object, and calculate the composite center of gravity angle of the passenger or the mounted object and the inverted wheel type traveling body based on the obtained measurement value. Conceivable. As such a configuration, for example, it has been proposed to actually measure the barycentric angle using a sensing unit including a plurality of gyro sensors and a plurality of inclination sensors (see, for example, Patent Document 3). However, such countermeasures are not always preferable because there is a possibility of causing concerns such as an increase in the size and weight of the traveling body, an increase in manufacturing cost, and a complicated control system.

  On the other hand, instead of directly measuring the composite center-of-gravity angle of the occupant or the loaded object and the inverted wheel type traveling body as described above, for example, the acceleration / deceleration phenomenon of the traveling body is measured, and the magnitude of the speed deviation of the traveling body is measured. Attempts have also been made to achieve stable inverted traveling by correcting the target posture angle based on the above (see, for example, Patent Document 2). However, with such indirect feedback correction, there is a transitional state until a stable inverted traveling state is established, and for example, it is difficult to strictly control the braking distance. Has the problem of being difficult. In addition, it is difficult to accurately calculate the combined center-of-gravity angle of the occupant or the mounted object and the traveling body using a limited number of existing sensors.

  By the way, in the inverted wheel type traveling body, auxiliary wheels are provided before and after the driving body (drive wheel) in order to ensure stability when the passenger gets on and off and loads and unloads the load, and to prevent the vehicle from falling. (For example, refer to Patent Document 2). In this case, in a state where the auxiliary wheel is grounded (for example, in a waiting state for boarding or in a decelerating traveling state in which the auxiliary wheel is grounded, hereinafter referred to as an “auxiliary wheel grounded state”), the auxiliary wheel is separated from the floor. The floor reaction force received also affects the center of gravity angle of the traveling body. In addition, in order to make the transition from the auxiliary wheel grounding state to the state where the auxiliary wheel is not grounded and the traveling body is inverted only by the driving wheel (hereinafter referred to as “inverted state”) continuously and smoothly. Therefore, it is necessary to detect the composite barycentric angle of the occupant or the mounted object and the traveling body in the auxiliary wheel grounding state.

  As described above, in order to detect the combined center-of-gravity angle of the occupant or the loaded object and the traveling body in the auxiliary wheel grounding state, it is conceivable to provide a sensor or the like that detects the floor reaction force that the auxiliary wheel receives from the floor. However, as described above, such countermeasures are not always preferable because they may cause concerns such as an increase in the size and weight of the traveling body, an increase in manufacturing cost, and a complicated control system.

  Therefore, even when the auxiliary wheel is grounded, the floor reaction force is estimated without providing a sensor or the like for detecting the floor reaction force that the auxiliary wheel receives from the floor, and the combined barycentric angle of the occupant or the load and the traveling body is obtained. It is necessary to estimate. However, in the prior art, the center of gravity angle of the entire inverted wheel type traveling body in both the inverted state and the auxiliary wheel grounded state, particularly in the auxiliary wheel grounded state (the combined center of gravity angle of the occupant and the mounted object and the inverted wheel type traveling body) ) And ground reaction forces have not been attempted.

US Pat. No. 5,975,225 JP 2009-201321 A US Pat. No. 6,332,103

  As described above, in an inverted wheel type traveling body, while avoiding concerns such as an increase in the size and weight of the traveling body, an increase in manufacturing cost, and a complicated control system, the control performance of the inverted traveling is improved, and the auxiliary wheel is grounded. There is a need in the art to establish a method that can achieve a continuous and smooth transition from a state to an inverted state.

  That is, an object of the present invention is to use the minimum necessary sensors in an inverted wheel type traveling body, and the center-of-gravity angle of the entire inverted wheel type traveling body in both the inverted state and the auxiliary wheel grounding state (passengers and mounted objects). And an inverted wheel-type traveling body controlled by the same method.

The above purpose is
Driving wheels,
A main body supported by the axle of the drive wheel;
An arm part movably connected to the main body part;
An auxiliary wheel rotatably connected to the opposite side of the arm portion from the body portion;
Driving wheel driving means for driving the driving wheel;
Arm driving means for driving the arm portion;
Detecting means for detecting the actual state of the inverted wheel type traveling body;
Control means for controlling the drive wheel drive unit or the arm drive unit based on a deviation between the actual state and the target state detected by the detection unit to achieve the target state;
With
An inverted state in which the auxiliary wheel is not grounded and is inverted only by the driving wheel,
An auxiliary wheel grounding state in which the auxiliary wheel is grounded;
A center-of-gravity angle estimation method for the entire boardable inverted wheel type traveling body having:
Designing a state observer based on dynamic equations of motion for each of the inverted state and the auxiliary wheel grounding state;
Introducing an affine term into the state equation of the state observer for the auxiliary wheel grounding state;
Using the state observer for the inverted state, the variation (Δη) and disturbance (w) of the center-of-gravity angle of the entire inverted wheel type traveling body due to the mass and the position of the center of gravity of the passenger or the load in the inverted state And using the state observer for the auxiliary wheel grounding state, the center of gravity angle of the entire inverted wheel type traveling body due to the mass and the position of the center of gravity of the passenger or the load in the auxiliary wheel grounding state Estimating the variation (Δη), the floor reaction force (f _c ) that the auxiliary wheel receives from the floor, and the disturbance (w),
This is achieved by the method of estimating the center-of-gravity angle of the entire inverted wheel type traveling body.

The above purpose is
Driving wheels,
A main body supported by the axle of the drive wheel;
An arm part movably connected to the main body part;
An auxiliary wheel rotatably connected to the opposite side of the arm portion from the body portion;
Driving wheel driving means for driving the driving wheel;
Arm driving means for driving the arm portion;
Detecting means for detecting the actual state of the inverted wheel type traveling body;
Control means for controlling the drive wheel drive unit or the arm drive unit based on a deviation between the actual state and the target state detected by the detection unit to achieve the target state;
With
An inverted state in which the auxiliary wheel is not grounded and is inverted only by the driving wheel,
An auxiliary wheel grounding state in which the auxiliary wheel is grounded;
An inverted wheel-type traveling body capable of boarding,
The control means is
Designing a state observer based on dynamic equations of motion for each of the inverted state and the auxiliary wheel grounding state;
Introducing an affine term into the state equation of the state observer for the auxiliary wheel grounding state;
Using the state observer for the inverted state, the variation (Δη) and disturbance (w) of the center-of-gravity angle of the entire inverted wheel type traveling body due to the mass and the position of the center of gravity of the passenger or the load in the inverted state And using the state observer for the auxiliary wheel grounding state, the center of gravity angle of the entire inverted wheel type traveling body due to the mass and the position of the center of gravity of the passenger or the load in the auxiliary wheel grounding state Estimating the variation (Δη), the floor reaction force (f _c ) that the auxiliary wheel receives from the floor, and the disturbance (w),
This is achieved by an inverted wheel type vehicle.

Changes in the center-of-gravity angle of the entire inverted wheel type traveling body due to the weight and the position of the center of gravity of the occupant or the loaded object in the inverted state, without adding a new sensing unit, etc. (Δη) and disturbance (w) are estimated, and the variation of the center-of-gravity angle of the entire inverted wheel-type traveling body (Δη) due to the mass and the position of the center of gravity of the occupant or mounted object in the auxiliary wheel grounding state, the auxiliary wheel It is possible to estimate the floor reaction force (f _c ) and disturbance (w) received from the floor.

  As described above, in the inverted wheel type traveling body, the control of the inverted traveling is avoided using the minimum necessary sensors while avoiding concerns such as an increase in the size and weight of the traveling body, an increase in manufacturing cost, and a complicated control system. Improvement of performance, realization of continuous and smooth transition from the auxiliary wheel grounding state to the inverted state, and the like can be achieved.

It is the schematic which shows the structure of the inverted wheel type traveling body which concerns on one embodiment of this invention in an inverted state. It is the schematic which shows the structure of the inverted wheel type traveling body which concerns on one embodiment of this invention in an auxiliary wheel grounding state. It is a block diagram showing the feedback system which makes the state variable vector estimated value by the state observer about the inverted wheel type traveling body which concerns on one embodiment of this invention in an inverted state converge on a real state. It is a block diagram showing the feedback system which converges the state variable vector estimated value by the state observer about the inverted wheel type traveling body which concerns on one embodiment of this invention in an auxiliary wheel grounding state to a real state.

The present invention relates to an inverted wheel-type traveling body that can be boarded, and the center-of-gravity angle of the entire inverted wheel-type traveling body caused by the mass and position of the passenger or the load in the inverted state without adding a new sensing unit or the like. Variation (Δη) and disturbance (w), and the variation of the center-of-gravity angle of the entire inverted wheel type traveling body (Δη) due to the mass and position of the passenger or the load when the auxiliary wheel is in contact with the ground, the auxiliary wheel is the floor An object of the present invention is to provide a method for estimating the floor reaction force (f _c ) and disturbance (w) received from the motor.

As described above, attempts have been made to estimate the center-of-gravity angle (composite center-of-gravity angle of the occupant or the loaded object and the inverted wheel type traveling body) and the floor reaction force in the auxiliary wheel grounded state. Absent. However, as a result of diligent research to achieve the above object, the present inventor has introduced an affine term into the state equation of the state observer for the auxiliary wheel grounding state, thereby using the minimum necessary sensor to ground the auxiliary wheel. Not only the center-of-gravity angle of the entire inverted wheel-type traveling body in the state (the composite center-of-gravity angle of the occupant or the load and the inverted wheel-type traveling body), but also the floor reaction force (f _c ) that the auxiliary wheel receives from the floor when the auxiliary wheel is grounded ) Can also be estimated.

That is, the first aspect of the present invention is:
Driving wheels,
A main body supported by the axle of the drive wheel;
An arm part movably connected to the main body part;
An auxiliary wheel rotatably connected to the opposite side of the arm portion from the body portion;
Driving wheel driving means for driving the driving wheel;
Arm driving means for driving the arm portion;
Detecting means for detecting the actual state of the inverted wheel type traveling body;
Control means for controlling the drive wheel drive unit or the arm drive unit based on a deviation between the actual state and the target state detected by the detection unit to achieve the target state;
With
An inverted state in which the auxiliary wheel is not grounded and is inverted only by the driving wheel,
An auxiliary wheel grounding state in which the auxiliary wheel is grounded;
A center-of-gravity angle estimation method for the entire boardable inverted wheel type traveling body having:
Designing a state observer based on dynamic equations of motion for each of the inverted state and the auxiliary wheel grounding state;
Introducing an affine term into the state equation of the state observer for the auxiliary wheel grounding state;
Using the state observer for the inverted state, the variation (Δη) and disturbance (w) of the center-of-gravity angle of the entire inverted wheel type traveling body due to the mass and the position of the center of gravity of the passenger or the load in the inverted state And using the state observer for the auxiliary wheel grounding state, the center of gravity angle of the entire inverted wheel type traveling body due to the mass and the position of the center of gravity of the passenger or the load in the auxiliary wheel grounding state Estimating the variation (Δη), the floor reaction force (f _c ) that the auxiliary wheel receives from the floor, and the disturbance (w),
This is a method for estimating the center-of-gravity angle of the entire inverted wheel type traveling body.

  The drive wheels are configured as a pair of wheels arranged symmetrically on the same rotation axis (that is, having a coaxial rotation axis). The main body portion of the inverted wheel type traveling body is supported by the axle of the drive wheel in a state where it can tilt in a direction orthogonal to the rotation axis of the drive wheel. Drive wheel drive means for driving the drive wheels is connected to each of the left and right drive wheels. As the driving wheel driving means, various driving devices widely used in the technical field can be used. For example, a driving device including an electric motor or the like as a power source can be used.

  By the way, the inverted wheel type traveling body according to the present invention is a traveling body on which a passenger can board. Of course, it is also possible to mount a load on the inverted wheel type traveling body according to the present invention. Therefore, for example, the main body of the inverted wheel type traveling body according to the present invention includes a mechanism (for example, a seat, a loading platform, etc.) for the passenger to board or to mount a load.

  Further, the arm portion is movably connected to the main body portion, and an auxiliary wheel is rotatably connected to the other end (the opposite side of the main body portion) of the arm portion. In the arm portion, the auxiliary wheel is grounded to the floor in the grounded state of the auxiliary wheel, and supports the inverted wheel type traveling body together with the drive wheel, while the auxiliary wheel is not grounded to the floor in the inverted state (that is, It does not contribute to the support of the inverted wheel type traveling body), and is controlled by the arm driving means so that only the driving wheel supports the inverted wheel type traveling body. As the arm driving means, various driving devices widely used in the technical field can be used. For example, a driving device including a servo motor or the like as a power source can be used.

  The detection means for detecting the actual state of the inverted wheel type traveling body includes, for example, an encoder for detecting the rotation angle and rotation angular velocity of each drive device (for example, a motor constituting the drive wheel drive means and the arm drive means), and the main body And a gyro sensor for detecting the pitch angle and pitch angular velocity of the part. The sensor for measuring the pitch angle and pitch angular velocity of the main body is not limited to the gyro sensor, and can be used for measuring the pitch angle and pitch angular velocity, for example, a gravitational acceleration sensor or a weight suspension type tilt angle meter. Various instruments can be used. Since the configuration of the inverted wheel type traveling body is known, the barycentric angle of the main body of the inverted wheel type traveling body can be obtained based on the pitch angle detected by such a sensor.

  As described above, such an inverted wheel type traveling body is controlled so as to move while maintaining a stable state by correcting the position of the center of gravity of the occupant or the loaded object and the inverted wheel type traveling body by driving the driving wheel. The In order to correct the center-of-gravity position, the detection means, for example, as the actual state of the inverted wheel type traveling body, for example, the rotation angle and rotation angular velocity of the motor constituting the driving wheel driving means and the arm driving means, the main body portion The pitch angle, pitch angular velocity, etc. are detected. From the pitch angle and pitch angular velocity thus detected, the center of gravity angle and the center of gravity angular velocity of the main body of the inverted wheel type traveling body are calculated.

  The actual state of the inverted wheel type traveling body detected by the detection means as described above is sent to the control means as an output signal sent from the detection means, for example. The control means controls the drive wheel drive unit or the arm drive unit based on the deviation between the actual state and the target state detected by the detection unit to achieve the target state. The control means includes, for example, a control computer, and the control computer includes, for example, a CPU, a ROM, a RAM, and the like. For example, the control computer executes a control program stored in the ROM, calculates a deviation between the actual state of the traveling body and the target state based on the output signal sent from the detection means, A control command value for achieving the state is calculated. The control command value calculated by the control computer is output to the drive wheel drive unit or the arm drive unit, and the drive wheel or arm unit is controlled based on the control command value.

  As described above, in order to achieve the target state in the inverted wheel type traveling body, it is necessary to detect the center of gravity angle or the like of the entire inverted wheel type traveling body. However, as described above, the inverted wheel type traveling body according to the present invention is a traveling body on which a passenger can board. The center-of-gravity position and center-of-gravity angle of such a riding-type traveling body greatly vary depending on the weight, physique, heel, obstacle, and the like of the occupant and the weight and shape of the loaded object. Therefore, in order to detect the center of gravity angle of the entire inverted wheel type traveling body and correct the center of gravity position as described above, the weight and the center of gravity position are measured by attaching a sensor to the passenger or the load. It is conceivable to calculate the barycentric angle based on the obtained measurement value.

  However, countermeasures such as the addition of a new sensing unit as described above are not always preferable because they may cause concerns such as an increase in the size and weight of the traveling body, an increase in manufacturing costs, and a complicated control system. Absent. Moreover, it is not realistic and not preferable to attach a complicated sensor to a passenger or a load. Therefore, in the center-of-gravity angle estimation method for the entire inverted wheel type traveling body according to the present invention, based on the actual state detected by the existing detection means as described above without adding a new sensing unit, The fluctuation of the center of gravity caused by the mounted object is estimated using a state observer, and the center of gravity of the entire inverted wheel type traveling body is detected.

  Further, as described above, in the inverted wheel type traveling body according to the present invention, the auxiliary wheel does not contact the floor (that is, does not contribute to the support of the inverted wheel type traveling body), and only the driving wheel is the inverted wheel. There are two states: an inverted state that supports the traveling type body, and an auxiliary wheel grounding state that supports the inverted wheel type traveling body together with the driving wheels while the auxiliary wheels are grounded to the floor surface.

  In the former inverted state, various disturbances (for example, disturbances caused by physical parameter errors and linearization errors) also affect the driving wheels of the traveling body and the equations of motion of the main body. On the other hand, in the latter auxiliary wheel grounding state, the floor reaction force that the auxiliary wheel receives from the floor also affects the gravity center angle of the traveling body. In order to improve the stability in the auxiliary wheel grounding state and to make the transition from the auxiliary wheel grounding state to the inverted state continuous and smooth, the passenger, the load and the traveling body in the auxiliary wheel grounding state It is necessary to detect the composite centroid angle. Therefore, in order to detect the composite barycentric angle of the passenger or the load and the traveling body in the auxiliary wheel grounding state, it is necessary to detect the floor reaction force that the auxiliary wheel receives from the floor. However, as described above, adding a new sensing unit or the like for detecting a floor reaction force causes concerns such as an increase in the size and weight of the traveling body, an increase in manufacturing cost, and a complicated control system. It is not preferable because of fear.

  Therefore, in the center of gravity angle estimation method for the entire inverted wheel type traveling body according to the present invention, the actual state detected by the existing detection means as described above without adding a new sensing unit such as a floor reaction force sensor. Based on the above, the state reaction force and disturbance that the auxiliary wheel receives from the floor in the inverted state and the auxiliary wheel grounding state are estimated using the state observer, and the center-of-gravity angle of the entire inverted wheel type traveling body is detected.

  As described above, in the technical field, various attempts have been proposed as a method for controlling an inverted wheel type traveling body. However, like the method of estimating the center of gravity of the entire inverted wheel type traveling body according to the present invention. Estimate the center-of-gravity angle (composite center-of-gravity angle of the occupant and the mounted object and the inverted wheel-type traveling body) and the floor reaction force when the auxiliary wheel is in contact with the ground. No attempt has been made to estimate the combined barycentric angle.

  However, in the method of estimating the center-of-gravity angle of the entire inverted wheel type traveling body according to the present invention, the state observer is designed based on the equation of motion of the inverted wheel type traveling body in the inverted state and the auxiliary wheel grounding state. For the inverted state, using the observer for the inverted state, the fluctuation (Δη) and disturbance (w) of the center-of-gravity angle of the entire inverted wheel type traveling body due to the mass and position of the passenger or the load in the inverted state presume.

On the other hand, for the auxiliary wheel grounding state, as described above, the affine term is introduced into the state equation of the observer for the auxiliary wheel grounding state. Thus, based on the actual state detected by the existing detection means as described above, the variation of the center-of-gravity angle of the entire inverted wheel type traveling body due to the mass and position of the passenger or the mounted object in the auxiliary wheel grounding state It is possible to estimate not only (Δη) but also the floor reaction force (f _c ) and disturbance (w) that the auxiliary wheel receives from the floor. That is, the affine term is introduced into the state observer in order to reflect the influence of gravity, more specifically, the influence of the floor reaction force that the auxiliary wheel receives from the floor.

As described above, in this technical field, a model that introduces an affine term into the state equation of the state observer for the auxiliary wheel grounding state as in the method for estimating the center of gravity angle of the entire inverted wheel type traveling body according to the present invention has hitherto been used. Not proposed. In the method of estimating the center-of-gravity angle of the entire inverted wheel type traveling body according to the present invention, it is possible to estimate the floor reaction force (f _c ) that the auxiliary wheel receives from the floor when the auxiliary wheel is grounded by introducing the affine term. It became. Thereby, in the center-of-gravity angle estimation method for the entire inverted wheel type traveling body according to the present invention, the inverted wheel type traveling caused by the mass and position of the passenger or the mounted object in the inverted state using the state observer for the inverted state. In addition to being able to estimate the fluctuation (Δη) and disturbance (w) of the center-of-gravity angle of the entire body, boarding in the auxiliary wheel grounding state using the state observer for the auxiliary wheel grounding state that introduced the affine term The variation of the center-of-gravity angle (Δη) of the entire inverted wheel type traveling body caused by the mass and position of the person or the load, the floor reaction force (f _c ) that the auxiliary wheel receives from the floor, and the disturbance (w) are also estimated. I was able to do it.

Therefore, according to the center-of-gravity angle estimation method for the entire inverted wheel type traveling body according to the present invention, in the inverted wheel type traveling body that can be boarded, the inverted state and the auxiliary wheel grounding state can be provided without adding a new sensing unit or the like. The variation of the center of gravity angle (Δη) and disturbance (w) of the entire inverted wheel type traveling body due to the mass and position of the passenger or the load in both states, and the floor that the auxiliary wheel receives from the floor when the auxiliary wheel is grounded The reaction force (f _c ) is estimated, and the center-of-gravity angle of the entire inverted wheel type traveling body can be controlled through both the inverted state and the auxiliary wheel grounding state.

  As a result, according to the center-of-gravity angle estimation method for the entire inverted wheel type traveling body according to the present invention, in the inverted wheel type traveling body, the required size of the traveling body is increased, the weight is increased, and the manufacturing cost is reduced. While avoiding concerns such as an increase and complication of the control system, it is possible to improve the control performance of the inverted traveling, achieve a continuous and smooth transition from the auxiliary wheel grounding state to the inverted state, and the like. Further, according to the center-of-gravity angle estimation method for the entire inverted wheel type traveling body according to the present invention, the variation (Δη) of the center-of-gravity angle of the entire inverted wheel type traveling body can be estimated. In addition, by detecting the change in the center of gravity angle of the entire inverted wheel type traveling body due to dangerous phenomena such as catching on the loaded object, it is possible to prevent the fall of the inverted wheel type traveling object, the fall of the passenger or the loaded object, etc. You can also.

  By the way, as described above, in the method of estimating the center-of-gravity angle of the entire inverted wheel type traveling body according to the present invention, the state observer is designed based on the dynamic equation of motion for each of the inverted state and the auxiliary wheel grounding state. . Here, the outline of the design of the state observer for each of the inverted state and the auxiliary wheel grounding state will be described (details will be described later).

  In one embodiment of the present invention, first, it is assumed that an equation of motion in an inverted state of an inverted wheel type traveling body according to this embodiment is expressed by a state equation and an output equation shown in the following equation (8).

  In the above formula,

である。

It is.

  Based on the state equation and the output equation, the state observer in the inverted state of the inverted wheel type traveling body according to the present embodiment is represented by the state equation and the output equation shown in the following equation (9).

  In the above formula,

It is.

  On the other hand, it is assumed that the equation of motion of the inverted wheel type traveling body according to this embodiment in the grounded state of the auxiliary wheel is represented by the state equation and the output equation shown in the following equation (26).

  In the above formula,

It is.

  Based on the state equation and the output equation, the state observer in the auxiliary wheel grounding state of the inverted wheel type traveling body according to the present embodiment is represented by the state equation and the output equation shown in the following equation (27).

  In the above formula,

It is.

  As described above, in the present embodiment, the state observer in the inverted state and the auxiliary wheel grounding state of the inverted wheel type traveling body is expressed by the state equation and the output equation shown in the equations (9) and (27), respectively. Since the observable matrix composed of the system matrix and the output matrix included in Equation (9) and Equation (27) are both full rank (details will be described later), the observer gain is appropriately set in each state observer. Thus, the state variable vector estimation value in each state observer can obtain an asymptotic convergence value to the real state.

In other words, in the center-of-gravity angle estimation method for the entire inverted wheel type traveling body according to the present embodiment, the inverted state and the auxiliary wheel grounding represented by the state equation and the output equation shown in Equation (9) and Equation (27), respectively. Using the state observer in the state, the variation (Δη) and disturbance (w) of the center-of-gravity angle of the entire inverted wheel type traveling body due to the mass and position of the occupant or the loaded object in the inverted state and the auxiliary wheel ground state, and The floor reaction force (f _c ) that the auxiliary wheel receives from the floor when the auxiliary wheel is in contact with the ground is estimated.

That is, the second aspect of the present invention is
The center-of-gravity angle estimation method for the entire inverted wheel type traveling body according to the first aspect, wherein the state observer for the inverted state is represented by a state equation and an output equation represented by the following equation (9):

  In the above formula,

And
On the other hand, the state observer for the auxiliary wheel grounding state is represented by a state equation and an output equation shown in the following equation (27),

  In the above formula,

This is a method for estimating the center-of-gravity angle of the entire inverted wheel type traveling body.

As described above, in the center-of-gravity angle estimation method for the entire inverted wheel type traveling body according to the present invention, the variation of the center-of-gravity angle of the entire inverted wheel type traveling body due to the mass and position of the passenger or the load (Δη ), Disturbance (w), and floor reaction force (f _c ) that the auxiliary wheel receives from the floor based on known observations. That is, the state variable vector of the equation of motion of the inverted wheel type traveling body according to the present invention includes a known variable and an unknown variable. In addition, as a known observation value, various parameters can be selected according to the kind of detection means with which the target inverted wheel type traveling body is provided.

  In one embodiment of the present invention, the state variable vector of the equation of motion of the inverted wheel type traveling body in the inverted state includes, for example, the center-of-gravity angle of the inverted wheel type traveling body before the rider or the mounted object rides, In addition to the observed values of the center of gravity angular velocity of the inverted wheel type traveling body and the rotational angular velocity of the drive wheel before the passenger or the loaded object rides, the center of gravity angle of the entire inverted wheel type traveling body (passenger, loaded object and inverted wheel type traveling) An unknown component such as a variation (Δη) and a disturbance (w) from the center of gravity angle of the inverted wheel type traveling body before the rider or the mounted object on the composite body center of gravity angle) is included. Here, as the disturbance, for example, a disturbance due to a physical parameter error or a linearization error is assumed.

Further, the state variable vector of the equation of motion of the inverted wheel type traveling body in the auxiliary wheel grounding state includes, for example, the gravity center angular velocity of the inverted wheel type traveling body before the rider or the mounted object rides, the rotational angular velocity of the driving wheel, and In addition to the observation value of the rotation angle (q) of the auxiliary wheel arm (arm part), the centroid angle of the entire inverted wheel type traveling body (the occupant and the occupant of the loaded object and the inverted wheel type traveling body) And an unknown component such as a variation (Δη) from the center-of-gravity angle of the inverted wheel type traveling body before the mounted object is on and a floor reaction force (f _c ) that the auxiliary wheel receives from the floor when the auxiliary wheel is grounded.

That is, the third aspect of the present invention is
A method of estimating a center-of-gravity angle of the entire inverted wheel type traveling body according to the second aspect, wherein the Δη and the disturbance w are estimated using the following equation (7) as a state variable vector in the inverted state, and the auxiliary In the wheel contact state, the Δη and the f _c and the disturbance w are estimated using the following equation (25) as a state variable vector:

  In the above formula,

This is a method for estimating the center-of-gravity angle of the entire inverted wheel type traveling body.

  As described above, according to the center-of-gravity angle estimation method for the entire inverted wheel type traveling body according to the first to third aspects of the present invention, in the inverted wheel type traveling body, using the minimum necessary sensors, While avoiding concerns such as increased size and weight of the vehicle, increased manufacturing costs, and complicated control systems, improved control performance of inverted driving, continuous and smooth transition from auxiliary wheel grounding to inverted state It is possible to achieve the above.

  Further, the center of gravity angle estimation method for the entire inverted wheel type traveling body according to any one of the first to third aspects of the present invention estimates the variation (Δη) of the center of gravity angle of the entire inverted wheel type traveling body. Therefore, it is also effective as a means for detecting a dangerous phenomenon accompanied by fluctuation of the barycentric angle. More specifically, for example, the variation (Δη) from the center of gravity of the main body of the center of gravity of the entire inverted wheel type traveling body (the combined center of gravity of the occupant or the mounted object and the inverted wheel type traveling body) is previously determined. By issuing a warning and informing the passenger of the occurrence of a dangerous phenomenon when the value is larger than the predetermined value, it is possible to obviate an unfavorable situation such as a fall of an inverted wheel type traveling body or a drop of the passenger or the load. Can be prevented. In addition, as said dangerous phenomenon, a passenger's going out, the catch of a load, etc. are mentioned, for example.

That is, the fourth aspect of the present invention is
A method for estimating the center-of-gravity angle of the entire inverted wheel type traveling body according to any one of the first to third aspects, wherein a warning is issued when the Δη is greater than a predetermined value. This is a method of estimating the center-of-gravity angle of the entire inverted wheel type traveling body.

  The warning means include, for example, those that appeal to hearing such as a buzzer or sound, those that appeal to the visual sense such as a display on an operation screen used by the passenger, vibrations of the passenger's seat or wearing equipment, etc. However, it is not limited to a specific configuration.

  By the way, in the inverted wheel type traveling body controlled by the center-of-gravity angle estimation method for the entire inverted wheel type traveling body according to the first to fourth aspects of the present invention, as described above, the minimum necessary sensors are provided. By using it, avoiding concerns such as larger and heavier running bodies, increased manufacturing costs, and complicated control systems, while improving the control performance of inverted traveling, continuously from the auxiliary wheel grounding state to the inverted state It becomes possible to achieve smooth transition and the like. In addition, as described above, the traveling body has means for detecting a dangerous phenomenon accompanied by fluctuations in the barycentric angle, so that it can be a safer traveling body.

That is, the fifth aspect of the present invention is
Driving wheels,
A main body supported by the axle of the drive wheel;
An arm part movably connected to the main body part;
An auxiliary wheel rotatably connected to the opposite side of the arm portion from the body portion;
Driving wheel driving means for driving the driving wheel;
Arm driving means for driving the arm portion;
Detecting means for detecting the actual state of the inverted wheel type traveling body;
Control means for controlling the drive wheel drive unit or the arm drive unit based on a deviation between the actual state and the target state detected by the detection unit to achieve the target state;
With
An inverted state in which the auxiliary wheel is not grounded and is inverted only by the driving wheel,
An auxiliary wheel grounding state in which the auxiliary wheel is grounded;
An inverted wheel-type traveling body capable of boarding,
The control means is
Designing a state observer based on dynamic equations of motion for each of the inverted state and the auxiliary wheel grounding state;
Introducing an affine term into the state equation of the state observer for the auxiliary wheel grounding state;
Using the state observer for the inverted state, the variation (Δη) and disturbance (w) of the center-of-gravity angle of the entire inverted wheel type traveling body due to the mass and the position of the center of gravity of the passenger or the load in the inverted state And using the state observer for the auxiliary wheel grounding state, the center of gravity angle of the entire inverted wheel type traveling body due to the mass and the position of the center of gravity of the passenger or the load in the auxiliary wheel grounding state Estimating a variation (Δη), a floor reaction force (f _c ) that the auxiliary wheel receives from the floor, and a disturbance (w);
An inverted wheel type traveling body characterized by the above.

The sixth aspect of the present invention is
The inverted wheel type traveling body according to the fifth aspect, wherein the state observer for the inverted state is represented by a state equation and an output equation shown in the following equation (9):

  In the above formula,

And
On the other hand, the state observer for the auxiliary wheel grounding state is represented by a state equation and an output equation shown in the following equation (27),

  In the above formula,

This is an inverted wheel type traveling body.

Furthermore, the seventh aspect of the present invention provides
In the inverted wheel type traveling body according to the sixth aspect, the Δη and the disturbance w are estimated using the following equation (7) as a state variable vector in the inverted state, and the state in the auxiliary wheel grounding state Estimating the Δη, the f _c , and the disturbance w using the following equation (25) as a variable vector:

  In the above formula,

This is an inverted wheel type traveling body.

Still further, the eighth aspect of the present invention provides:
The inverted wheel type traveling body according to any one of the fifth aspect to the seventh aspect, further comprising barycentric angle warning means for issuing a warning when the Δη is larger than a predetermined value. And an inverted wheel type traveling body.

  The configuration of the inverted wheel type traveling body according to any one of the fifth aspect to the eighth aspect described above, the estimation method of the center-of-gravity angle and the like and the warning method of the dangerous state executed in the control of the traveling body are as described above. Since it is the same as that described in the description regarding the first aspect to the fourth aspect, description thereof is omitted here.

As described above, according to the present invention, in an inverted wheel-type traveling body that can be boarded, without the addition of a new sensing unit or the like, the vehicle is inverted due to the mass and the position of the center of gravity of the passenger or the load in the inverted state. Estimating the variation (Δη) and disturbance (w) of the center-of-gravity angle of the entire wheel-type traveling body, and the entire inverted wheel-type traveling body due to the mass and the position of the center of gravity of the occupant or the load in the grounded auxiliary wheel It is possible to estimate the variation of the barycentric angle (Δη), the floor reaction force (f _c ) that the auxiliary wheel receives from the floor, and the disturbance (w).

  As described above, in the inverted wheel type traveling body, the control of the inverted traveling is avoided using the minimum necessary sensors while avoiding concerns such as an increase in the size and weight of the traveling body, an increase in manufacturing cost, and a complicated control system. Advantages such as improvement of performance, realization of continuous and smooth transition from the auxiliary wheel grounding state to the inverted state, and prevention of a dangerous phenomenon accompanied by fluctuation of the center of gravity angle can be achieved.

  Hereinafter, the center-of-gravity angle estimation method for the entire inverted wheel type traveling body according to a specific embodiment of the present invention will be described with reference to the accompanying drawings. However, the following description is for illustrative purposes only, and the scope of the present invention should not be construed as being limited to the following description.

  First, in the present embodiment, an inverted wheel type traveling body to which the center-of-gravity angle estimation method for the entire inverted wheel type traveling body according to a specific embodiment of the present invention is applied will be described. 1 and 2 are schematic views showing the configuration of an inverted wheel type traveling body according to one embodiment of the present invention, which is in an inverted state and an auxiliary wheel grounding state, respectively, as described above.

A1. It explained estimation of the center of gravity angle of the entire inverted wheel type traveling body at the center of gravity angle inverted state of the entire inverted wheel type traveling body in the inverted state. First, the center-of-gravity angle of the entire inverted wheel type traveling body in the inverted state will be described in detail. As shown in FIG. 1, the main body 120 of the inverted wheel type traveling body 100 according to this embodiment includes a horizontal portion that extends in a substantially horizontal direction through the axle 111 of the drive wheel 110 and a vertical portion that extends in a substantially vertical direction from the axle 111. It is schematically represented by an inverted T-shaped figure made up of parts, and is hatched. As described above, the main body 120 is supported by the axle 111 of the drive wheel 110. A circle mark m ₀ drawn in the middle of the vertical portion represents the center of gravity of the main body 120 of the inverted wheel type traveling body 100.

  As described above, the arm portions 131 and 132 having the auxiliary wheels 141 and 142 at the other end are connected to both ends of the horizontal portion via the connecting portions 133 and 134, respectively. Since the inverted wheel type traveling body 100 shown in FIG. 1 is in an inverted state, the arm portions 131 and 132 are held at an angle at which the auxiliary wheels 141 and 142 included in the arm portions 131 and 132 are not grounded.

  The inverted wheel type traveling body 100 according to the present embodiment will be described on the assumption that the passenger 150 is boarded as described above. However, the inverted wheel type traveling body according to another embodiment is provided with the above riding means. Alternatively, or in addition to the boarding means, a loading means for loading a load such as luggage can be provided. Moreover, in the inverted wheel type traveling body 100 according to the present embodiment, as described above, the auxiliary wheel and the arm portion that support the main body 120 together with the driving wheel 110 are in the inverted wheel type traveling body 100 in the grounded auxiliary wheel state. It is provided not only in the traveling direction (indicated by a gray arrow in the figure) but also on the opposite side of the traveling direction. However, in the inverted wheel type traveling body according to another embodiment, the auxiliary wheel and the arm portion may be provided only on either one of the front and rear sides of the traveling direction of the inverted wheel type traveling body.

Further, the inverted wheel type traveling body 100 according to the present embodiment can be boarded by a passenger, and the passenger 150 is schematically represented as a bar-like figure linked to the upper end of the vertical portion via a link 151. Yes. Actually, the passenger 150 gets on the inverted wheel type traveling body 100 according to the present embodiment by sitting on a boarding means (not shown) such as a passenger seat provided in the main body 120. . Note that a circle m _h drawn on the other end of the rod-shaped figure represents the center of gravity of the passenger 150 (the mass of the passenger 150 is m _h ).

Another circle m ₁ shown in FIG. 1 indicates the center of gravity m ₀ of the main body portion of the inverted wheel type traveling body 100 (the mass of the main body portion 120 of the inverted wheel type traveling body 100 is m ₀ ) and the center of gravity m of the passenger 150. Represents the combined center of gravity with _h . That is, the total mass of the inverted wheel type traveling body 100 main body 120 and the occupant 150 is m ₁ and is located at the circle m ₁ . As shown in FIG. 1, the center of gravity m ₀ of the main body 120 of the inverted wheel type traveling body 100 is on a line inclined by η from the vertical direction (indicated by a dotted line). That is, the center of gravity angle of the main body 120 of the inverted wheel type traveling body 100 is η. When an occupant 150 having a center of gravity m _h rides there at an attitude angle η _h , the center of gravity of the inverted wheel type traveling body 100 changes from the circle mark m ₀ to the circle mark m ₁ , and the inverted wheel type traveling body. The centroid angle of 100 varied from η by Δη to become a combined centroid angle (η + Δη).

A2. Principle of Estimating Center of Gravity Angle of Whole Inverted Wheel Type Traveling Body in Inverted State The center of gravity m _{h of the} occupant 150 and the variation Δη of the centroid angle of the inverted wheel type traveling body 100 due to the passenger 150 getting on are unknown. The combined barycentric angle (η + Δη) between the main body 120 of the inverted wheel type traveling body 100 and the occupant 150 affects the movement of the inverted wheel type traveling body 100 through the equation of motion. Using this fact, the unknown center-of-gravity angle variation Δη (and disturbance w) is estimated from the equation of motion of the inverted wheel type traveling body 100 and the limited observation value.

A3. Derivation of the equation of motion of the entire inverted wheel type traveling body in the inverted state In the inverted state, the wheel type 1 link inverted pendulum having the center of mass m ₁ of the main body 120 of the inverted wheel traveling body 100 and the passenger 150 as the center of mass. The inverted wheel type traveling body 100 is modeled as follows. The equation of motion of the wheel type 1 link inverted pendulum is given by the following equation (1).

In the above formula,

Here, η: the center of gravity angle of the inverted wheel type traveling body 100, θ: the rotation angle of the drive wheel 110, Δη: the variation of the center of gravity angle of the inverted wheel type traveling body 100, m _l : boarding with the inverted wheel type traveling body 100 Total mass with the passenger 150, m ₂ : mass of the driving wheel 110, l: distance of the composite center of gravity from the axle 111, I ₁ : moment of inertia around the axle 111 between the inverted wheel type traveling body 100 and the passenger 150, J _ml : moment of inertia of rotor of drive wheel drive motor (not shown), J ₂ : moment of inertia around drive wheel 110 axis, n ₁ : gear ratio of drive wheel drive mechanism (motor: drive wheel = 1: n ₁ ), R: radius of the drive wheel 110, f ₁ : coefficient of viscous friction between the axle 111 and the main body 120, u _l : drive torque value of the drive wheel drive motor, g ₀ : gravity acceleration, φ: mass / center of gravity From distance, moment of inertia, friction coefficient, etc. Physical parameter vector that is.

  Next, linear approximation of the nonlinear equation of Expression (1) is performed in the vicinity of the point where the barycentric angle is 0. First, assuming that there is no sudden change in the centroid angle near the point where the centroid angle is 0,

It can be. Therefore, the equation of motion of the inverted wheel type traveling body 100 represented by the equation (1) can be approximately expressed by the following equation (2).

  In the above formula,

Furthermore, since M _r (φ) is regular, the equation (2) can be rearranged as the following equation (3).

  In the above formula,

From the equation (3), it can be seen that the inverted wheel type traveling body 100 according to the present embodiment performs the inverted traveling motion along the equation (3) by applying the driving torque u ₁ of the driving wheel 110. Here, the variable Δη included in the right side of the equation (3) is a variation (Δη) of the center-of-gravity angle of the entire inverted wheel type traveling body 100 due to the mass of the passenger 150 and the center-of-gravity position (m _h ). Note that it is an unknown value that is not detected by a detection means such as an existing sensor.

Incidentally, as described above, the inverted wheel type traveling body 100 according to the present embodiment is provided with a minimum necessary sensor as a detecting means (not shown). Furthermore, (not shown) control means inverted wheel type traveling body 100 is provided, according to the observation values obtained from the detection means (sensor), than to generate an appropriate control input u _l, stable inverted motion control It can be performed. In general, the observed value is based on the center of gravity angle of the inverted wheel type traveling body 100 before the rider or the load is on the gravity acceleration direction (vertical direction) (the pitch angle of the main body 120 of the inverted wheel type traveling body 100). Obtained), the center-of-gravity angular velocity of the inverted wheel type traveling body 100 (obtained from the pitch angular velocity of the main body portion 120 of the inverted wheel type traveling body 100), and the rotational angular velocity of the driving wheel 110 with respect to the main body portion 120 (inverted wheel type traveling body 100). And can be described by the following vectors:

  The gravity center angular velocity of the main body 120 of the inverted wheel type traveling body 100, which is the second component on the right side of the equation (4), can be detected by, for example, a gyro sensor installed on the inverted wheel type traveling body 100. By integrating the detected barycentric angular velocity of the main body 120, the barycentric angle of the main body 120 of the inverted wheel type traveling body 100, which is the first component, can be obtained. The rotational angular velocity of the drive wheel 110 can be calculated by approximate differentiation of the observation value by an encoder installed on the rotation shaft of the drive wheel drive motor. Although the rotation angle of the drive wheel 110 can be measured by an encoder, since there is a slip phenomenon between the drive wheel 110 and the floor surface 190, the rotation angle of the drive wheel 110 itself is not used as an observation value.

A4. Creation of Basic Model for Estimating Center of Gravity Variation Subsequently, the variation (Δη) of the center of gravity angle of the inverted wheel type traveling body 100 due to the mass of the occupant 150 and the position of the center of gravity (circle mark m _h ) is estimated. Create a basic model for When estimating the variation of the barycentric angle (Δη), the physical parameters included in the equation of motion represented by Equation (3) must be known. However, the position of the center of gravity and the angle of center of gravity of the inverted wheel-type traveling body 100 vary greatly depending on the weight, physique, bag, and obstacles of the occupant 150 (weight, shape, etc., when a load such as luggage is mounted). For this reason, it is often difficult to actually grasp the physical parameters accurately.

  Therefore, when the equation (3) representing the equation of motion of the inverted wheel type traveling body 100 to be controlled is expressed by the nominal model and the disturbance using the nominal value of the physical parameter, it can be expressed as the following equation (5). can do.

In the above equation, φ ₀ is a nominal value of the physical parameter vector φ, w is a disturbance, and D _w is a path through which the disturbance enters.

The disturbance w represents a disturbance due to a physical parameter error or a linearization error, and appears as a large value in the equation of motion of the drive wheel 110, but appears as a relatively small value in the equation of motion of the inverted wheel type traveling body 100. confirmed. Thus, in the present embodiment, focusing on the disturbance entering the equation of motion of the drive wheel 110, it is defined as follows a path D _w of the start of the disturbance w and the disturbance w.

By using Equation (5) and appropriately designing the state observer based on the torque value u ₁ of the drive wheel drive motor as the control input and the observation amount y detected by the above-described various sensors, A certain center-of-gravity angle variation Δη can be estimated.

A5. State Observer Design Next, the state observer design will be described. In order to estimate the center-of-gravity angle variation Δη and the disturbance w, which are unknown variables, a state variable vector expressed by the following equation (7) is defined.

  As described above, the first component to the third component of the state variable vector are observation amounts, and the fourth component and the fifth component are the unknown centroid angle variation Δη and the disturbance w. Note that the center-of-gravity angle variation Δη and the disturbance w are independent independent variables.

  When the equation (5) is converted using the state variable vector represented by the equation (7), it can be expressed as the following equation (8).

  In the above formula,

  Here, it was confirmed by numerical calculation that the observable matrix (C, A) of the system represented by the equation (8) is full rank. That is, it is determined that the state variable vector including the unknown variable centroid angle variation Δη and the disturbance w can be estimated by the observer design.

  By the way, the role of the state observer is to operate in parallel with the controlled object represented by equation (8) and estimate the state variable vector based on the system matrix and observation amount y included in equation (8). . From the estimated value, the center-of-gravity angle variation Δη and the disturbance w can be obtained. The state observer can be expressed by the following equation (9).

  In the above formula,

A6. In the state observer represented by the equation (9) for determining the observer gain, by feeding back the output deviation (difference between the output vector y and its estimated value), the estimated state variable vector value of the observer is inverted wheel type traveling. It converges to the real state of the body. A block diagram representing such a feedback system is shown in FIG.

  As can be easily understood from FIG. 3, the convergence speed of the estimation by the state observer is affected by the magnitude of the feedback gain L. A large feedback gain L is equivalent to a large weighting of the output vector y that is an observed value, and the calculation of the estimated value by the state observer is performed with priority on the observed value. That is, the influence of the model is reduced, and the update speed of the estimated state due to the output deviation is increased. However, increasing the feedback gain L has a demerit that it is easily influenced by noise or the like included in the observed value.

  On the other hand, a small feedback gain L of the state observer means that the influence of the output vector y that is an observed value is small. In this case, calculation of the estimated value by the state observer mainly follows the basic model, and although the update rate of the estimated value due to the output deviation is reduced, the effect of observation noise is reduced, so that the estimation accuracy is increased. is there.

  As described above, the feedback gain L of the state observer can be appropriately set according to the operating frequency of each component of the state variable vector and the purpose of estimation. The state observer used in the method for estimating the center-of-gravity angle of the entire inverted wheel type traveling body according to the present invention is intended to estimate the fluctuation of the center-of-gravity angle with a slow rate of change and the steady disturbance, so that the estimation speed and accuracy can be reduced. Both are required to be high. Therefore, in this embodiment, the observer gain is obtained by solving the optimal control problem. This will be described in detail below.

  A method for determining the observer gain will be described below. A state estimation deviation (difference between the state variable vector x and its estimated value) is calculated from the controlled object represented by Expression (8) and the observer represented by Expression (9). The dynamic equation of the state estimation deviation is the following equation (10).

In Equation (10), since the eigenvalue of (A + LC) is equal to the eigenvalue of (A ^T + C ^T L ^T ), the design problem of the observer gain in Equation (8) is the feedback gain L ^T in Equation (11) below. It can be said that it is equivalent to a design problem.

  Therefore, it can be understood that the optimum feedback gain L can be obtained by solving the optimum regulation problem expressed by the following equation (12).

Here, (Q, R) is a weight matrix. By appropriately setting (Q, R), the trade-off between the convergence speed of the state estimation deviation and the magnitude of the output deviation can be adjusted. Using the optimal solution L ^T of the thus obtained formula (12), can be obtained eigenvalues of the closed loop system matrix equation (10).

  In the above equation, λ (*) means the eigenvalue of the matrix (*). Depending on the convergence state of each component of the state variable vector x, as shown in the following equation (14), the eigenvalue of the deviation system of equation (10) is moved from the optimum value obtained from equation (13) to be desired. It can also be placed in position.

  In the above equation, α is a design parameter, and can be appropriately determined according to the convergence speed of each component of the state estimation deviation by simulation and preliminary experiment results. Further, as shown in the following equation (15), the feedback gain L of the state observer represented by the equation (9) can be determined so that the eigenvalue arranged as described above is obtained.

  As the final stage of the state observer design of the inverted wheel type traveling body in the inverted state, the discrete state observer is discretized by discretizing the continuous state observer represented by the equation (15) designed as described above. For example, it is implemented by a control computer provided in the control means of the inverted wheel type traveling body.

  As described above, the state observer design of the inverted wheel type traveling body in the inverted state has been described in detail. Next, the center-of-gravity angle observer of the entire inverted wheel type traveling body in the auxiliary wheel grounding state will be described in detail below.

B1. The center of gravity angle of the entire inverted wheel type traveling body in the auxiliary wheel grounded state In the grounded auxiliary wheel grounded state, one of the front and rear auxiliary wheels or the front and rear auxiliary wheels is grounded, and the drive wheel and the main body of the inverted wheel type traveling body and the auxiliary wheel The rotating arm (arm portion) and the auxiliary wheel geometrically form a closed link system together with the floor surface, and receive a floor reaction force at the contact point of the auxiliary wheel. In the auxiliary wheel grounding state shown in FIG. 2, unlike the inverted state shown in FIG. 1, the front auxiliary wheel in the traveling direction (indicated by a gray arrow in the figure) is in contact with the floor surface. In other words, in the auxiliary wheel grounding state shown in FIG. 2, the auxiliary wheel rotating arm (arm portion) 131 is in a position rotated downward about the connecting portion 133 by the rotation angle q, and the auxiliary wheel 141 is placed on the floor surface 190. Grounded. In the ground point to the floor 190 of the auxiliary wheel 141, the floor reaction force f _c from the floor acting vertically upward.

The state shown in FIG. 2 is basically the same as the state shown in FIG. 1 except that the auxiliary wheel 141 is in contact with the floor surface 190 as described above. That is, also in FIG. 2, the angle η + Δη is a composite barycentric angle between the main body 120 of the inverted wheel type traveling body 100 and the passenger 150. Strictly speaking, the center-of-gravity angle as a whole is determined based on the center of gravity of the wheel-type 1-link inverted pendulum centering on the combined center of gravity (circle m ₁ ) of the main body 120 of the inverted wheel-type traveling body 100 and the passenger 150 and the auxiliary. It is a composite center of gravity composed of the center of gravity of the wheel rotation arm (arm part). However, in general, the mass of the auxiliary wheel rotating arm (arm portion) 131 or 132 is sufficiently smaller than the total mass of the main body portion 120 and the passenger 150 of the inverted wheel type traveling body 100. Therefore, it is considered that the center-of-gravity angle as a whole substantially coincides with η + Δη. However, the variation (Δη) of the center-of-gravity angle of the inverted wheel type traveling body 100 due to the mass and the center-of-gravity position (circle mark m _h ) of the passenger 150 is an unknown variable that is not measured by detection means such as a sensor.

  The center-of-gravity angle can be changed in the auxiliary wheel grounding state. For example, by driving the auxiliary wheel rotating arm (arm portion) 131, the grounding posture of the inverted wheel type traveling body 100 can be changed to adjust the barycentric angle. Further, it is possible to change the center-of-gravity angle of the main body 120 of the inverted wheel type traveling body 100 by driving the drive wheels 110 and to change the center-of-gravity angle of the entire inverted wheel-type traveling body 100. In this way, by properly adjusting the center-of-gravity angle of the entire inverted wheel type traveling body 100, it is possible to realize stable ground traveling or stable start-up operation from the auxiliary wheel grounding state to the inverted state.

B2. Principle of Estimating Center of Gravity Angle of Inverted Wheel Type Traveling Body in Auxiliary Wheel Ground State The center of gravity motion in the auxiliary wheel grounded state is the wheel type inverted pendulum motion of drive wheel 110, main body 120 of inverted wheel type traveling body 100, and passenger 150. It can be described by the equation and the rotational motion equation of the auxiliary wheel arm (arm part). The auxiliary wheel 141 is subjected to a floor reaction force f _c from the floor at the ground point. Although the floor reaction force f _c is to reach an effect on movement of the inverted wheel type traveling body 100, unless provided detecting means such as the floor reaction force sensor, the floor reaction force f _c is the unknown variables. Therefore, in the auxiliary wheel grounding state, one piece of information is further lacking compared to the inverted state, and thus detection of the center of gravity in the auxiliary wheel grounding state is more difficult than detection of the center of gravity in the inverted state.

Therefore, in the present embodiment, based on the motion characteristics of the inverted wheel type traveling body 100, the auxiliary wheels can be obtained based on the observation values by the existing limited detection means without newly providing detection means such as a floor reaction force sensor. we propose to estimate inverted wheel type traveling body entire unknown centroid angle variation Δη in the ground state and the floor reaction force f _c separated respectively. Specific estimation procedure of inverted wheel type traveling body entire unknown centroid angle variation Δη and floor reaction force f _c of the auxiliary wheel contact state will be described in detail below.

B3. Derivation of Equation of Motion of Inverted Wheel Type Running Body in Auxiliary Wheel Ground State The equation of motion of the auxiliary wheel rotating arm (arm portion) when the auxiliary wheel is not grounded can be expressed by the following equation (16).

In the above formula,

Where η: the center of gravity angle of the main body 120 of the inverted wheel type traveling body 100, q: the rotation angle of the auxiliary wheel arm (arm part), m _LB : the mass of the auxiliary wheel rotation arm (arm part), l _LB : auxiliary Distance from the center of gravity of the wheel rotation arm (arm part) to the arm rotation axis, I _LB : Moment of inertia around the center of gravity of the auxiliary wheel rotation arm (arm part), J _m3 : The rotor of the arm part drive motor (not shown) Moment of inertia, n ₂ : gear ratio of the arm part drive mechanism (motor: arm part = 1: n ₂ ), f _q : viscous friction coefficient of the arm part rotary joint (joint part), u ₂ : arm part drive motor Drive torque value, g ₀ : gravitational acceleration, φ: physical parameter vector composed of mass, center of gravity distance, moment of inertia, coefficient of friction, and the like.

  The equations of motion of the main body 120 and the drive wheels 110 of the inverted wheel type traveling body 100 in the inverted state where the auxiliary wheels are not grounded have already been obtained as Equation (1). Here, Formula (1) is utilized. However, while the center of gravity in the equilibrium state in the inverted state is directly above the axle 111, the number of equilibrium states in the auxiliary wheel grounding state is not limited to one, and an arbitrary grounding posture can be taken. Therefore, when linearly approximating the nonlinear equation of Formula (1), the linear approximation point is that the center-of-gravity angle of the inverted wheel-type traveling body 100 at a certain time is used instead of setting the center-of-gravity angle of the main body 120 to 0 (zero). As η, Taylor conversion of the nonlinear function is performed.

  If the change rate of the center-of-gravity angle variation Δη of the entire inverted wheel type traveling body, the center-of-gravity angle η of the main body 120, and the rotation angle θ of the drive wheel 110 are sufficiently low, the following approximation can be made. .

  Therefore, the equation of motion of the inverted wheel type traveling body 100 represented by the equation (1) can be approximately expressed by the following equation (17).

  Further, in the movement in the auxiliary wheel grounding state, the driving wheel 110, the main body 120 of the inverted wheel type traveling body 100, and the auxiliary wheel rotating arm (arm unit 131) constitute a closed link system with the floor surface, and the inverted wheel type traveling is performed. The center-of-gravity angle η of the main body 120 of the body 100 and the rotation angle q of the arm 131 are geometrically constrained as shown in the following equation (18).

In the above equation, r ₁ is the distance from the contact point of the drive wheel 110 to the base of the arm portion support frame of the main body 120 of the inverted wheel type traveling body 100 (the junction point between the horizontal portion and the vertical portion described above), r ₂ is the radius of the auxiliary wheel, d is the distance to the axle 111 and the auxiliary wheel rotation arm joint (articulation point 133). Further, LLB is the distance from the center of the auxiliary wheel to the auxiliary wheel rotating arm joint (continuous contact 133).

When determining the partial differential from the ground constraint condition represented by the above formula (18), the partial derivative matrix is the Jacobian matrix J, is a path in the ground point of the auxiliary wheel enters the floor reaction force f _c received from the floor . The transposed matrix of the Jacobian matrix J is shown in the following formula (19).

Therefore, the simultaneous equation of motion of the inverted wheel type traveling body 100 in the auxiliary wheel grounding state is based on the center of mass of the combined center of gravity (the circle m ₁ shown in FIG. 2) of the main body 120 and the passenger 150 of the inverted wheel type traveling body 100. Equation (17) as the equation of motion of the wheel-type 1-link inverted pendulum, Equation (17) as the equation of motion of the rotation angle of the drive wheel 110, Equation (16) as the equation of motion of the arm 131 of the auxiliary wheel 141, and the Jacobian matrix floor reaction force f _c acting through J, can be expressed as the following equation (20).

  In the above formula,

  As shown in the following formula (21), the inertia matrix of the equation of motion in the auxiliary wheel ground contact state represented by the formula (20) is regular.

  Therefore, by multiplying both sides of the equation (20) by the inverse matrix of the inertia matrix from the left side, the equation (20) can be expressed as the following equation (22).

  In the above formula,

Comparing the equation of motion (22) in the auxiliary wheel grounding state and the equation of motion (3) in the inverted state, the equation (22) shows the angular variable q, floor reaction force f _c , and gravity term of the auxiliary wheel rotating arm. It can be seen that equation (22) is more complicated in that b ₀ and the auxiliary wheel rotary arm drive input u ₂ are included.

B4. Each coefficient matrix included in the right side of the creation formula (22) of the basic model for estimating the center of gravity variation is a function of the center of gravity angle η of the main body 120 of the inverted wheel type traveling body 100 and the rotation angle q of the arm 131. And varies depending on the values of η and q. Since the movable range of the main body 120 is relatively small and the movable range of the arm 131 is also limited, the value of (η, q) in each coefficient matrix on the right side of Equation (22) is set to (η, q). By fixing to the middle point (η ₀ , q ₀ ) of the fluctuation range, the system represented by the equation (22) can be simplified as the following equation (23). In addition, since it is often difficult in practice to accurately grasp the physical parameter φ, when the nominal value of the physical parameter is used to express it with a nominal model and a disturbance, the equation (22) is expressed by the following equation (23 ).

As described above, (η ₀ , q ₀ ) in the above equation is an intermediate point of the variation range of (η, q). Φ ₀ is a nominal value of the physical parameter vector φ, w is a disturbance, and D _g is a path through which the disturbance w enters. The variable Δη included in the right side of the equation (23) is a variation (Δη) of the center of gravity angle of the inverted wheel type traveling body 100 due to the mass of the passenger 150 and the center of gravity (m _h ). is the unknown value can not be detected by the detecting means such as sensors, also the floor reaction force f _c Note that an unknown value can not be observed unless the newly provided a detecting means such as the floor reaction force sensor.

Note that the disturbance w in the equation (23) represents a disturbance due to a physical parameter error, an external force disturbance, or the like. In the present embodiment, paying attention to disturbances appearing in the equation of motion of the drive wheels of inverted wheel type traveling body (the second line of the formula (23)), defined as follows path D _g of the start of the disturbance w and the disturbance w To do.

  As described in the description relating to the expression (4), the inverted wheel type traveling body 100 is provided with a minimum necessary detecting means (sensor or the like). In the auxiliary wheel grounding state, in addition to the observation value included in the equation (4), the rotation angle q of the arm portion can be measured by a detecting means such as an encoder. The rotational angular velocity of the arm part is calculated by approximate differentiation of the observation value by an encoder or the like. That is, as shown in the following formula (24), the known variables in the auxiliary wheel grounding state are as follows.

B ₀ in Equation (23) can be calculated based on the observed value. Therefore, based on the simultaneous motion equation (23) of the inverted wheel type traveling body 100 in the auxiliary wheel grounding state, the control torques u ₁ and u ₂ given to the equation (23), and the observation value shown in the equation (24). designing an observer can estimate the inverted wheel type traveling body of the center of gravity of the whole angle variation Δη and floor reaction force f _c of the auxiliary wheel contact state is unknown variables.

B5. Next, the design of the center-of-gravity observer when the auxiliary wheel is grounded will be described. To estimate the unknown is the center of gravity angle variation Δη of inverted wheel type traveling body 100 and the floor reaction force f _c, defines the state variable vector of the auxiliary wheel contact state by the following equation (25).

As shown in the above equation (25), the upper three components of the state variable vector in the auxiliary wheel grounding state are known observation variables, and the lower three components are the unknown variables centroid angle variation Δη, floor reaction force f _c , And disturbance w. That is, in the state variable vector shown in the above equation (25), the gravity center angle variation Δη, the floor reaction force f _c , and the disturbance w are separated independent variables.

In the auxiliary wheel contact state, when the estimated velocity of the observer is sufficiently faster than the change and the change of the floor reaction force f _c of the center of gravity angle variation Δη is the change rate of the center of gravity angle variation Δη and floor reaction force f _c 0 ( Zero). Thus, the equation of motion in the auxiliary wheel grounding state of Equation (23) can be re-expressed as in Equation (26) below using the state variable vector defined in Equation (25).

  In the above formula,

  Here, an observable matrix of the system represented by the equation (26) by numerical calculation.

Confirmed full rank. Thus, it is determined that the state variable vector in the ground contact state of the auxiliary wheel including the unknown center-of-gravity angle variation Δη, floor reaction force f _c , and disturbance w can be estimated by the observer design.

  By the way, since the affine term b exists in the state equation of Expression (26), it becomes a special system different from a general linear system. However, the state observer design for linear affine systems is generally not found. Here, by utilizing the fact that the affine term b is a function of the observation amount, the affine term b is introduced into the observer, and a linear affine observer as shown in the following equation (27) is created.

In the above formula,

B6. Similarly to the state observer in the inverted state represented by Equation (9), the output deviation (the difference between the output vector and its estimated value) is also applied to the state observer in the auxiliary wheel grounding state represented by the observer gain determination equation (27). ) Is converged to the actual state of the inverted wheel type traveling body. A block diagram representing such a feedback system in an auxiliary wheel grounding state is shown in FIG.

  Also in the linear affine observer of Expression (27), by appropriately designing the observer gain, the estimated state value obtained from the observer is asymptotic to the state of the inverted wheel type traveling body 100 represented by Expression (26). Can be converged to.

  A method for determining the observer gain will be described below. First, the state estimation deviation (the difference between the state variable vector and its estimated value) is calculated from the controlled object represented by Expression (26) and the state observer represented by Expression (27). The dynamic equation of state estimation deviation is expressed as follows:

  From equation (27) and equation (26), a differential equation of state estimation deviation shown in the following equation (28) is obtained.

  It can be seen that the differential equation of the state estimation deviation in the auxiliary wheel grounding state represented by the above equation (28) has the same form as the differential equation of the state estimation deviation in the inverted state represented by the equation (10). Therefore, the gain of the state observer in the auxiliary wheel grounding state can be designed in the same procedure as the gain L of the state observer in the inverted state.

  By appropriately designing the gain of the state observer in the auxiliary wheel grounding state, the estimated state value obtained from the state observer represented by the equation (27) is obtained from the inverted wheel type traveling body 100 represented by the equation (26). Asymptotically converge to the state.

  In the state observer in the auxiliary wheel grounding state represented by Expression (27), the convergence speed of the estimated state is determined by the magnitude of the observer gain, similarly to the state observer in the inverted state represented by Expression (9). That is, the estimated convergence speed is determined by the size of the pole of the characteristic equation obtained from the state observer. A large observer gain means that the weight of the observation value is large, and the observation value is given priority in the calculation of the estimated value in the state observer. That is, the influence of the model on the state estimation is reduced, and the update rate of the estimated value by the observed value is increased. However, if the gain of the observer is large, it is easily affected by noise included in the observed value.

  On the other hand, a small observer gain means that the influence of the observed value is small. In this case, the estimated value calculation of the observer mainly follows the basic model, and the update rate of the estimated value by the observed value is low, while the influence of the observation noise is weak, so the estimation accuracy is high. Therefore, the feedback gain of the state observer in the auxiliary wheel grounding state can be appropriately set according to the operating frequency of each component of the state variable vector in the auxiliary wheel grounding state and the purpose of estimation. The state observer used in the method for estimating the center-of-gravity angle of the entire inverted wheel type traveling body according to the present invention is intended to estimate the change in the center-of-gravity angle with a slow rate of change and the estimation of the floor reaction force. Both are required to be high. Therefore, in this embodiment, the optimal control problem is solved, and the observer poles are arranged near the obtained poles. Details will be described below.

  The coefficient matrix of the auxiliary wheel ground contact state estimation deviation system described by Equation (28) is

And this eigenvalue is

Are the same as the eigenvalues, observer bar gain design problem of the formula (28) is equal to the feedback gain of the design problem of the formula (29).

  Therefore, it can be understood that the optimum feedback gain can be obtained by solving the optimum regulation problem expressed by the following equation (30).

  here,

Is a weight matrix. By appropriately setting the weight matrix, it is possible to adjust the trade-off between the convergence speed of the state estimation deviation and the magnitude of the output deviation. The eigenvalue of the closed-loop system matrix of Equation (29) can be obtained using the optimal solution of Equation (30) obtained in this way.

  In the above equation, λ (*) means the eigenvalue of the matrix (*). As shown in the following equation (32), the eigenvalue of the deviation system of equation (28) can be moved from the eigenvalue obtained from equations (30) and (31) and arranged at a desired position.

  In the above equation, the second term on the right-hand side is a design parameter, and can be appropriately determined according to the convergence speed of each component of the state estimation deviation based on simulations and preliminary experiment results. Further, as shown in the following equation (33), the feedback gain of the state observer represented by the equation (27) can be determined so that the eigenvalue arranged as described above can be obtained.

From the estimated value according to the designed state observer as described above, to obtain a ground reaction force f _c received from variation (.DELTA..eta) and the auxiliary wheel 141 the floor of the center of gravity angle of the entire inverted wheel type traveling body 100 of the auxiliary wheel contact state be able to.

  As the final stage of the state observer design of the inverted wheel type traveling body in the auxiliary wheel grounding state, the discrete state is obtained by discretizing the continuous state observer represented by the equation (27) designed as described above. The observer is designed and mounted, for example, by a control computer provided in the control means of the inverted wheel type traveling body.

As described above, the state observer design of the inverted wheel type traveling body in the inverted state and the auxiliary wheel grounding state has been described in detail. Thus, according to the present invention, in an inverted wheel type traveling body that can be boarded, the inverted wheel caused by the mass and the position of the center of gravity of the passenger or the load in the inverted state without adding a new sensing unit or the like. Of the center of gravity of the inverted wheel type traveling body due to the estimation of the variation (Δη) and disturbance (w) of the center of gravity angle of the entire traveling body, and the mass and the position of the center of gravity of the occupant or mounted object in the grounded auxiliary wheel It is possible to estimate the angular variation (Δη), the floor reaction force (f _c ) that the auxiliary wheel receives from the floor, and the disturbance (w).

  As described above, in the inverted wheel type traveling body, the control of the inverted traveling is avoided using the minimum necessary sensors while avoiding concerns such as an increase in the size and weight of the traveling body, an increase in manufacturing cost, and a complicated control system. Advantages such as improvement of performance, realization of continuous and smooth transition from the auxiliary wheel grounding state to the inverted state, and prevention of a dangerous phenomenon accompanied by fluctuation of the center of gravity angle can be achieved.

  As mentioned above, for the purpose of explaining the present invention, some embodiments for estimating the variation of the center of gravity of the entire inverted wheel type traveling body in a specific procedure for the inverted wheel type traveling body having a specific configuration. As described above, the scope of the present invention is not limited to these exemplary embodiments, and modifications can be made as appropriate within the scope of the claims and the specification. Needless to say.

  DESCRIPTION OF SYMBOLS 100 ... Inverted wheel type traveling body, 110 ... Drive wheel, 111 ... Drive wheel axle, 120 ... Main-body part, 131 ... Arm part (front side of a moving direction), 132 ... Arm part (back side of a moving direction), 133 ... Connection part ( Traveling direction front side), 134 ... articulated part (traveling direction rear side), 141 ... auxiliary wheel (forwarding direction front side), 142 ... auxiliary wheel (traveling direction rear side), 150 ... passenger, 151 ... link, and 190 ... floor surface.

Claims (8)

Driving wheels,
A main body supported by the axle of the drive wheel;
An arm part movably connected to the main body part;
An auxiliary wheel rotatably connected to the opposite side of the arm portion from the body portion;
Driving wheel driving means for driving the driving wheel;
Arm driving means for driving the arm portion;
Detecting means for detecting the actual state of the inverted wheel type traveling body;
Control means for controlling the drive wheel drive unit or the arm drive unit based on a deviation between the actual state and the target state detected by the detection unit to achieve the target state;
With
An inverted state in which the auxiliary wheel is not grounded and is inverted only by the driving wheel,
An auxiliary wheel grounding state in which the auxiliary wheel is grounded;
A center-of-gravity angle estimation method for the entire boardable inverted wheel type traveling body having:
Designing a state observer based on equations of motion for each of the inverted state and the auxiliary wheel grounding state;
Introducing an affine term into the state equation of the state observer for the auxiliary wheel grounding state;
Using the state observer for the inverted state, the variation (Δη) and disturbance (w) of the center-of-gravity angle of the entire inverted wheel type traveling body due to the mass and the position of the center of gravity of the passenger or the load in the inverted state And using the state observer for the auxiliary wheel grounding state, the center of gravity angle of the entire inverted wheel type traveling body due to the mass and the position of the center of gravity of the passenger or the load in the auxiliary wheel grounding state Estimating a variation (Δη), a floor reaction force (f _c ) that the auxiliary wheel receives from the floor, and a disturbance (w);
A center-of-gravity angle estimation method for the entire inverted wheel type traveling body, characterized by The center of gravity angle estimation method for the entire inverted wheel type traveling body according to claim 1, wherein the state observer for the inverted state is represented by a state equation and an output equation represented by the following equation (9):

In the above formula,

And
On the other hand, the state observer for the auxiliary wheel grounding state is represented by a state equation and an output equation shown in the following equation (27),

In the above formula,

A center-of-gravity angle estimation method for an entire inverted wheel type traveling body, characterized in that: The center-of-gravity angle estimation method for the entire inverted wheel type traveling body according to claim 2, wherein in the inverted state, the Δη and the disturbance (w) are estimated using the following equation (7) as a state variable vector: In the auxiliary wheel grounding state, the Δη, the f _c , and the disturbance (w) are estimated using the following equation (25) as a state variable vector:

In the above formula,

A center-of-gravity angle estimation method for an entire inverted wheel type traveling body, characterized in that:   The method for estimating the center-of-gravity angle of the entire inverted wheel type traveling body according to any one of claims 1 to 3, wherein a warning is issued when the Δη is greater than a predetermined value. The center-of-gravity angle estimation method for the entire inverted wheel type traveling body. Driving wheels,
A main body supported by the axle of the drive wheel;
An arm part movably connected to the main body part;
An auxiliary wheel rotatably connected to the opposite side of the arm portion from the body portion;
Driving wheel driving means for driving the driving wheel;
Arm driving means for driving the arm portion;
Detecting means for detecting the actual state of the inverted wheel type traveling body;
Control means for controlling the drive wheel drive unit or the arm drive unit based on a deviation between the actual state and the target state detected by the detection unit to achieve the target state;
With
An inverted state in which the auxiliary wheel is not grounded and is inverted only by the driving wheel,
An auxiliary wheel grounding state in which the auxiliary wheel is grounded;
An inverted wheel-type traveling body capable of boarding,
The control means is
Designing a state observer based on equations of motion for each of the inverted state and the auxiliary wheel grounding state;
Introducing an affine term into the state equation of the state observer for the auxiliary wheel grounding state;
Using the state observer for the inverted state, the variation (Δη) and disturbance (w) of the center-of-gravity angle of the entire inverted wheel type traveling body due to the mass and the position of the center of gravity of the passenger or the load in the inverted state And using the state observer for the auxiliary wheel grounding state, the center of gravity angle of the entire inverted wheel type traveling body due to the mass and the position of the center of gravity of the passenger or the load in the auxiliary wheel grounding state Estimating a variation (Δη), a floor reaction force (f _c ) that the auxiliary wheel receives from the floor, and a disturbance (w);
An inverted wheel type traveling body characterized by The inverted wheel type traveling body according to claim 5, wherein the state observer for the inverted state is represented by a state equation and an output equation represented by the following equation (9):

In the above formula,

And
On the other hand, the state observer for the auxiliary wheel grounding state is represented by the state equation and the output equation shown in the following equation (27),

In the above formula,

An inverted wheel type traveling body characterized by being. The inverted wheel type traveling body according to claim 6, wherein in the inverted state, the Δη and the disturbance (w) are estimated using the following equation (7) as a state variable vector, and in the grounded state of the auxiliary wheel: Estimates the Δη, the f _c , and the disturbance (w) using the following equation (25) as a state variable vector:

In the above formula,

An inverted wheel type traveling body characterized by being.   The inverted wheel type traveling body according to any one of claims 5 to 7, further comprising barycentric angle warning means for issuing a warning when the Δη is larger than a predetermined value. Inverted wheel-type traveling body.

Images