JP6187310B2 - Inverted moving body - Google Patents

Description

  The present invention relates to an inverted moving body.

  An inverted mobile body (hereinafter referred to as a mobile body) that can be operated by a user has been proposed.

  For example, in Patent Document 1, an approach to a moving ground plane is detected by acquiring a ground plane acceleration, which is an acceleration of a ground contact portion between a wheel and a ground plane, and a circumferential acceleration of a wheel body. An inverted pendulum vehicle that changes the moving speed is disclosed.

Japanese Patent No. 5297960

  A situation is assumed in which a user enters a moving sidewalk from a stationary sidewalk when the user gets on an inverted moving body and moves. FIG. 14 is an image diagram illustrating a state in which the inverted moving body V enters the moving sidewalk from the side surface. Here, when the inverted moving body V enters a moving sidewalk with a high friction coefficient μ, the inverted moving body that operates nonholonomically may be dragged toward the moving sidewalk.

  When the inverted moving body is dragged to the side of the moving sidewalk, it is considered that the measurement values related to the wheels are inaccurate. In the vehicle according to Patent Document 1, since the approach detection to the moving sidewalk is performed using the contact surface acceleration which is a measurement value related to the wheel and the circumferential acceleration of the wheel body, there is a possibility that the disturbance cannot be accurately estimated. .

  The present invention has been made to solve such a problem, and an object of the present invention is to provide an inverted moving body capable of more accurately estimating when a disturbance occurs.

The inverted mobile body according to the present invention is
A gyroscope for detecting the yaw angular velocity of the inverted moving body;
A direction indicating sensor for detecting a lean angle of the inverted moving body;
Wherein compares the value of the direction indication sensor divided by lean angle detected by the yaw angular velocity gyroscopes detects a value obtained by dividing the maximum value of the lean angle at maximum angular velocity of the yaw direction, a deviation between Calculating a correction value of the turning amount of the inverted mobile body using the deviation when it is determined that the deviation is larger than the threshold ;
And a control unit for correcting the turning amount of the inverted vehicle using the correction value calculated by the calculation unit. In this way, the estimation unit detects the disturbance using the detection results of the gyroscope, the acceleration sensor, and the direction indication sensor instead of the measurement value related to the wheel, and therefore more accurately performs the estimation when the disturbance occurs. Can do.

  It is possible to provide an inverted moving body capable of more accurately estimating when a disturbance occurs.

1 is an external view illustrating an inverted moving body according to a first embodiment. FIG. 3 is a block diagram illustrating a configuration example of a sensor and a calculation unit in the moving body according to the first embodiment. In Embodiment 1, it is an example of the graph of the 1st angle and the 2nd angle when there is no disturbance. In Embodiment 1, it is an image figure which showed the state when a train started when a user boarded a moving body and got on the train. In Embodiment 1, it is the image figure which showed the state which gets into the moving walk from the stationary ground in the state which the user got on the moving body and is moving. In Embodiment 1, it is an example of the graph of the 1st angle and the 2nd angle in the state of Drawing 4A. In Embodiment 1, it is an example of the graph of the 1st angle and 2nd angle in the state of FIG. 4B. FIG. 3 is an example of a graph showing acceleration data measured by an acceleration sensor when riding on a moving sidewalk from a sidewalk where a moving body is stationary in Embodiment 1. FIG. 6B is a graph showing angle data calculated by the calculation unit using the acceleration measured in FIG. 6A in the first embodiment. 6B is an example of a graph showing acceleration data measured by an acceleration sensor when processing for correcting the influence of disturbance is performed on the acceleration data in FIG. 6A in the first embodiment. 6 is a graph showing angle data calculated by the calculation unit using the acceleration measured in FIG. 6C in the first embodiment. In Embodiment 1, it is another example of the graph which showed the data of the acceleration which the acceleration sensor measured when boarding the moving walk from the walk where the moving body is stationary. 7B is a graph showing angle data calculated by the calculation unit using the acceleration measured in FIG. 7A in the first embodiment. FIG. 7B is an example of a graph showing acceleration data measured by the acceleration sensor when processing for correcting the influence of disturbance is performed on the acceleration data in FIG. 7A in the first embodiment. 7 is a graph showing angle data calculated by the calculation unit using the acceleration measured in FIG. 7C in the first embodiment. In Embodiment 1, it is an example of the graph which showed the ideal relationship of the angular velocity to a yaw direction, and a lean angle when there is no disturbance. In Embodiment 1, it is the image figure which showed the state when a strong wind blows from the diagonally right back, when the user has boarded the moving body and is moving. In Embodiment 1, it is a graph of the angular velocity and lean angle of the yaw direction in the state of FIG. In Embodiment 1, it is an example of the graph which showed the relationship between the angular velocity to a yaw direction when there is a disturbance, and a lean angle. In Embodiment 1, it is another example of the graph which showed the inclination of the lean angle with respect to the angular velocity to a yaw direction when there exists a disturbance. 3 is a flowchart illustrating an example of control of a moving object in the first embodiment. It is the image figure which showed a mode that the inverted type mobile body V in related technology approached the moving sidewalk.

[Embodiment 1]
Embodiment 1 of the present invention will be described below with reference to the drawings. FIG. 1 is a diagram illustrating an appearance of an inverted moving body according to the first embodiment. FIG. 1 is an external view illustrating an example of an inverted moving body (hereinafter referred to as a moving body) according to the first embodiment. The moving body 10 includes a handle 11, a frame 12, a step unit 13, and wheels 14. In FIG. 1, the right side is the front (traveling direction) when the passenger is on board, and the left side is the rear when the passenger is on board.

  A handle 11 is provided on the upper part of the moving body 10, and the passenger can stably board the moving body 10 by being held by the handle 11. Further, the handle 11 may be provided with an operation unit for the passenger to operate the traveling of the moving body 10. The frame 12 connects the handle 11 and the step portion 13. A step portion 13 (boarding board) is provided at a lower portion of the frame 12, and a passenger can put his / her foot on the step portion 13 when the moving body 10 is boarded. Two wheels 14 are provided on the left and right of the lower portion of the step unit 13. The wheel 14 is driven by a motor or the like (not shown).

  FIG. 2 is a block diagram illustrating a configuration example of the sensor and the calculation unit in the moving body 10. The moving body 10 includes a gyroscope 15 (gyro sensor), a direction indication sensor 16 and an acceleration sensor 17 as sensors. Furthermore, the moving body 10 includes a calculation unit 18 and a control unit 19 as a calculation unit. In addition, description of the battery etc. which supply a voltage to each part of the moving body 10 is abbreviate | omitted.

  The gyroscope 15 measures the angular velocity of the moving body 10. For example, the gyroscope 15 can detect the angular velocity in the pitch direction and the angular velocity in the yaw direction of the moving body 10 by measuring the speed of the moving body 10 in the X-axis, Y-axis, and Z-axis directions. it can. The gyroscope 15 may further detect an angular velocity in the lean angle direction (an angular velocity in the roll direction).

  The direction instruction sensor 16 detects movement instruction information of the moving body 10. Here, the “movement instruction information” includes a movement direction and a movement value (direction instruction value) of the moving body 10 instructed (input) by the passenger or the like to the moving body 10. The direction indication value is input to the moving body 10 from an operation unit provided on the handle 11, for example. By detecting the movement instruction information, the direction instruction sensor 16 detects the direction in which the vehicle body is facing. Specifically, the direction indicating sensor 16 can detect at least the lean angle (that is, the angle in the roll direction) of the moving body 10. Here, the direction indication sensor 16 may be an input device that instructs the posture of the moving body 10 like a joystick for the user to operate the moving body 10. In this case, the direction indicating sensor 16 detects the lean angle of the moving body 10 in accordance with an input from the user. Alternatively, the direction indicating sensor 16 may actually measure and detect the lean angle, such as a lean angle sensor or a tilt sensor.

  The acceleration sensor 17 measures the acceleration of the moving body 10. For example, the acceleration sensor 17 detects the angular acceleration in the pitch direction and the angular acceleration in the yaw direction of the moving body 10 by measuring the acceleration of the moving body 10 in the X-axis, Y-axis, and Z-axis directions. be able to. The acceleration sensor 17 may further detect angular acceleration in the lean direction (that is, angular acceleration in the roll direction).

  The calculation unit 18 calculates an angle in the pitch direction of the moving body 10 (hereinafter referred to as a first angle) based on the angular velocity in the pitch direction measured by the gyroscope 15. Further, the calculation unit 18 calculates an angle in the pitch direction of the inverted moving body (hereinafter referred to as a second angle) based on the acceleration measured by the acceleration sensor 17.

  Note that the calculation unit 18 obtains an angle by performing time integration of angular velocity (or acceleration). Further, the calculation unit 18 includes a high-pass filter circuit. After the measurement result of the angular velocity in the pitch direction measured by the gyroscope 15 is passed through the high-pass filter circuit, the influence of the offset value (bias) in the measurement result is suppressed. The first angle is calculated by performing time integration of the angular velocity. Further, if time integration of a long time angular velocity is performed to calculate the first angle, there is a possibility that the offset value is integrated. Therefore, the calculation unit 18 appropriately resets the integral value (first angle value).

  The calculation unit 18 compares the calculated first angle and second angle, and determines whether or not there is a deviation larger than a predetermined threshold between the first angle and the second angle. When the calculation unit 18 determines that the first angle and the second angle have a deviation larger than a predetermined threshold, the calculation unit 18 estimates that the moving body 10 is subjected to disturbance. Then, using the deviation between the first angle and the second angle, the deviation caused by the disturbance in the acceleration detected by the acceleration sensor 17 is calculated (estimated). That is, the calculation unit 18 estimates a deviation (deviation) between the measured acceleration value when there is no disturbance in the acceleration sensor 17 and the measured acceleration value when there is a disturbance.

  Further, the calculation unit 18 compares the angular velocity in the yaw direction of the moving body 10 measured by the gyroscope 15 with the lean angle of the moving body 10 detected by the direction indication sensor 16, and determines the angular velocity and lean angle in the yaw direction. 2 is calculated (that is, angular velocity / lean angle in the yaw direction). The calculation unit 18 obtains a deviation between the calculated slope of the graph and a predetermined value, and estimates that the moving body 10 is disturbed when the deviation is larger than a predetermined threshold. The calculation unit 18 calculates (estimates) a correction value for the turning amount (control value) of the moving body 10 using the deviation. That is, the calculation unit 18 estimates a deviation of movement caused by the disturbance of the moving body 10.

  Thus, the calculation unit 18 functions as an estimation unit that estimates the disturbance received by the moving body 10. Further, the calculation unit 18 estimates a deviation caused by the disturbance in the acceleration detected by the acceleration sensor 17 and estimates a correction value of the turning amount (control value) of the moving body 10.

  The control unit 19 determines the current state of the moving body 10 using the angular velocity data measured by the gyroscope 15 and the acceleration data measured by the acceleration sensor 17, and controls the running and posture of the moving body 10. I do. For example, when the moving body 10 is moving at a predetermined acceleration, the posture of the moving body 10 is controlled so that the moving body 10 is stably inverted. Further, the control unit 19 turns the moving body 10 using the lean angle detected by the direction indication sensor 16.

  Further, the control unit 19 calculates when the calculation unit 18 determines that there is a deviation larger than a predetermined threshold between the first angle and the second angle (when the calculation unit 18 estimates the presence of a disturbance). It functions as a correction unit that corrects the acceleration value measured by the acceleration sensor 17 using the deviation of acceleration due to disturbance estimated by the unit 18. That is, the control unit 19 corrects the measurement result of the acceleration sensor 17 using the estimated value relating to the disturbance. Furthermore, the control unit 19 uses the correction value for the turning amount of the moving body 10 estimated by the calculation unit 18 when the slope of the graph calculated by the calculation unit 18 deviates from a predetermined value by more than a threshold value. It functions as a correction unit that corrects the turning amount (control value) of the moving body 10. That is, the control unit 19 corrects a deviation caused by a disturbance in the movement of the moving body 10.

  Hereinafter, a problem to be solved by the moving body 10 according to the first embodiment will be described.

Case 1.
First, the first case will be described. FIG. 3 is an example of a graph of the first angle and the second angle calculated by the calculation unit 18 when there is no disturbance (when the moving body 10 is traveling normally). The solid line graph indicates the first angle, and the broken line graph indicates the second angle. (In FIG. 3, the first angle is described as “gyro” and the second angle as “acceleration”.) The horizontal axis of the graph is time, the vertical axis is the pitch angle (the upward direction of the vertical axis is Plus direction, down direction is minus direction). The pitch angle is a positive direction when the moving body 10 is inclined forward from the vertical axis, and a negative direction when the moving body 10 is inclined backward from the vertical axis. The description of this graph is the same in FIGS. 5A and 5B.

  The second angle calculated based on the acceleration has more noise than the first angle calculated based on the angular velocity. However, although the second angle graph has spike noise, a difference of more than a certain amount of deviation does not occur in seconds compared to the first angle graph. That is, the interval between graphs is not more than a predetermined distance. Here, since the measurement result of the gyroscope 15 does not deviate greatly in seconds, the first angle is considered to be an accurate value.

  Next, a case where a disturbance occurs when the user is on the moving body 10 will be described. FIG. 4A is an image diagram showing a state where the train T starts when the user U gets on the moving body 10 and gets on the train T. FIG. The train T is traveling forward as viewed from the user and the moving body 10. At this time, since the vehicle body suddenly starts moving for the user U and the moving body 10, a backward inertial force is received. That is, the user U and the moving body 10 move with a backward acceleration.

  FIG. 4B is an image diagram showing a state where the user U gets on the moving sidewalk R2 from the stationary ground R1 while the user U is on the moving body 10 and moving. The moving sidewalk R2 moves the ground forward as viewed from the user U and the moving body 10. At this time, since the vehicle body suddenly starts to move for the user U and the moving body 10, backward acceleration is generated in the user U and the moving body 10 as in FIG. 4A.

  FIG. 5A shows a first angle calculated based on the angular velocity in the pitch direction measured by the gyroscope 15 and a second angle calculated based on the acceleration measured by the acceleration sensor 17 in the state of FIG. 4A. It is an example of a graph.

  Before the train starts moving, the moving body 10 is in a stationary state. Then, a backward acceleration is generated in the moving body 10 at the timing when the train starts to move. Therefore, the angular acceleration measured by the acceleration sensor 17 becomes inaccurate due to the disturbance due to the backward acceleration. That is, since the acceleration sensor 17 cannot distinguish between the acceleration caused by the external factor and the acceleration caused by the operation of the moving body 10, the acceleration caused by the external factor is also measured as the acceleration caused by the operation of the moving body 10. End up. As described above, since the acceleration sensor 17 measures the inaccurate angular acceleration, the second angle calculated by the calculation unit 18 also becomes an inaccurate value. Therefore, as shown in FIG. 5A, the second angle has a deviation as compared with the first angle. (In FIG. 5A, the deviation is indicated by an arrow.) Specifically, the second angle is less than the first angle (the moving body 10 is less than the first angle) until some time has passed since the timing of starting movement. The value is the direction of tilting backward).

  FIG. 5B is an example of a graph of the first angle and the second angle in the state of FIG. 4B. There is almost no difference between the first angle and the second angle before the moving body 10 gets on the moving sidewalk.

  As described above, backward acceleration is generated in the moving body 10 at the timing when the moving body 10 gets on the moving sidewalk. Therefore, for the same reason as described above, the acceleration measured by the acceleration sensor 17 becomes inaccurate due to the disturbance caused by the backward acceleration. Therefore, as shown in FIG. 5B, the second angle has a deviation as compared with the first angle. (In FIG. 5B, the deviation is indicated by an arrow.)

  As described above, it is conceivable that a force is applied to the moving body 10 due to an external factor, not due to the operation of the moving body 10, and a deviation occurs between the first angle and the second angle. Here, since the control unit 19 performs posture control based on the angular velocity measured by the gyroscope 15 and the acceleration measured by the acceleration sensor 17, the posture control that does not match the actual posture of the moving body 10 is performed. Can be considered.

  In order to solve the above problem, the calculation unit 18 compares the calculated first angle and the second angle, and calculates a deviation between the first angle and the second angle (the first angle and the second angle). It is determined whether or not the absolute value of the difference is greater than a predetermined positive threshold value. If the deviation is equal to or less than the threshold value, the calculation unit 18 determines that no disturbance has occurred, and the control unit 19 does not perform special control. When there is a deviation larger than the threshold, the calculation unit 18 estimates the deviation caused by the disturbance in the acceleration detected by the acceleration sensor 17, and the control unit 19 uses the estimated deviation to calculate the measurement result of the acceleration sensor 17. to correct. That is, the calculation unit 18 and the control unit 19 estimate the disturbance and then feed back and correct the deviation data (that is, the estimated disturbance data) to the measurement value of the acceleration sensor 17. This correction is made so that the angle calculated based on the acceleration measured by the acceleration sensor 17 approaches the same value as the angle calculated based on the angular velocity measured by the gyroscope 15. Thereby, since the measured value of the acceleration sensor 17 can be made accurate, the control unit 19 can perform the posture control of the moving body 10 so as to match the actual posture of the moving body 10.

  FIG. 6A is an example of a graph showing acceleration data measured by the acceleration sensor 17 when the moving body 10 rides on a moving sidewalk from a stationary sidewalk. In FIG. 6A, the broken line graph is the acceleration graph in the X-axis direction (left-right direction) of the moving body 10, the solid line graph is the acceleration graph in the Y-axis direction (front-rear direction), and the alternate long and short dash line graph is in the Z-axis direction (vertical direction). (Direction) is a graph of acceleration. The horizontal axis of FIG. 6A is time (sec), and the vertical axis is acceleration (m / sec ^ 2) (the same description is given in FIGS. 6C, 7A, and 7C). In FIG. 6A, the measurement data of the acceleration sensor 17 is not corrected. Accordingly, in the acceleration in the Y-axis direction, disturbance during boarding occurs (that is, acceleration occurs) in the circled portion in FIG. 6A.

  FIG. 6B is a graph showing the angle data calculated by the calculation unit 18 using the acceleration measured in FIG. 6A. In FIG. 6B, a broken line graph is a roll angle graph of the moving body 10, and a solid line is a pitch angle graph. The horizontal axis in FIG. 6B is time (sec), and the vertical axis is angle (deg) (the same description is given in FIGS. 6D, 7B, and 7D). As described above, since the influence of disturbance at the time of boarding has occurred in the acceleration in the Y-axis direction, the pitch angle has a disturbance at the time of boarding in the part circled in FIG. 6B (the value of the angle is It has increased).

  FIG. 6C is an example of a graph showing acceleration data measured by the acceleration sensor 17 when processing for correcting the influence of disturbance is performed on the acceleration data in FIG. 6A. In the part circled in FIG. 6C, the influence of the disturbance is removed, and the acceleration value in the Y-axis direction does not increase.

  FIG. 6D is a graph showing the angle data calculated by the calculation unit 18 using the acceleration measured in FIG. 6C. Since the acceleration is corrected, the pitch angle is not affected by the disturbance at the time of boarding in the part circled in FIG. 6D (the value of the angle is not increased).

  FIG. 7A is another example of a graph showing acceleration data measured by the acceleration sensor 17 when the moving body 10 gets on a moving sidewalk from a stationary sidewalk. Even in FIG. 7A, the measurement data of the acceleration sensor 17 is not corrected, and disturbances during boarding occur in the acceleration in the Y-axis direction in the portion circled in FIG. 7A.

  FIG. 7B is a graph showing the angle data calculated by the calculation unit 18 using the acceleration measured in FIG. 7A. In the pitch angle, a disturbance at the time of boarding occurs in a circled portion in FIG. 7B.

  FIG. 7C is an example of a graph showing acceleration data measured by the acceleration sensor 17 when processing for correcting the influence of disturbance is performed on the acceleration data in FIG. 7A. In the part circled in FIG. 7C, the influence of the disturbance is removed, and the acceleration value in the Y-axis direction does not increase.

  FIG. 7D is a graph showing the angle data calculated by the calculation unit 18 using the acceleration measured in FIG. 7C. Since the acceleration is corrected, the pitch angle is not affected by the disturbance at the time of boarding in the circled portion in FIG. 7D.

Case 2.
Next, a case different from the first case will be described. FIG. 8 is an example of a graph showing an ideal relationship between the angular velocity in the yaw direction measured by the gyroscope 15 and the lean angle of the moving body 10 detected by the direction indicating sensor 16 when there is no disturbance. The horizontal axis of the graph indicates the lean angle, and the vertical axis indicates the yaw angular velocity. In addition, the yaw angular velocity is a negative direction when the moving body 10 rotates to the right in the horizontal plane, and a positive direction when the mobile body 10 rotates to the left. The lean angle is defined as a minus direction when the mobile body 10 is tilted to the left with the front-rear axis as the axis, and as a positive direction when the mobile body 10 is tilted to the right with the front-rear axis as the axis. In this example, the maximum lean angle value that the moving body 10 can take is +13.5 deg, and the minimum lean angle value that can be taken is -13.5 deg. Further, the maximum angular velocity value in the yaw direction that can be taken by the moving body 10 is +130 deg / sec, and the minimum angular velocity value that can be taken is -130 deg / sec.

  As shown in FIG. 8, the relationship between the yaw angular velocity and the lean angle is substantially linear in a state where there is no disturbance. In other words, the change angle in the lean angle is proportional to the change angular velocity in the yaw angular velocity. Here, when the user performs a left turn by operating the moving body 10, not only the yaw angular velocity is generated in the positive direction but also the lean angle is generated in the negative direction. That is, the moving body 10 not only rotates to the left from the horizontal plane, but also tilts to the left with the front and rear as axes. When making a right turn, the yaw angular velocity and lean angle occur in the opposite directions. In FIG. 8, the fact that the yaw angular velocity and the lean angle have an inversely proportional (inverse correlation) relationship indicates the above.

  FIG. 9 is an image diagram showing a state when a strong wind blows from diagonally right behind when the user U is moving on the moving body 10. At this time, the path is shifted to the left side from the state in which the moving body 10 is traveling straight due to the strong wind.

  FIG. 10 is a graph of the angular velocity in the yaw direction measured by the gyroscope 15 and the lean angle detected by the direction indication sensor 16 in the state of FIG. (In FIG. 10, the angular velocity in the yaw direction is a graph indicated by a broken line and is described as “Yaw Gyro Value”. The lean angle is a graph indicated by a solid line and is described as “Lean Angle Value”. Note that the horizontal axis of the graph represents time, and the vertical axis represents the yaw angular velocity and lean angle (upward on the vertical axis is the plus direction, and down is the minus direction). The definitions of the positive direction and the negative direction of the yaw angular velocity and the lean angle are as described in FIG.

  As described above, the moving body 10 turns left due to the strong wind. Therefore, the yaw angular velocity to be measured by the gyroscope 15 becomes inaccurate due to the disturbance. That is, since the gyroscope 15 cannot distinguish between the angular velocity generated by the external factor and the angular velocity generated by the operation of the moving body 10, the angular velocity generated by the external factor is also measured as the angular velocity generated by the operation of the moving body 10. End up. This situation indicates that the moving body 10 turns to the left due to a disturbance factor even though there is no turning command based on the lean angle of the direction indicating sensor 16. For this reason, the moving body 10 may move in a direction not intended by the user.

  FIG. 11 is an example of a graph showing the relationship between the angular velocity in the yaw direction measured by the gyroscope 15 and the lean angle of the moving body 10 detected by the direction indicating sensor 16 when there is a disturbance in the turning direction. 11 is a graph showing an example of the angular velocity and lean angle in the yaw direction when there is a disturbance, and the graph shown by a broken line is the angular velocity in the yaw direction when there is no disturbance. It is the graph which showed the ideal relationship of the lean angle. When there is a disturbance, the angular velocity in the yaw direction and the lean angle are not in a linear relationship. And the deviation shown with the arrow in FIG. 11 arises compared with the case where there is no disturbance.

  In order to solve the above problems, the calculation unit 18 uses the angular velocity (yaw rate value) in the yaw direction measured by the gyroscope 15 as the lean angle (direction indication sensor value) of the moving body 10 detected by the direction indication sensor 16. Calculate the divided value. Then, the calculation unit 18 compares the calculated value with the value obtained by dividing the maximum value of the lean angle (maximum direction indicating sensor value) by the maximum angular velocity (maximum turning rate value) in the yaw direction, and the deviation ( It is compared whether or not the absolute value of the difference between the two is below a predetermined positive threshold value. Note that the maximum direction indication sensor value and the maximum angular velocity refer to the maximum direction indication sensor value and the angular velocity when the mobile body 10 turns without a disturbance.

  If the deviation is equal to or less than the threshold value, the calculation unit 18 determines that no disturbance has occurred, and the control unit 19 does not perform special control. When the deviation is larger than the threshold value, the calculation unit 18 calculates (estimates) a correction value of the turning amount (control value) of the moving body 10 using the deviation, and the control unit 19 moves using the correction value. The turning amount (control value) of the body 10 is corrected. In other words, the calculation unit 18 and the control unit 19 estimate the disturbance, and then correct the turning amount of the moving body 10 (feedback to the turning control of the moving body 10). Thereby, the control unit 19 can turn the moving body 10 along the lean angle detected by the direction indicating sensor 16.

FIG. 12 is another example of a graph showing the relationship between the angular velocity in the yaw direction measured by the gyroscope 15 and the lean angle of the moving body 10 detected by the direction indicating sensor 16 when there is a disturbance in the turning direction. is there. Black dots in FIG. 12 indicate data on angular velocity and lean angle in the yaw direction when there is a disturbance. The graph shown by the broken line is a graph showing an ideal relationship between the angular velocity in the yaw direction and the lean angle in a state where there is no disturbance. Due to the disturbance, the angular velocity and the lean angle in the yaw direction in the measurement result do not transition on the broken line in FIG. Here, the inclination m of the lean angle with respect to the angular velocity in the yaw direction when there is no disturbance is
m = maximum direction indicating sensor value / maximum yaw rate value = tan (a) (Equation 1)
Is calculated as The slope m ′ of the lean angle with respect to the angular velocity in the yaw direction in the measurement result is
m ′ = direction indicating sensor value / yaw rate value = tan (b) (Expression 2)
Is calculated as Note that a and b are angles indicating inclination. After calculating the slopes m and m ′, the calculation unit 18 determines whether or not the deviation between m and m ′ is equal to or less than a predetermined positive threshold value.

  The details of the disturbance in Case 1 and Case 2 and the processing performed by the calculation unit 18 and the control unit 19 have been described above. In Case 1, the measurement data of the acceleration sensor is affected by the disturbance, and the measurement data of the gyro sensor is not affected by the disturbance. In case 2, it is the measurement data of the yaw gyro that is affected by the disturbance, and the detection data of the lean angle value is not affected by the disturbance. In both cases 1 and 2, the disturbance in the measurement value can be corrected by the same algorithm.

  FIG. 13 is a flowchart illustrating an example of the control of the moving body 10. Hereinafter, control of the moving body 10 will be described with reference to FIG.

  First, the gyroscope 15 measures the value of the angular velocity (particularly, the angular velocity in the pitch direction and the angular velocity in the yaw direction) of the moving body 10 (step S1). The calculating unit 18 calculates the pitch direction angle (first angle) of the moving body 10 by integrating the angular velocity in the pitch direction measured by the gyroscope 15. The calculation unit 18 calculates the yaw direction angle of the moving body 10 based on the angular velocity in the yaw direction measured by the gyroscope 15 (step S2).

  The direction indication sensor 16 detects the lean angle (direction indication sensor value) of the moving body 10 (step S3). The acceleration sensor 17 detects the acceleration of the moving body 10 (step S4).

  The calculation unit 18 calculates an angle (second angle) in the pitch direction of the moving body 10 from the X-axis, Y-axis, and Z-axis accelerations measured by the acceleration sensor 17 (step S5). In addition, the order of the process of the above steps S1-S5 is not restricted to this order, It can change suitably.

  The calculation unit 18 compares the calculated first angle and second angle, and determines whether there is a deviation larger than a predetermined threshold between the first angle and the second angle (step S6). . When the first angle and the second angle have a deviation larger than a predetermined threshold (Yes in step S6), the calculation unit 18 uses the deviation between the first angle and the second angle to detect the acceleration sensor. 17, the deviation of the acceleration caused by the disturbance is estimated, and the control unit 19 corrects the measurement result of the acceleration sensor 17 using the deviation estimated by the calculation unit 18 (step S7).

  When the deviation between the first angle and the second angle is equal to or less than a predetermined threshold (No in step S6) or when the measurement result of the acceleration sensor 17 is corrected in step S7, the calculation unit 18 The direction indication sensor value / yaw rate value is compared with the maximum direction indication sensor value / maximum yaw rate value to determine whether there is a deviation greater than a predetermined threshold (step S8).

  When there is a deviation larger than a predetermined threshold value between the direction indication sensor value / yaw rate value and the maximum direction indication sensor value / maximum yaw rate value (Yes in step S8), the calculation unit 18 calculates the deviation. The correction value of the turning amount (control value) of the moving body 10 is estimated using the correction value, and the control unit 19 corrects the turning amount of the moving body 10 using the correction value estimated by the calculation unit 18 (step S9). Then, the calculation unit 18 resets the integral value (first angle value) (step S10).

  Note that the determination process in step S8 may be executed simultaneously with or before the determination process in step S6. The process of step S7 may be executed simultaneously with or before the process of step S9. Moreover, the process of step S10 may be performed before step S8 and step S9.

  And the control part 19 is the attitude | position which the moving body 10 should take based on the data of the angular velocity acquired from the gyroscope 15, and the acceleration (acceleration after correction | amendment when corrected in step S7) acquired from the acceleration sensor. Calculate the angle. The control unit 19 controls the wheels 14 (speed and direction control) based on the calculated attitude angle. Furthermore, when the turning amount of the moving body 10 is corrected in step S9, the control unit 19 controls the wheels 14 based on the corrected turning amount.

  As described above, the moving body 10 according to the first embodiment has a large deviation between the pitch angle calculated based on the speed measured by the gyroscope 15 and the pitch angle calculated based on the acceleration measured by the acceleration sensor 17. When there is a noise, it can be detected that a disturbance has been applied, and the measurement result of acceleration can be corrected using the deviation. Thereby, the attitude | position and driving | running | working of the moving body 10 can be controlled.

  In an inverted type moving body (especially an inverted two-wheeled moving body), the movement of the floor surface can be a disturbance of travel control in a place where the floor surface relatively moves such as when moving on a moving sidewalk or a train. As a countermeasure, it is conceivable to estimate the disturbance of the moving floor (moving sidewalk, escalator, etc.) using information on the rotational speed of the wheels. However, the non-holonomic inverted moving body is dragged toward the moving sidewalk having a high friction coefficient μ when entering the moving sidewalk obliquely. As described above, the inverted moving body cannot estimate the acceleration of the disturbance from the measured value (for example, the rotation speed) regarding the wheel in the dragged state.

  In the moving body 10 according to the first embodiment, since the disturbance (floor surface movement) is estimated without using the measurement values related to the wheels, the estimation accuracy of the disturbance can be improved. Further, since only the sensors used in the existing technology such as a gyroscope, a direction indicating sensor, and an acceleration sensor are used for estimating the disturbance, the disturbance correction can be performed without additional cost.

  Furthermore, when a disturbance cannot be detected, it is assumed that the moving body outputs more torque than necessary in order to cancel the posture change of itself. However, since the moving body 10 according to the first embodiment outputs only a necessary amount of torque (it is not necessary to have more torque output capability than necessary), it can contribute to downsizing of the moving body 10.

  In addition, an inverted type moving body (especially a lightweight and compact moving body) may change the moving direction of the vehicle body due to the influence of the disturbance when a strong disturbance such as wind is applied. Even in such a case, since the moving body 10 according to the first embodiment can correct the turning amount, the vehicle body can be directed in the direction intended by the user.

[Embodiment 2]
The second embodiment of the present invention will be described below. The appearance of the moving body, the configuration of the sensor, and the calculation unit according to the second embodiment are the same as those of the first embodiment, and thus description thereof is omitted.

  The gyroscope 15 of the moving body 10 measures the angular velocity of the moving body 10, and the acceleration sensor 17 measures the acceleration in the vertical direction of the moving body 10.

  The calculation unit 18 integrates the angular velocity of the moving body 10 measured by the gyroscope to calculate the attitude angle (first angle) of the moving body 10. Furthermore, the calculation unit 18 calculates the posture angle (second angle) of the moving body 10 using the acceleration of the moving body 10 measured by the acceleration sensor 17.

  The calculation unit 18 compares the first angle and the second angle, and determines whether there is a deviation larger than a predetermined threshold value between the first angle and the second angle. When the control unit 19 determines that the first angle and the second angle have a deviation larger than a predetermined threshold (when it is estimated that a disturbance has occurred), the calculation unit 18 calculates the first angle and the second angle. The deviation of the acceleration due to the disturbance in the acceleration sensor 17 is estimated using the deviation from the angle of 2. The control unit 19 corrects the measurement result of the acceleration sensor 17 (that is, the acceleration in the vertical direction) using the acceleration deviation estimated by the calculation unit 18.

  The logic of the above processing is the same as that of the calculation of the angle in the pitch direction in the first embodiment, the comparison, and the correction of the acceleration in the Y-axis direction measured by the acceleration sensor 17. By performing this process, it is possible to estimate a disturbance that occurs when the vehicle moves when the moving body 10 rides on a vehicle such as an elevator that moves in the vertical direction. The same algorithm can be used not only for vehicles that move only in the vertical direction but also for ascending escalators and descending escalators. Thereby, inversion control according to disturbance can be performed.

  Note that the present invention is not limited to the above-described embodiment, and can be changed as appropriate without departing from the spirit of the present invention. For example, the process of Embodiment 1 and the process of Embodiment 2 can be performed in appropriate combination.

  In the first embodiment, the calculation unit 18 compares the inclination between the direction indication sensor value and the yaw rate value. However, the moving body 10 has a table (comparison table) of values that the direction indication sensor value and the yaw rate value when there is no disturbance in the memory map, and the control unit 19 refers to the comparison table, The ideal direction indication sensor value and yaw rate value data may be compared with the direction indication sensor value and yaw rate value data of the measurement result.

  Although the control in the pitch angle direction has been described in the first embodiment, the same control can be performed in an angular direction other than the pitch angle direction.

DESCRIPTION OF SYMBOLS 10 Mobile body 11 Handle 12 Frame 13 Step 14 Wheel 15 Gyroscope 16 Direction indication sensor 17 Acceleration sensor 18 Calculation part 19 Control part

Claims (1)

An inverted mobile,
A gyroscope for detecting the yaw angular velocity of the inverted moving body;
A direction indicating sensor for detecting a lean angle of the inverted moving body;
Wherein compares the value of the direction indication sensor divided by lean angle detected by the yaw angular velocity gyroscopes detects a value obtained by dividing the maximum value of the lean angle at maximum angular velocity of the yaw direction, a deviation between Calculating a correction value of the turning amount of the inverted mobile body using the deviation when it is determined that the deviation is larger than the threshold ;
A control unit that corrects the turning amount of the inverted moving body using the correction value calculated by the calculation unit;
Inverted mobile body with

Images