JP2010271918A - Mobile unit, correction value calculation method and program - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To reduce the influence due to errors that may occur in an output value from an acceleration sensor, and control the mobile unit more accurately. <P>SOLUTION: An inverted two-wheel robot 100 is an mobile unit that spatially moves, in a state where a relative position of a body 50 is controlled with respect to wheels 51. The inverted two-wheel robot 100 includes a sensor 10 detecting the angular velocity of its body 50; an encoder 15 measuring variations in the relative position between the body 50 and the wheel 51, in a rotational direction of the wheel 51; and an instruction value calculator 20 that generates an instruction for controlling the relative position between the wheel 51 and the body 50, in a condition where the error included in the output value of the sensor 10 is corrected, according to a correction value preliminarily calculated, on the basis of each of outputs from the sensor 10 and the encoder 15. According to the adoption of this configuration, deterioration in controllability of the robot 100 due to the deterioration in the output value from the sensor 10 is suppressed. <P>COPYRIGHT: (C)2011,JPO&INPIT

Description

  The present invention relates to a moving object, a correction value calculation method, and a program.

  In recent years, with the development of robot technology, technology related to robots used for human space movement has also been dramatically advanced. Various electric / mechanical components such as various sensors, a microcomputer, a power source, and a link mechanism are built in a housing of a robot used as a moving body.

  Such a mobile robot is strongly required to have stability / safety during movement. In order to ensure safety / stability during movement of the robot, it is strongly required that the posture control of the robot be executed with high accuracy based on the input of various sensors. In general, an angular velocity sensor (hereinafter also referred to as a gyro sensor) and an acceleration sensor are used for posture control of a moving body. In order to reduce the cost of the moving body, it is required to use an inexpensive gyro sensor.

  An inexpensive gyro sensor uses MEMS (Micro-Electro Mechanical Systems). Specifically, a three-dimensional structure is formed with high definition on a substrate such as a semiconductor or glass by utilizing semiconductor process technology. In such a gyro sensor, generally, the Coriolis force received by the gyro sensor is electrically detected by electrically detecting the spatial displacement of the vibrating body. For example, the Coriolis force is electrically detected by placing the vibrating body in a vibrating state and detecting a change in capacitance value between the vibrating body and the detection electrode. Various types of gyro sensor structures and Coriolis force detection methods have been developed.

  Patent Document 1 discloses an inverted wheel type traveling body, and particularly discloses a technique for suddenly braking the vehicle body of an inverted wheel type traveling body without greatly inclining backward. Patent Document 2 discloses a technique for ensuring a safe and comfortable posture on a wheelchair even in a steeply inclined region or rough terrain.

JP 2007-203965 A JP 2000-116718 A

  As described above, in order to reduce the price of a moving body, it is required to employ an inexpensive gyro sensor. However, with an inexpensive gyro sensor, its output value may deteriorate over time. For example, due to the influence of vibration of a vibrating body incorporated in the gyro sensor, parts in the gyro sensor may be partially worn and an error may occur in the output value of the gyro sensor. In addition, an error may occur in the output value of the gyro sensor due to the influence of other parts (for example, a heat source) incorporated in the housing of the robot. When the output value of the gyro sensor deteriorates, it is impeded to realize the attitude control of the mobile robot with high accuracy.

  As is clear from the above description, there is a strong demand to control the moving body with high accuracy by reducing the influence of errors that may occur in the output value of the acceleration sensor.

  The moving body according to the present invention is a moving body that moves spatially in a state where the relative position of the main body portion is controlled with respect to the wheel portion, the detection portion that detects the angular velocity of the main body portion, and the rotation of the wheel portion A measurement unit that measures a change in relative position between the main body unit and the wheel unit in a direction, and a detection value of the detection unit according to a correction value calculated in advance based on each output of the detection unit and the measurement unit A command generation unit configured to generate a command for controlling a relative position between the wheel unit and the main body unit under a condition in which an error included in the output value is corrected. By adopting this configuration, it is possible to reduce the influence of an error that may occur on the output value of the acceleration detection unit, and to control the moving body with high accuracy.

  Based on the output of the measurement unit, the timing at which the main body unit is positioned in the vertical direction is calculated, and based on the calculated timing, the variation in the output value of the detection unit is evaluated to calculate the correction value. It is preferable to further include a correction value calculation unit.

  The correction value calculation unit may calculate the correction value based on outputs of the detection unit and the measurement unit acquired during a period in which the main body unit rotates with the wheel unit in a fixed state as a rotation center.

  The correction value calculation unit includes at least a moment of inertia of the main body, a characteristic value of a drive source that relatively moves between the wheel unit and the main body, a torque generated by the drive source, and an output value of the measurement unit It is preferable to estimate the external force applied to the main body based on the angle value determined from

  The correction value calculation unit may detect a timing at which the calculated value of the external force becomes zero as a timing at which the main body unit is positioned in the vertical direction.

  The detection unit also detects acceleration of the main body, and the command generation unit evaluates the posture of the main body based on the output of the sensor and calculates a posture angle of the main body. And a second calculation unit that generates the command based on the outputs of the first calculation unit and the measurement unit.

  The correction value calculation unit may calculate the correction value by evaluating a variation in the output value of the first calculation unit based on the calculated timing.

  The detection unit may include a mechanism for detecting Coriolis force.

  It is preferable to further include a correction value holding unit that holds the correction value.

  The main body may rotate at a substantially constant speed with the wheel portion as a rotation center.

  The moving body according to the present invention is a moving body that moves spatially in a state where the relative position of the main body portion is controlled with respect to the wheel portion, the detection portion that detects the angular velocity of the main body portion, and the rotation of the wheel portion A measurement unit that measures a change in relative position between the main body unit and the wheel unit in a direction, and a timing at which the main body unit is positioned in the vertical direction based on an output of the measurement unit, A correction value calculating unit that evaluates fluctuations in the output value of the detection unit based on the timing and calculates a correction value for correcting an error included in the output value of the detection unit.

  The correction value calculation method according to the present invention is a correction value for correcting an output value of an angular velocity sensor unit incorporated in a moving body that moves spatially with a relative position of a main body unit controlled with respect to a wheel unit. In the calculation method, an output of the angular velocity sensor unit is acquired during a period in which the main body unit rotates with the wheel unit in a fixed state as a rotation center, and the main body unit rotates with the wheel unit in a fixed state as a rotation center. In a period, an output of a measurement unit that measures a change in relative position between the main body unit and the wheel unit in a rotation direction of the wheel unit is acquired, and the main body unit is in a vertical direction based on the output of the measurement unit The timing positioned above is calculated, and based on the calculated timing, the fluctuation of the output value of the angular velocity sensor unit is evaluated, and a correction value for correcting the output value of the angular velocity sensor unit is calculated.

  A program according to the present invention causes a computer to calculate a correction value for correcting an output value of an angular velocity sensor unit incorporated in a moving body that moves spatially with a relative position of a main body unit controlled with respect to a wheel unit. The timing at which the main body is positioned in the vertical direction is determined based on the output value of the measurement unit that measures the change in the relative position between the main body and the wheel in the rotation direction of the wheel. Based on the calculated timing, the fluctuation value of the angular velocity sensor unit is evaluated to calculate the correction value.

  ADVANTAGE OF THE INVENTION According to this invention, the influence of the error which can arise in the output value of an acceleration sensor can be reduced, and a moving body can be controlled more accurately.

It is a schematic schematic diagram of the moving body concerning 1st Embodiment of this invention. It is a schematic block diagram of the moving body concerning a 1st embodiment of the present invention. It is explanatory drawing which shows the normal operation | movement of the moving body concerning 1st Embodiment of this invention. It is explanatory drawing which shows the attitude | position control of the moving body concerning 1st Embodiment of this invention. It is a schematic flowchart which shows the offset value calculation operation | movement of the moving body concerning 1st Embodiment of this invention. It is a schematic perspective view of the jig | tool used at the time of the offset value calculation operation | movement of the moving body concerning 1st Embodiment of this invention. It is a schematic diagram which shows the offset value calculation operation | movement of the moving body concerning 1st Embodiment of this invention. It is a schematic diagram which shows the time fluctuation of the output value of the encoder part of the moving body concerning 1st Embodiment of this invention. It is a schematic diagram which shows the time fluctuation of the external force value which the mobile body concerning 1st Embodiment of this invention calculated. It is a schematic diagram which shows the time fluctuation | variation of the attitude | position angle which the mobile body concerning 1st Embodiment of this invention calculated. It is a schematic schematic diagram which shows the calculation method of the correction value of the moving body concerning 1st Embodiment of this invention.

  Hereinafter, embodiments of the present invention will be described with reference to the drawings. Each embodiment is simplified for convenience of explanation. Since the drawings are simple, the technical scope of the present invention should not be interpreted narrowly based on the drawings. The drawings are only for explaining the technical matters, and do not reflect the exact sizes or the like of the elements shown in the drawings. The same elements are denoted by the same reference numerals, and redundant description is omitted. Words indicating directions such as up, down, left, and right are used on the assumption that the drawing is viewed from the front.

[First Embodiment]
The first embodiment of the present invention will be described below with reference to FIGS. FIG. 1 is a schematic diagram of a moving body. FIG. 2 is a schematic block diagram of the moving body. FIG. 3 is an explanatory diagram showing a normal operation of the moving body. FIG. 4 is an explanatory diagram illustrating posture control of the moving body. FIG. 5 is a schematic flowchart showing an offset value calculation operation. FIG. 6 is a schematic perspective view of a jig used during the offset value calculation operation. FIG. 7 is a schematic diagram showing an offset value calculation operation. FIG. 8 is a schematic diagram showing temporal variation of the output value of the encoder unit. FIG. 9 is a schematic diagram showing temporal variation of the external force calculated by the moving body. FIG. 10 is a schematic diagram illustrating temporal variation of the posture angle calculated by the moving body. FIG. 11 is a schematic diagram illustrating a correction value calculation method.

  As shown in FIG. 1, an inverted two-wheeled robot (hereinafter also simply referred to as a robot) 100 is a moving body on which a person is boarded, and has a main body 50 and a pair of wheels (wheels) 51. . The main body 50 includes a standing part 50a, a seating part 50b, and a footrest 50c. The set of wheels 51 includes a wheel 51a and a wheel 51b.

  The main body 50 incorporates various electric / mechanical components such as various sensors, a microcomputer, a power supply, and a link mechanism. The seating part 50b is a part on which a passenger sits. The standing portion 50a is a portion that receives the back of the passenger. The footrest 50c is a portion on which the passenger's feet are placed. The wheel 51a and the wheel 51b are arranged on a common axis AX. The rotation axis of the wheel 51a coincides with the axis AX. The rotation axis of the wheel 51b coincides with the axis AX.

  As schematically illustrated in FIG. 1, the robot 100 includes a motor unit 53, a controller 54, a power supply 55, and an input unit 56. The motor unit 53 includes a motor 53a and a motor 53b corresponding to the wheel 51a and the wheel 51b. The rotation axes of the motors 53a and 53b are arranged so as to coincide with the axis AX. The output of the power supply 55 is supplied to the controller 54. Moreover, the output of the power supply 55 is supplied to the input part 56 as needed. The output of the controller 54 is connected to the motor unit 53.

  The controller 54 supplies a command value calculated according to an input value from the input unit 56 to the motor unit 53. The motor 53a rotates the wheel 51a according to the command value supplied from the controller 54. Similarly, the motor 53b rotates the wheel 51b according to the command value supplied from the controller 54.

  The input unit 56 corresponds to an input device such as an angular velocity sensor, an acceleration sensor, or an operator. The motor 53a and the motor 53b are individually controlled by the controller 54. By making it possible to individually control the rotation of the wheels 51a and 51b, the operator can move in the space in any direction while riding the inverted two-wheeled robot 100.

  The specific configuration of the inverted two-wheeled robot 100 is arbitrary. There is no need for a plurality of wheels 51. The connection method between the main-body part 50 and the wheel 51 is arbitrary. The arrangement positions of the motors 53a and 53b are arbitrary.

  Referring to FIG. 2, a schematic block diagram of the robot 100 is shown. As shown in FIG. 2, the robot 100 includes a sensor unit (detection unit) 10, an encoder unit (measurement unit) 15, a command value calculation unit (command generation unit) 20, an offset value calculation unit (correction value calculation unit) 30, An offset value storage unit (correction value storage unit) 35 and a drive unit 40 are included. The sensor unit 10 and the encoder unit 15 are included in the input unit 56 of FIG. The drive unit 40 corresponds to the motor unit 53 in FIG. The command value calculation unit 20 is included in the controller 54 of FIG. The command value calculation unit 20 and the offset value calculation unit 30 can be realized by either hardware / software. More preferably, the functions of the command value calculation unit 20 and the offset value calculation unit 30 may be realized by executing a program stored in a storage such as a hard disk by a CPU (Central Processing Unit).

  The sensor unit 10 includes an angular velocity sensor unit 11 and an acceleration sensor unit 12. The encoder unit 15 includes a right wheel encoder 16 and a left wheel encoder 17. The command value calculation unit 20 includes a posture angle calculation unit (first calculation unit) 21 and an inversion control calculation unit (second calculation unit) 22. The offset value calculation unit 30 includes an external force estimation execution unit 31 and an offset value calculation execution unit 32. The drive unit 40 includes a right wheel drive unit 41 and a left wheel drive unit 42. The angular velocity sensor unit 11 is sometimes called a gyro sensor.

  First, the connection relationship between the above-described elements will be described. The output of the angular velocity sensor unit 11 is connected to the attitude angle calculation unit 21. The output of the acceleration sensor unit 12 is connected to the attitude angle calculation unit 21. The output of the attitude angle calculation unit 21 is connected to the inverted control calculation unit 22. The output of the inverted control calculation unit 22 is connected to the right wheel drive unit 41 and the left wheel drive unit 42. The output of the right wheel encoder 16 is connected to the inverted control calculation unit 22 and is also connected to the external force estimation execution unit 31. The output of the left wheel encoder 17 is connected to the inversion control calculation unit 22 and also connected to the external force estimation execution unit 31. The output of the external force estimation execution unit 31 is connected to the offset value calculation execution unit 32. The output of the attitude angle calculation unit 21 is also connected to the offset value calculation execution unit 32. The output of the offset value calculation execution unit 32 is connected to the offset value storage unit 35. The output of the offset value storage unit 35 is connected to the attitude angle calculation unit 21.

  As shown in FIG. 3, the robot 100 sequentially performs operations such as sensing (s101), command generation (s102), and drive (s103). Specifically, it is as follows.

  The sensor unit 10 performs sensing at a predetermined timing (s101). The angular velocity sensor unit 11 acquires an angular velocity. The acceleration sensor unit 12 acquires acceleration. The encoder unit 15 acquires the rotation amount of the wheel 51. Next, the command value calculation unit 20 calculates a command value according to the output values of the angular velocity sensor unit 11, the acceleration sensor unit 12, and the encoder unit 15 (s102). Next, the drive unit 40 individually controls the rotation of the wheels 51a and 51b in accordance with the output value from the command value calculation unit 20 (s103). The robot 100 after driving performs each operation such as sensing, command generation, and driving in the same manner as described above.

  By repeating such a feedback operation, the robot 100 can maintain an inverted state, for example. As schematically shown in FIG. 4, in order to maintain the robot 100 in an inverted state, the value of the angle θ1 formed by the fixed axis AX2 of the robot 100 along the axis along the vertical direction becomes zero (θ1 = 0 °). The command value is generated as follows. If the command value is calculated in consideration of the direction instruction input by the pilot, the robot 100 can be moved to the position intended by the pilot.

  In the present embodiment, the angular velocity sensor unit 11 is a sensor utilizing MEMS (Micro-Electro Mechanical Systems). Specifically, the angular velocity sensor unit 11 is manufactured by forming a three-dimensional structure with high definition on a substrate such as a semiconductor or glass by utilizing semiconductor process technology. The angular velocity sensor unit 11 electrically detects the Coriolis force received by the gyro sensor by electrically detecting the spatial displacement of the vibrating body arranged on the substrate. For example, the angular velocity sensor unit 11 electrically detects the Coriolis force by setting the vibrating body in a vibrating state and detecting a change in the capacitance value between the vibrating body and the detection electrode. The specific structure of the gyro sensor and the method for detecting the Coriolis force are arbitrary.

  In order to realize highly accurate posture control of the robot 100, it is required that the output value of the angular velocity sensor unit 11 be zero when the natural axis AX2 of the robot 100 coincides with the axis along the vertical direction. However, the output value of the angular velocity sensor unit 11 may deteriorate with time due to various factors. If the output value of the angular velocity sensor unit 11 includes an error, when it is determined that the robot 100 is standing in the vertical direction, the body unit 50 is actually tilted forward or diagonally as viewed from the position in the vertical direction. There is a risk of leaning back. In such a case, when the passenger rides on the robot 100 and moves, the passenger may feel physical stress or it may be difficult to maintain his / her own stability. Further, since the output value of the acceleration sensor unit 12 deteriorates with time, the need for regular maintenance of the robot 100 may increase. If the replacement of the angular velocity sensor unit 11 is assumed, the management cost of the robot 100 may increase.

  In the present embodiment, in view of these points, a correction value for correcting the output value of the angular velocity sensor unit 11 is calculated. Then, based on the calculated correction value, the influence of the error included in the output value of the angular velocity sensor unit 11 is reduced. Specifically, the command value transmitted to the drive unit 40 is calculated under the condition that the output value of the angular velocity sensor unit 11 is corrected by the correction value. Thereby, it is possible to prevent the posture control accuracy of the robot 100 from deteriorating due to an error included in the output value of the angular velocity sensor unit 11. Hereinafter, the configuration and operation of the robot 100 will be described more specifically based on the above points.

  As described above, the angular velocity sensor unit 11 is a sensor utilizing MEMS, detects the angular velocity, and outputs a value corresponding to the angular velocity. It is assumed that the angular velocity sensor unit 11 outputs a digital value after converting an analog value into a digital value. In addition, the angular velocity sensor unit 11 detects rotation in three axis directions of roll, pitch, and yaw as an angular velocity.

  Similar to the angular velocity sensor unit 11, the acceleration sensor unit 12 is a sensor that utilizes MEMS, detects acceleration, and outputs a value corresponding to the detected acceleration. The acceleration sensor unit 12 converts an analog value into a digital value and outputs a digital value generated by A / D conversion. In addition, the acceleration sensor unit 12 detects acceleration in three axis directions of roll, pitch, and yaw. The principle of the acceleration sensor unit 12 is well known to those skilled in the art, and a description thereof will be omitted.

  The posture angle calculation unit 21 calculates a posture angle based on the angular velocity value and the acceleration value transmitted from the angular velocity sensor unit 11 and the acceleration sensor unit 12. As is well known, the angle is calculated by integrating the acceleration values. The speed is calculated by integrating the acceleration values. The traveling direction of the robot 100 is calculated from the acceleration value. The posture angle calculation unit 21 calculates a posture angle from these values. Considering the point at which the current posture angle is output by the posture angle calculation unit 21, the sensor unit 10 and the posture angle calculation unit 21 can be comprehensively understood as a gyro sensor unit.

  The posture angle indicates an angle formed by the natural axis of the robot 100 with respect to the axis along the vertical direction, and corresponds to θ1 shown in FIG. In FIG. 4, the y axis is an axis along the vertical direction, and the axis AX <b> 2 corresponds to a natural axis of the robot 100. The robot 100 has a frame bar 52 corresponding to the natural axis. The method for defining the posture angle is arbitrary, and this may be defined by other methods.

  In the present embodiment, the posture angle calculation unit 21 calculates the posture angle on the condition that the error included in the output value of the angular velocity sensor unit 11 is corrected according to the offset value stored in the offset value storage unit 35. For example, the offset value is subtracted from the angle value calculated by integrating the output value of the angular velocity sensor unit 11. As a result, it is possible to prevent the posture control accuracy of the robot 100 from deteriorating due to an error occurring in the output value of the angular velocity sensor unit 11. When the offset value is not stored in the offset value storage unit 35, the posture angle calculation unit 21 calculates the posture angle based on the angular velocity value and the acceleration value as described above without considering the offset value.

  The inverted control calculation unit 22 calculates a command value for controlling the robot 100 based on the posture angle calculated by the posture angle calculation unit 21 and the output value from the encoder unit 15. Note that the inverted control calculation unit 22 can also advance the robot 100 in accordance with the output value of the operator by the operator.

  The inverted control calculation unit 22 detects the current posture of the robot 100 based on the output of the posture angle calculation unit 21. The inverted control calculation unit 22 detects a relative position change between the main body unit 50 and the wheel 51 in the rotation direction of the wheel 51 based on the output of the encoder unit 15.

  The specific calculation procedure of the inverted control calculation unit 22 is arbitrary. For example, the inverted control calculation unit 22 generates a command value so that the output value of the sensor unit 10 approaches a preset target value. Similarly, the inverted control calculation unit 22 generates a command value so that the output value of the encoder unit 15 approaches a preset target value. As a specific example, in order to maintain the inverted state in a stationary state, the inversion control calculation unit 22 is set so that the output value of the angular velocity sensor unit 11 = 0, and the output values of the right wheel encoder 16 and the left wheel encoder 17 = 0. Generate a command value. When the current posture angle calculated by the posture angle calculation unit 21 is as shown in FIG. 4, the inversion control calculation unit 22 performs control to rotate the wheels 51 a and 51 b backward. In order to move upside down and move forward, the inversion control calculation unit 22 sets the command value so that the output value of the angular velocity sensor unit 11 is 0, and the output values of the right wheel encoder 16 and the left wheel encoder 17 are integers. Generate. As described above, the specific calculation method of the inversion control calculation unit 22 is arbitrary. For example, the inversion control calculation unit 22 converts the input values from the attitude angle calculation unit 21 and the encoder unit 15 into a predetermined relational expression. The command value is calculated by inputting.

  The right wheel drive unit 41 rotates the right wheel by an arbitrary amount of rotation in an arbitrary direction according to the command value transmitted from the inversion control calculation unit 22. Similarly to the right wheel drive unit 41, the left wheel drive unit 42 also rotates the left wheel by an arbitrary amount of rotation in an arbitrary direction according to the command value transmitted from the inversion control calculation unit 22.

  The right wheel encoder 16 detects the relative rotation amount of the right wheel and outputs a count value corresponding to the rotation amount. The specific configuration of the right wheel encoder 16 is arbitrary. For example, either an optical or magnetic encoder may be employed. In the case of an optical encoder, the right wheel encoder 16 optically detects the number of slits formed in the outer peripheral area of the rotating body provided on the wheel, and outputs a count value corresponding to the number of slits. In the case of a magnetic encoder, the right wheel encoder 16 may magnetically detect the number of magnets sequentially arranged in the outer peripheral area of the rotating body and output a count value corresponding to the number of magnets.

  The left wheel encoder 17 is the same as the right wheel encoder 16. However, the rotation amount of the left wheel is detected instead of the rotation amount of the right wheel.

  As will be apparent from the following description, the offset value calculation unit 30 estimates a change in the external force received by the main body unit 50 based on the count value output from the encoder unit 15, and at the timing when the external force becomes zero, the posture angle The offset value is calculated by evaluating the fluctuation of the output of the calculation unit 21. The offset value storage unit 35 stores the offset value calculated by the offset value calculation unit 30. The offset value storage unit 35 is a general storage element such as a register.

  Next, an offset value calculation method will be described with reference to FIGS. From these descriptions, the operation of the offset value calculation unit 30 becomes clear.

  As described above, the inexpensive angular velocity sensor unit 11 may cause an error in the output value over time. The offset value is calculated in order to avoid an adverse effect due to an error in the output value of the angular velocity sensor unit 11. Therefore, the time point at which the offset value is calculated is usually after a considerable period has elapsed after the robot 100 is shipped. However, an offset value may be obtained immediately after the manufacture of the robot 100 to inspect whether the robot 100 has an initial defect. That is, the time point at which the offset value is calculated is arbitrary. It is assumed that no offset value is stored in the offset value storage unit 35.

  As shown in FIG. 5, first, initialization is performed (s201). Specifically, the robot 100 is fixed by the jig 60 shown in FIG. 6 as schematically shown in FIG.

  As shown in FIG. 6, the jig 60 includes a base plate 61, a column 62, and a convex portion 63. The jig 60 is manufactured by molding metal or the like, for example.

  As schematically shown in FIG. 7, the wheels 51 of the robot 100 are fixed by a pair of jigs 60. It is assumed that the wheel 51 is provided with a concave portion that receives the convex portion 63. In addition, you may position the jig | tool 60 and the wheel 51 by fitting by forming the convex part 63 in prismatic shape, and providing the recessed part corresponding to this in the wheel 51. FIG. In this case, the wheel 51 can be reliably fixed.

  Next, a rotation instruction is given (s202). Specifically, for example, in response to an instruction from a timing controller or the like, the inversion control calculation unit 22 generates a command and causes the drive unit 40 to generate torque. In this state, when a person releases his / her hand from the main body 50, the main body 50 moves from the position inclined most backward to the position inclined most forward, as shown schematically in FIG. Rotate. In the present embodiment, the inverted control calculation unit 22 generates a command so that the rotation speed of the main body 50 is constant.

  Next, sensing is performed (s203). Specifically, immediately after the rotation instruction, the angular velocity sensor unit 11 starts sensing according to the instruction of the timing controller. The same applies to the acceleration sensor unit 12. The same applies to the right wheel encoder 16 and the left wheel encoder 17.

  Next, an offset value calculation process is executed (s204). Specifically, it is as follows.

First, the external force estimation execution unit 31 estimates the external force by the following equation (1) based on the count value output from one or both of the right wheel encoder 16 and the left wheel encoder 17. However, each code | symbol is as follows. τe is a torque generated in the drive unit 40. I is the moment of inertia of the main body 50. q is an angle value calculated from the output value of the encoder unit 15. D is the viscosity coefficient of the motor (the characteristic characteristic value of the drive unit 40). τa is an external force (external force torque).
... (1)

  The count value output from the encoder unit 15 increases at a constant rate along the time axis as shown in FIG. This is because, as described above, the main body 50 rotates at a constant speed with the wheel 51 as the rotation center during the sensing period. The angle value q described above is determined from the count value at each time point. The angle value is relative angle information.

  Torque τe generated in the drive unit is calculated from a command value generated by the inverted control calculation unit 22. The moment of inertia I of the main body 50 and the viscosity coefficient D of the motor are known in advance.

  The external force τa is calculated by substituting the above-described value into the expression (1) and performing calculation processing. It is assumed that the count value is substituted into equation (1) in time series order. Thereby, the time change of the external force τa can be calculated. That is, it is possible to detect the time change of the external force shown in FIG. 9 based on the time change of the count value shown in FIG.

  After the external force is estimated, an offset value is calculated. With the above-described external force estimation process, it is possible to detect the temporal change of the external force shown in FIG. When the value of the external force is zero, it can be determined that the main body 50 is located on the axis along the vertical direction.

  On the other hand, at the time of sensing described above, the posture angle calculation unit 21 calculates the posture angle based on the output values of the angular velocity sensor unit 11 and the acceleration sensor unit 12 as shown in FIG. Then, the output value of the posture angle calculation unit 21 is supplied to the offset value calculation execution unit 32.

  As schematically shown in FIG. 11, the offset value calculation execution unit 32 calculates a time t1 when the external force becomes zero from the output value of the external force estimation execution unit 31, and the posture angle calculation unit 21 at the time t1 Calculate the output value. The offset value calculation execution unit 32 uses the calculated output value (−5 degrees in FIG. 11) of the posture angle calculation unit 21 at time t1 as an offset value. In this way, the offset value is calculated based on the output values of the encoder unit 15 and the sensor unit 10. The offset value calculation method is arbitrary. The offset value may be calculated based on the calculation of the difference between the time when the external force τa becomes zero and the time when the posture angle becomes 0 degrees.

  Next, the offset value is stored (s205). Specifically, the offset value calculation execution unit 32 transfers the calculated offset value to the offset value storage unit 35. The offset value storage unit 35 holds the offset value transmitted from the offset value calculation execution unit 32. By storing the offset value in the offset value storage unit 35, the posture angle calculation unit 21 calculates the posture angle on the condition that the error included in the output value of the angular velocity sensor unit 11 is corrected by the offset value as described above. To do.

  As is apparent from the above description, in the present embodiment, in consideration of the possibility that the output value of the angular velocity sensor unit 11 may deteriorate with time, the command is issued under the condition that the error included in the angular velocity sensor unit 11 is corrected. Generate and drive the drive unit. Thereby, the inexpensive angular velocity sensor unit 11 can be employed, and the cost of the robot 100 can be reduced.

  Further, in the present embodiment, in view of the fact that the value of the external force approaches zero when the main body 50 is positioned on the axis along the vertical direction, the external force is estimated based on the output value of the encoder unit 15, and the external force is The offset value is calculated by evaluating the time change of the posture angle based on the time when it becomes zero. As a result, the offset value can be calculated by utilizing the existing encoder unit 15, and the necessity of separately preparing new hardware can be eliminated. This is particularly advantageous in an inverted two-wheeled mobile body that also requires compactness. If the point at which the posture angle is determined based on the angular velocity is taken into account, it can be understood that the offset value is calculated by evaluating the fluctuation of the output value of the angular velocity sensor unit 11 based on the time when the external force becomes zero. .

  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. The specific structure of the angular velocity sensor unit 11 is arbitrary. The specific structure of the acceleration sensor unit 12 is also arbitrary. The specific calculation method of the command value calculation unit 20 is arbitrary. The specific calculation method of the offset value calculation unit 30 is arbitrary. The offset value storage unit 35 may be a removable memory device. The offset value calculation unit 30 does not need to be incorporated in the robot 100, but may be stored in an external personal computer and executed on the external personal computer. You may connect between the main-body part 50 and the wheel 51 via the link mechanism of multiple joints. The timing controller is optional. The offset value calculation operation may be executed a plurality of times, an average value of these may be calculated, and stored in the offset value storage unit 35. Further, according to the above description, the contents of the program that causes the computer to calculate the correction value are also obvious.

100 Inverted two-wheeled robot

DESCRIPTION OF SYMBOLS 10 Sensor part 11 Angular velocity sensor part 12 Acceleration sensor part

15 Encoder section 16 Right wheel encoder 17 Left wheel encoder

20 Command value calculator 21 Attitude angle calculator 22 Inverted control calculator

30 Offset value calculation unit 31 External force estimation execution unit 32 Offset value calculation execution unit

35 Offset value storage

40 Driving part 41 Right wheel driving part 42 Left wheel driving part

50 Main body 50a Standing part 50b Seating part 50c Footrest

51 Wheel 51a Wheel 51b Wheel

53 Motor section 54 Controller 55 Power supply 56 Input section

60 Jig 61 Base plate 62 Column 63 Projection

Claims (18)

A moving body that moves in space in a state in which the relative position of the main body is controlled with respect to the wheel,
A detector for detecting an angular velocity of the main body;
A measurement unit that measures a change in relative position between the main body unit and the wheel unit in the rotation direction of the wheel unit;
The relative position between the wheel unit and the main body unit is obtained under the condition that the error included in the output value of the detection unit is corrected according to the correction value calculated in advance based on the outputs of the detection unit and the measurement unit. A command generation unit that generates a command to control;
A moving body comprising:   Based on the output of the measurement unit, the timing at which the main body unit is positioned in the vertical direction is calculated, and based on the calculated timing, the variation in the output value of the detection unit is evaluated to calculate the correction value. The mobile body according to claim 1, further comprising a correction value calculation unit for performing the correction.   The correction value calculation unit calculates the correction value based on outputs of the detection unit and the measurement unit acquired during a period in which the main body unit rotates with the wheel unit in a fixed state as a rotation center. The moving body according to claim 2.   The correction value calculation unit includes at least a moment of inertia of the main body, a characteristic value of a drive source that relatively moves between the wheel unit and the main body, a torque generated by the drive source, and an output value of the measurement unit The moving body according to claim 3, wherein an external force applied to the main body portion is estimated based on an angle value determined from   5. The correction value calculation unit detects a timing at which the calculated value of the external force becomes zero as a timing at which the main body portion is positioned in the vertical direction. 6. Moving body. The detection unit also detects acceleration of the main body,
The command generation unit is configured to evaluate a posture of the main body unit based on an output of the detection unit and calculate a posture angle of the main body unit, and each of the first calculation unit and the measurement unit. The mobile body according to claim 1, further comprising: a second calculation unit that generates the command based on an output.   The mobile object according to claim 6, wherein the correction value calculation unit calculates the correction value by evaluating a variation in an output value of the first calculation unit based on the calculated timing.   The mobile unit according to claim 1, wherein the detection unit includes a mechanism that detects Coriolis force.   The mobile body according to claim 1, further comprising a correction value holding unit that holds the correction value.   The moving body according to any one of claims 1 to 9, wherein the main body portion rotates at a substantially constant speed with the wheel portion as a rotation center. A moving body that moves in space in a state in which the relative position of the main body is controlled with respect to the wheel,
A detector for detecting an angular velocity of the main body;
A measurement unit that measures a change in relative position between the main body unit and the wheel unit in the rotation direction of the wheel unit;
Based on the output of the measurement unit, the timing at which the main body unit is positioned in the vertical direction is calculated, and based on the calculated timing, the fluctuation of the output value of the detection unit is evaluated, and the output value of the detection unit A correction value calculation unit for calculating a correction value for correcting an error included in
A moving object comprising:   A command generation unit configured to generate a command for controlling a relative position between the wheel unit and the main body unit under a condition in which an error included in the output value of the detection unit is corrected according to the correction value; The moving body according to claim 11, wherein the moving body is characterized.   The correction value calculation unit calculates the correction value based on outputs of the detection unit and the measurement unit acquired during a period in which the main body unit rotates with the wheel unit in a fixed state as a rotation center. The moving body according to claim 11 or 12.   The correction value calculation unit includes at least a moment of inertia of the main body, a characteristic value of a driving source that relatively moves between the wheel unit and the main body, a torque generated by the driving source, and an output value of the measuring unit The moving body according to claim 13, wherein an external force applied to the main body portion is estimated based on an angle value determined from: A method of calculating a correction value for correcting an output value of an angular velocity sensor unit incorporated in a moving body that moves spatially in a state where the relative position of the main body unit is controlled with respect to the wheel unit,
The output of the angular velocity sensor unit is acquired during a period in which the main body unit rotates with the wheel unit in a fixed state as a rotation center,
The output of the measuring unit that measures the change in the relative position between the main body unit and the wheel unit in the rotation direction of the wheel unit is acquired during the period in which the main body unit rotates with the wheel unit in the fixed state as the rotation center. And
Calculate the timing at which the main body is positioned in the vertical direction based on the output of the measurement unit,
A correction value calculation method for evaluating a variation in an output value of the angular velocity sensor unit based on the calculated timing and calculating a correction value for correcting the output value of the angular velocity sensor unit.   The main body based on an inertial moment of the main body, a characteristic value of a driving source that relatively moves between the wheel portion and the main body, a torque generated by the driving source, and an angle value determined from an output value of the measuring section. The correction value calculation method according to claim 15, wherein an external force applied to the unit is estimated.   The correction value calculation method according to claim 15, wherein the angular velocity sensor unit includes a mechanism for detecting Coriolis force. A program for causing a computer to calculate a correction value for correcting an output value of an angular velocity sensor unit incorporated in a moving body that moves spatially with a relative position of a main body unit controlled with respect to a wheel unit,
Based on an output value of a measurement unit that measures a change in relative position between the main body unit and the wheel unit in the rotation direction of the wheel unit, the timing at which the main body unit is positioned in the vertical direction is calculated.
A program for evaluating the fluctuation of the output value of the angular velocity sensor unit based on the calculated timing and calculating the correction value.

Images