WO2013001658A1 - 速度計画装置を備えた倒立振子型移動体 - Google Patents

速度計画装置を備えた倒立振子型移動体 Download PDF

Info

Publication number
WO2013001658A1
WO2013001658A1 PCT/JP2011/066010 JP2011066010W WO2013001658A1 WO 2013001658 A1 WO2013001658 A1 WO 2013001658A1 JP 2011066010 W JP2011066010 W JP 2011066010W WO 2013001658 A1 WO2013001658 A1 WO 2013001658A1
Authority
WO
WIPO (PCT)
Prior art keywords
moving body
speed
pendulum type
inverted pendulum
type moving
Prior art date
Application number
PCT/JP2011/066010
Other languages
English (en)
French (fr)
Inventor
亮介 中村
梓 網野
泰士 上田
Original Assignee
株式会社日立製作所
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by 株式会社日立製作所 filed Critical 株式会社日立製作所
Priority to KR1020137034762A priority Critical patent/KR101567456B1/ko
Priority to JP2013522670A priority patent/JP5871925B2/ja
Priority to US14/125,192 priority patent/US9180926B2/en
Priority to DE112011105397.5T priority patent/DE112011105397B4/de
Publication of WO2013001658A1 publication Critical patent/WO2013001658A1/ja

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B62—LAND VEHICLES FOR TRAVELLING OTHERWISE THAN ON RAILS
    • B62K—CYCLES; CYCLE FRAMES; CYCLE STEERING DEVICES; RIDER-OPERATED TERMINAL CONTROLS SPECIALLY ADAPTED FOR CYCLES; CYCLE AXLE SUSPENSIONS; CYCLE SIDE-CARS, FORECARS, OR THE LIKE
    • B62K11/00—Motorcycles, engine-assisted cycles or motor scooters with one or two wheels
    • B62K11/007—Automatic balancing machines with single main ground engaging wheel or coaxial wheels supporting a rider
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60L—PROPULSION OF ELECTRICALLY-PROPELLED VEHICLES; SUPPLYING ELECTRIC POWER FOR AUXILIARY EQUIPMENT OF ELECTRICALLY-PROPELLED VEHICLES; ELECTRODYNAMIC BRAKE SYSTEMS FOR VEHICLES IN GENERAL; MAGNETIC SUSPENSION OR LEVITATION FOR VEHICLES; MONITORING OPERATING VARIABLES OF ELECTRICALLY-PROPELLED VEHICLES; ELECTRIC SAFETY DEVICES FOR ELECTRICALLY-PROPELLED VEHICLES
    • B60L15/00—Methods, circuits, or devices for controlling the traction-motor speed of electrically-propelled vehicles
    • B60L15/20—Methods, circuits, or devices for controlling the traction-motor speed of electrically-propelled vehicles for control of the vehicle or its driving motor to achieve a desired performance, e.g. speed, torque, programmed variation of speed
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60L—PROPULSION OF ELECTRICALLY-PROPELLED VEHICLES; SUPPLYING ELECTRIC POWER FOR AUXILIARY EQUIPMENT OF ELECTRICALLY-PROPELLED VEHICLES; ELECTRODYNAMIC BRAKE SYSTEMS FOR VEHICLES IN GENERAL; MAGNETIC SUSPENSION OR LEVITATION FOR VEHICLES; MONITORING OPERATING VARIABLES OF ELECTRICALLY-PROPELLED VEHICLES; ELECTRIC SAFETY DEVICES FOR ELECTRICALLY-PROPELLED VEHICLES
    • B60L3/00—Electric devices on electrically-propelled vehicles for safety purposes; Monitoring operating variables, e.g. speed, deceleration or energy consumption
    • B60L3/0007—Measures or means for preventing or attenuating collisions
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60L—PROPULSION OF ELECTRICALLY-PROPELLED VEHICLES; SUPPLYING ELECTRIC POWER FOR AUXILIARY EQUIPMENT OF ELECTRICALLY-PROPELLED VEHICLES; ELECTRODYNAMIC BRAKE SYSTEMS FOR VEHICLES IN GENERAL; MAGNETIC SUSPENSION OR LEVITATION FOR VEHICLES; MONITORING OPERATING VARIABLES OF ELECTRICALLY-PROPELLED VEHICLES; ELECTRIC SAFETY DEVICES FOR ELECTRICALLY-PROPELLED VEHICLES
    • B60L3/00—Electric devices on electrically-propelled vehicles for safety purposes; Monitoring operating variables, e.g. speed, deceleration or energy consumption
    • B60L3/0007—Measures or means for preventing or attenuating collisions
    • B60L3/0015—Prevention of collisions
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
    • G05D1/0238—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors
    • G05D1/024—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors in combination with a laser
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/08—Control of attitude, i.e. control of roll, pitch, or yaw
    • G05D1/0891—Control of attitude, i.e. control of roll, pitch, or yaw specially adapted for land vehicles
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60L—PROPULSION OF ELECTRICALLY-PROPELLED VEHICLES; SUPPLYING ELECTRIC POWER FOR AUXILIARY EQUIPMENT OF ELECTRICALLY-PROPELLED VEHICLES; ELECTRODYNAMIC BRAKE SYSTEMS FOR VEHICLES IN GENERAL; MAGNETIC SUSPENSION OR LEVITATION FOR VEHICLES; MONITORING OPERATING VARIABLES OF ELECTRICALLY-PROPELLED VEHICLES; ELECTRIC SAFETY DEVICES FOR ELECTRICALLY-PROPELLED VEHICLES
    • B60L2200/00—Type of vehicles
    • B60L2200/16—Single-axle vehicles
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60L—PROPULSION OF ELECTRICALLY-PROPELLED VEHICLES; SUPPLYING ELECTRIC POWER FOR AUXILIARY EQUIPMENT OF ELECTRICALLY-PROPELLED VEHICLES; ELECTRODYNAMIC BRAKE SYSTEMS FOR VEHICLES IN GENERAL; MAGNETIC SUSPENSION OR LEVITATION FOR VEHICLES; MONITORING OPERATING VARIABLES OF ELECTRICALLY-PROPELLED VEHICLES; ELECTRIC SAFETY DEVICES FOR ELECTRICALLY-PROPELLED VEHICLES
    • B60L2240/00—Control parameters of input or output; Target parameters
    • B60L2240/10—Vehicle control parameters
    • B60L2240/12—Speed
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60L—PROPULSION OF ELECTRICALLY-PROPELLED VEHICLES; SUPPLYING ELECTRIC POWER FOR AUXILIARY EQUIPMENT OF ELECTRICALLY-PROPELLED VEHICLES; ELECTRODYNAMIC BRAKE SYSTEMS FOR VEHICLES IN GENERAL; MAGNETIC SUSPENSION OR LEVITATION FOR VEHICLES; MONITORING OPERATING VARIABLES OF ELECTRICALLY-PROPELLED VEHICLES; ELECTRIC SAFETY DEVICES FOR ELECTRICALLY-PROPELLED VEHICLES
    • B60L2240/00—Control parameters of input or output; Target parameters
    • B60L2240/10—Vehicle control parameters
    • B60L2240/14—Acceleration
    • B60L2240/20—Acceleration angular
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B62—LAND VEHICLES FOR TRAVELLING OTHERWISE THAN ON RAILS
    • B62B—HAND-PROPELLED VEHICLES, e.g. HAND CARTS OR PERAMBULATORS; SLEDGES
    • B62B1/00—Hand carts having only one axis carrying one or more transport wheels; Equipment therefor
    • B62B1/10—Hand carts having only one axis carrying one or more transport wheels; Equipment therefor in which the load is intended to be transferred totally to the wheels
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B62—LAND VEHICLES FOR TRAVELLING OTHERWISE THAN ON RAILS
    • B62B—HAND-PROPELLED VEHICLES, e.g. HAND CARTS OR PERAMBULATORS; SLEDGES
    • B62B2301/00—Wheel arrangements; Steering; Stability; Wheel suspension
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B62—LAND VEHICLES FOR TRAVELLING OTHERWISE THAN ON RAILS
    • B62B—HAND-PROPELLED VEHICLES, e.g. HAND CARTS OR PERAMBULATORS; SLEDGES
    • B62B5/00—Accessories or details specially adapted for hand carts
    • B62B5/0026—Propulsion aids
    • B62B5/0069—Control
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/60—Other road transportation technologies with climate change mitigation effect
    • Y02T10/64—Electric machine technologies in electromobility
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/60—Other road transportation technologies with climate change mitigation effect
    • Y02T10/72—Electric energy management in electromobility

Definitions

  • the present invention relates to an inverted pendulum type moving body provided with a speed planning device.
  • Patent Document 1 Japanese Unexamined Patent Publication No. 2007-168603
  • Patent Document 1 Japanese Unexamined Patent Publication No. 2007-168603
  • the obstacle detection unit outputs a stop instruction signal to the control unit.
  • the control unit sets a value obtained by multiplying the current vehicle speed by -1 based on the stop instruction signal as a target vehicle speed, and drives the drive wheels at a rotational speed corresponding to the target vehicle speed. For this reason, a force is applied to the vehicle body so as to advance in the direction opposite to the traveling direction, and the braking distance at the time of stopping is shortened. Further, the target vehicle speed is changed so as to gradually approach 0 until the vehicle body stops. Since the target vehicle speed can be changed while the posture of the vehicle body is controlled by this control unit, the vehicle body can be urgently stopped in a stable posture.
  • Patent Document 2 JP-A-2007-319991.
  • This prior art describes a mobile robot having a moving mechanism having left and right wheels and a traveling motor that rotationally drives the wheels, a body supported by the moving mechanism, and a control device that controls the moving mechanism. Yes.
  • This control device has a travel target generation unit that generates a plurality of time-series target values, and this travel target generation unit is a linear sum of the position and inclination of the mobile robot above the grounding point of the mobile robot.
  • the time series values of the feature points are generated so as to satisfy the movement condition for the plurality of time series target values.
  • the inverted pendulum type moving body Since the inverted pendulum type moving body has a small bottom area, there are cases where it is used as a moving body on which a human is boarded, or as a carriage for transporting objects such as service robots. Thus, the inverted pendulum type moving body is expected to play an active role in any environment while coexisting with humans.
  • Inverted pendulum type moving body that operates in the vicinity of humans should recover posture immediately from jumping out of a corner, contact, impact of disturbance when stepping over, etc., and stop at the shortest distance to ensure safety is important.
  • Patent Document 1 does not consider the maximum torque of the drive wheels, and the inverted pendulum type moving body does not necessarily reach the maximum acceleration, so it is difficult to stop at the shortest distance. In addition, when excessive acceleration is required, there is a possibility of falling.
  • the movement plan is performed in consideration of the maximum torque, but the plan cannot be performed after acceleration / deceleration in a state where the posture is disturbed. Therefore, there is a possibility that it cannot be used in a situation where it is difficult to change the speed during movement and a posture disorder such as an emergency stop is expected. In addition, since the maximum acceleration is not maintained, it is difficult to stop at the shortest distance.
  • An object of the present invention is to provide an inverted pendulum type moving body that enables a speed change including a safe emergency stop even in a situation where the posture is disturbed during acceleration / deceleration or overstepping a step.
  • the object is to provide a pair of wheels that are suspended on the moving body main body and have the floor surface as a running surface, and are provided perpendicular to the traveling direction, a drive mechanism that rotates the wheels, the center of gravity position of the moving body main body, and the pair of wheels.
  • An inverted pendulum comprising posture measuring means for measuring an upper body angular velocity formed in a vertical direction and a straight line connecting the axles of the wheels, and a mechanism control unit for maintaining the inverted state of the movable body by controlling the drive mechanism
  • the mobile body when a speed change is required during the movement of the mobile body, the mobile body includes a speed planning device that performs a movement plan so that the body angular velocity and the body angle have a quadratic function relationship Is achieved.
  • the above object is that when a change in speed occurs during the movement of the moving body, after generating a constant wheel driving force, the body angular velocity and the body angle have a quadratic function relationship. It is preferable to provide a speed planning device that performs movement planning.
  • the above object is preferably provided with a velocity planning device that plans a target value for which the body angle target value is non-zero and the body angular velocity is zero for a certain period of time.
  • the above object is provided with a speed planning device that does not output the body angle as zero and the body angular speed as a target value until the speed change ends when the speed is changed.
  • an inverted pendulum type moving body that enables a speed change including a safe emergency stop even in a situation where the posture is disturbed during acceleration / deceleration or overstepping a step.
  • FIG. 1A is a side view for explaining the mechanism configuration of the inverted pendulum type moving body 1 of this embodiment
  • FIG. 1B is a front view of the inverted pendulum type moving body 1.
  • the inverted pendulum type moving body 1 is largely composed of a moving mechanism 10 and an upper body 2.
  • the moving mechanism 10 includes coaxial wheels 13R, L, a mechanism control unit 11, and motors 12R, L which are a pair of left and right wheels, and the wheels 13R, L are connected to independent motors 12R, L, respectively.
  • the motors 12R and L are connected to the mechanism control unit 11 and controlled to generate a predetermined torque.
  • the rotation angle and rotation angular velocity of the wheels 13R, L are acquired by the mechanism control unit 11 via the motors 12R, L.
  • the upper body 2 is provided on the upper part of the moving mechanism 10.
  • the upper body 2 includes a computer 3 for performing calculations described later, an environmental sensor 4 for measuring surrounding conditions, a straight line connecting the center of gravity of the inverted pendulum type moving body 1 excluding the wheels 13R and L and the axle.
  • a posture sensor 5 for measuring the inclination angle ⁇ and the inclination angular velocity d ⁇ formed by the vertical direction is provided.
  • the computer 3 and the moving mechanism 10 are connected so that information can be exchanged.
  • FIG. 2 is a flowchart showing the system configuration of the inverted pendulum type moving body targeted by the present invention.
  • the computer 3 includes an obstacle recognition unit 21, an action determination unit 22, a route plan unit 23, a speed plan unit 24, and an inversion control unit 25.
  • the computer 3 is connected to the environment sensor 4, the attitude sensor 5, and the moving mechanism 10.
  • the environment sensor 4 is a sensor for observing the situation outside the inverted pendulum type moving body 1. For example, by using a laser scanner, a distance to a surrounding object can be measured in a plane or a solid. For example, the distance to the surrounding object can be recognized similarly by using a stereo camera.
  • the posture sensor 5 is configured by using, for example, a gyro sensor, and can thereby measure the inclination angle ⁇ and the inclination angular velocity d ⁇ of the upper body 2.
  • the obstacle recognizing unit 21 corrects the distance to the surrounding object obtained from the environment sensor 4 by multiplying cos ⁇ by using the inclination angle ⁇ obtained from the posture sensor 5, and calculates the position, speed, and shape of the obstacle. .
  • an obstacle calculation method for example, there is the following method (for example, the method disclosed in JP 2008-65555 A).
  • a sudden change point with respect to the angle of the distance value obtained from the laser scanner at a certain time t is detected, and the data string is divided into segments for each group of consecutive points.
  • a representative amount such as the center of gravity of each segment at time t and a feature quantity such as a shape are obtained.
  • the same calculation is performed at time t + ⁇ t to obtain the feature amount of each segment.
  • the feature quantity of the segment obtained at time t is compared with the feature quantity of the segment obtained at time t + ⁇ t, the segments having similar feature quantities are recognized as the same obstacle, and the change in the representative position The speed of the obstacle is calculated from the quantity.
  • the center coordinates of a circle inscribed in the obtained shape are calculated from the data string recognized as the same obstacle.
  • an obstacle whose moving speed is approximately 0 is regarded as a stationary obstacle, and each data point by the laser scanner is regarded as an obstacle having a width of 0.
  • the obstacle recognition unit 21 calculates the position, velocity, and shape of one or more obstacles.
  • the action determination unit 22 includes the position, velocity, and shape of the obstacle that is obtained from the obstacle recognition unit 21, the inclination angle ⁇ acquired from the posture sensor 5, the inclination angular velocity d ⁇ , the route plan acquired from the route plan unit 23, and the speed plan unit.
  • the behavior of the inverted pendulum type moving body 1 is determined from the speed plan acquired from 24.
  • FIG. 3 is a flowchart for explaining the processing of the action determining unit according to the embodiment of the present invention.
  • FIG. 4 is a diagram for explaining contact determination with an obstacle of the action determination unit according to the embodiment of the present invention.
  • the action determination unit 22 acquires a movement target that is set in advance, is determined by a program, or is input by a human.
  • the movement target includes a movement destination position pr, a movement destination turning direction ⁇ r, a maximum movement speed vr, and a motor maximum torque ⁇ r.
  • the action determination unit 22 transmits the movement target to the route plan unit 23 together with the examination command, and acquires the route plan and the speed plan from the route plan unit 23 and the speed plan unit 24.
  • the internal processing of the route planning unit 23 and the speed planning unit 24 will be described later.
  • the action determination unit 22 acquires the inclination angle ⁇ and the inclination angular velocity d ⁇ from the attitude sensor 5 and the current position p of the inverted pendulum type moving body 1 from the inversion control unit 25 via the velocity planning unit 24.
  • the action determining unit 22 determines whether the posture of the inverted pendulum type moving body 1 has collapsed using Equation 1.
  • ⁇ 1u, d ⁇ u, and ⁇ 2u are constants determined by the characteristics of the inverted pendulum type moving body 1, and ⁇ 1u> 0, d ⁇ u> 0, and ⁇ 2u> 0. If Formula 1 is true, the process proceeds to S22_10; otherwise, the process proceeds to S22_5.
  • the action determination unit 22 approximates the position po, the speed vo, and the shape of the obstacle with a circle from the obstacle recognition unit 21, and acquires the radius r.
  • the action determination unit 22 determines whether the inverted pendulum type moving body 1 and the obstacle are in contact with the obstacle information acquired in S22_5 and the route plan and speed plan acquired in S22_2. To determine the contact, for example, as shown in FIG. 4, the velocity of the inverted pendulum type moving body 1 at time t is v (t), the position is along the route plan, and the velocity integration from the current time to t0 seconds later is integrated.
  • the function p ( ⁇ v (t) dt) is used as an argument and the shape of the inverted pendulum type moving body 1 is a circle with a radius w as in the case of an obstacle
  • Equation 2 the contact between the inverted pendulum type moving body 1 and the obstacle is expressed by Equation 2. Predict.
  • the action determining unit 22 determines whether or not a movement command has been issued after obtaining the movement target in S22_1. If no movement command has been issued, the action determining unit 22 in S22_8 determines the route plan unit 23 and the speed plan. The movement target and movement command issued in S22_2 are transmitted to the unit 24, and the process further proceeds to S22_9. When the movement command has been transmitted, the behavior determination unit 22 advances the process to S22_9. In S22_S22_9, the action determination unit 22 compares the movement target acquired in S22_1 with the current position p acquired in S22_3 using Expression 3.
  • ⁇ is a preset constant.
  • the action determination unit 22 issues an emergency stop command to the route plan unit 23 and ends the process.
  • the route planning unit 23 acquires a movement target from the behavior determining unit 22 and generates a route.
  • the current position p and the movement destination position pr acquired from the movement mechanism 10 via the travel plan unit 24 are connected by a straight line.
  • a Bezier curve or spline curve that smoothly connects the current position p and the destination position p is used as a route. Also good.
  • the route planning unit transmits the generated route information as a position coordinate data group of points on the route to the action determining unit 22 and the speed planning unit 24, and the examination command and the movement command acquired from the action determining unit 22 are also received. It transmits to the speed plan part 24.
  • the speed planning unit 24 sets the movement target from the action determination unit 22, the path planning from the path planning unit 23, the inclination angle ⁇ and the inclination angular velocity d ⁇ of the upper body from the posture sensor 5, and the inversion control unit 25 from the movement mechanism 10.
  • the wheel speed d ⁇ is acquired and a speed plan is generated.
  • the generated speed plan and the acquired route plan are transmitted to the inversion control unit 25.
  • FIG. 5 is a flowchart for explaining the processing of the speed planning unit according to the embodiment of the present invention.
  • FIG. 6 is a diagram illustrating an example of modeling of the inverted pendulum type moving body according to the embodiment of the present invention.
  • the speed plan unit 24 acquires a movement target and a movement command or a review command from the behavior determination unit 22.
  • the speed planning unit 24 acquires route information from the route planning unit 23.
  • the speed planning unit 24 calculates the movement distance l from the acquired route information.
  • the movement distance l is calculated according to Equation 4 because the route information is a position coordinate data group consisting of n points.
  • the speed planning unit 24 acquires the body inclination angle ⁇ and the inclination angular velocity d ⁇ from the attitude sensor 5, and the wheel angular velocity d ⁇ and the current position from the inversion control unit 25.
  • the speed planning unit 24 determines whether an emergency stop command has been acquired from the action determining unit 22. If an emergency stop command has not been acquired, the process proceeds to S24_6. If acquired, the process proceeds to S24-7.
  • the speed planning unit 24 generates a speed plan based on the maximum movement speed vr extracted from the movement target, the motor maximum torque ⁇ r, the calculated movement distance l, the inclination angle ⁇ acquired from the attitude sensor, and the inclination angular speed d ⁇ . .
  • the speed plan is generated according to the motion equation of the inverted pendulum type moving body 1, and a model for calculating the motion equation is shown in FIG.
  • the radius of the wheel 13 is R
  • the mass is M0
  • the moment of inertia is J0
  • the mass of the other part of the inverted pendulum type moving body 1 is M1
  • the moment of inertia is J1
  • the distance from the center of gravity to the axle of the wheel 13 is L
  • the center of gravity The rotation angle of the wheel 13 from the straight line including the axle is ⁇
  • the rotation angular velocity is d ⁇
  • the gravitational acceleration is g.
  • the constant term part was replaced with S, T, U, and V to simplify the formula.
  • All state quantities can be designed by designing the fixed point q from Equation 6.
  • the rotation center of the entire inverted pendulum type moving body 1 at this time is q.
  • the technique described in Patent Document 2 can be used.
  • q (4) takes any value of 0, k, and ⁇ k (k> 0), and the state is stable at the start of movement (time ts) and at the end of movement (time te).
  • a data sequence along a time series of the tilt angle ⁇ , the tilt angular velocity d ⁇ , the wheel angle ⁇ , and the wheel angular velocity d ⁇ is generated as a speed plan.
  • the speed plan unit 24 proceeds to S24_9 if the command acquired from the action determination unit 22 is a review command, and proceeds to S24_10 if the command is a movement command.
  • the speed plan unit 24 transmits the generated speed plan to the action determination unit 22, and ends.
  • the speed plan unit 24 transmits the generated speed plan to the action determination unit 22 and the inversion control unit 25, and ends.
  • the speed planning unit 24 executes an emergency stop operation using the current state quantity as a start point and ends. In this case, since the state at the start of movement (time ts) and the end of movement (time te) is not stable, the method of Patent Document 2 cannot be used.
  • FIG. 7 is a flowchart showing the emergency stop operation process of the speed planning unit according to the embodiment of the present invention.
  • FIG. 8 is a diagram for explaining the emergency stop operation process of the speed planning unit according to the embodiment of the present invention.
  • q (2) is taken on the horizontal axis and q (3) is taken on the vertical axis, and these represent the relationship between the acceleration of the inverted pendulum type moving body 1 and the acceleration change rate.
  • the speed planning unit 24 calculates a deceleration curve 100 expressed by Formula 7 and a deceleration curve 101 expressed by Formula 8 where k is a constant.
  • k (> 0) is time-invariant and common, and is equal to the absolute value of q (4) .
  • d ⁇ obtained from the maximum motor torque ⁇ r, the maximum moving speed vr, and the radius of the wheel 13 and the equation It is calculated by using the relationship of 6.
  • q (4) is either k or -k (k> 0) if the torque is not directly specified in the plan. If the absolute value of k, q (4) , is determined from Equation 6, the maximum motor torque ⁇ r is a linear function of the maximum body inclination angle ⁇ .
  • the speed planning unit 24 acquires the inclination angle ⁇ and the inclination angular speed d ⁇ from the attitude sensor 5, converts them into q (2) and q (3) using the relationship of Expression 6, and the deceleration curve in FIG. Determine whether it is on the right or left of 100. If it is right, the process proceeds to S24_7_3. If it is near the deceleration curve 100, the process proceeds to S24_7_4. If it is sufficiently left, the process proceeds to S24_7_5.
  • the speed planning unit 24 sends an instruction to the inversion control unit 25 so that the motor torque ⁇ becomes the motor maximum torque ⁇ r, and returns the process to S24_7_2.
  • the state of the inverted pendulum type moving body 1 moves in the lower left direction as shown in FIG.
  • the speed planning unit 24 acquires the wheel angular velocity d ⁇ , the inclination angle ⁇ , and the inclination angular velocity d ⁇ from the attitude sensor 5 and the mechanism control unit 11 via the inversion control unit 25, and uses these state quantities using the relationship of Equation 6.
  • the speed planning unit 24 sends an instruction to the inversion control unit 25 so that the motor torque ⁇ becomes 0, and the inverted pendulum type moving body 1 is overturned to end the movement.
  • the inversion control unit 25 controls the motors 12R, L based on the plan or torque command sent from the speed planning unit 24 and the rotation angles ⁇ R, L of the wheels 13R, L acquired from the moving mechanism 10 and the rotation angular speeds d ⁇ R, L. For this purpose is transmitted to the mechanism control unit 11.
  • the target value corresponding to the current time is used according to the time series of the plan.
  • the current position p (t0) of the inverted pendulum type moving body 1 is calculated.
  • the calculation method of the position at the current time t0 is expressed by Equation 9.
  • t represents time and wheel angle ⁇ R, L and rotational angular velocity d ⁇ R, L are described as a function of t.
  • F is the distance between the wheels 13R and 13L.
  • ⁇ r, d ⁇ r, ⁇ , and d ⁇ are a body angle target value, a body angular velocity target value, a wheel angle target value, and a wheel angular velocity target value, respectively.
  • ⁇ is the average of the wheel rotation angles ⁇ R and L
  • d ⁇ is the average of the wheel rotation angular velocities d ⁇ R and L.
  • the inversion control unit 25 calculates the distance h to the point h [j] and the point h [j] on the route plan closest to the current position.
  • the target turning direction ⁇ r and the turning radius ⁇ at the point h [j] are calculated using the points h [j + 1] and h [j-1] before and after the point h [j] on the route plan.
  • the current target moving speed vr is divided by the turning radius ⁇ to obtain a target turning angular speed d ⁇ r.
  • the turning motor torque ⁇ c is calculated according to Equation 11.
  • the inversion control unit 25 transmits to the mechanism control unit 11 the sum of ⁇ s and ⁇ c as a torque to be output to the motor 12L and a difference between ⁇ s and ⁇ c as a torque to be output to the motor 12R.
  • the mechanism control unit 11 performs control so that the motors 12R and L do not output torque. Further, the angles and angular velocities of the wheels 13R, L are measured via the motors 12R, L, and transmitted to the inversion control unit 25.
  • FIG. 9 shows the behavior of the inverted pendulum type moving body 1 configured in the present embodiment according to the simulation when the emergency stop operation is performed.
  • FIG. 9 is a graph showing the simulation result of the effect according to the example of the present invention.
  • an example in which the emergency plan of the present embodiment is performed while the inverted pendulum type moving body 1 is accelerating is shown by a solid line.
  • the emergency stop plan is simply expressed by the wheel angular velocity d ⁇ , the inclination angle ⁇ , and the inclination angular velocity d ⁇ .
  • the value in which is set to 0 is represented by a broken line.
  • the conventional method cannot cope with a sudden change in target value and falls, the method of this embodiment succeeds in changing the speed and succeeds in stopping.
  • an emergency stop in which the speed is set to 0 during the forward movement is shown.
  • the speed can be set to 0 during the backward movement, or the speed can be changed to an arbitrary speed from the current speed.
  • the process is performed based on the acquired current information.
  • the simulation is performed based on the situation at the start of the plan, the timing is determined, and all plans are performed. May be generated in advance.
  • the route is generated by the route planning unit 23, but a route set in advance as in the case of the action determining unit 22 or route information directly from the outside may be acquired. Further, when the route plan is transmitted to the speed plan unit 24, the route plan information may not be transmitted as coordinate data, but may be transmitted as a function using time as a parameter or a data group including a combination of time and coordinates.
  • the stop is performed at the maximum acceleration.
  • the maximum acceleration is not used, and an appropriate small acceleration is used. good.
  • the maximum torque need not be used in S24_7_3 that moves the posture to the deceleration curve.
  • the inclination angle of the upper body is acquired from the attitude sensor.
  • the attitude sensor may measure only the inclination angular velocity
  • the calculator may obtain the inclination angle by integrating after acquiring the inclination angular velocity.
  • SYMBOLS 1 Inverted pendulum type moving body, 2 ... Upper body, 3 ... Computer, 4 ... Environmental sensor, 5 ... Attitude sensor, 10 ... Moving mechanism, 11 ... Mechanism control Part, 12 ... motor, 13 ... wheel.

Abstract

　倒立振子型移動体はその底面積が小さいことから、人間が搭乗する移動体やサービスロボットや物を運搬するための台車としての活躍することが期待される。そのため、姿勢の乱れから速やかに姿勢を回復することや、最短距離で停止し安全を確保することが重要である。そこで、移動体本体に懸架されて床面を走行面とし、進行方向に垂直な同一平面上に備えられた一対の車輪と、この車輪を回転させる駆動機構と、前記移動体本体の重心位置と前記一対の前記車輪の車軸を結ぶ直線と鉛直方向のなす上体の傾斜角速度を計測する姿勢計測手段と、前記駆動機構を制御することで移動体本体の倒立状態を維持する機構制御部とを備えた倒立振子型移動体において、前記移動体の移動中に速度変更の必要が生じた際において、前記上体角速度と上体角度が二次関数の関係になるように移動計画を行う速度計画装置を備えたものである。

Description

速度計画装置を備えた倒立振子型移動体
　本発明は、速度計画装置を備えた倒立振子型移動体に関する。
　本技術分野に関する従来技術としては、特開２００７－１６８６０３号公報（特許文献１）がある。　
　この従来技術では、駆動輪と操作部と障害物検知部と駆動輪を駆動制御する制御部を備えた同軸二輪移動台車があり、この台車の移動中にユーザが台車の停止を指示した場合には操作部が、障害物の存在が検知された場合には障害物検知部が制御部に停止指示信号を出力するようになっている。
　この制御部は、停止指示信号に基づき、現在の車速に－１を乗じた値を目標車速に設定すると共に、目標車速に応じた回転数で駆動輪を駆動させるようになっている。このため、車体に対して、進行方向と反対の方向に進むように力が加わることになり、停止時の制動距離が短くなる。また、目標車速は、車体が停止するまで逐次０に近づくように変化される。この制御部により車体の姿勢が制御されながら目標車速を変化させることができるため、安定した姿勢での車体の緊急停止が可能となっている。
　また、特開２００７－３１９９９１号公報（特許文献２）がある。　
　この従来技術では、左右の車輪及びこの車輪を回転駆動させる走行モータを有する移動機構と、この移動機構に支持された上体と移動機構を制御する制御装置とを備えた移動ロボットが記載されている。
　この制御装置は複数の時系列目標値を生成する走行目標生成部を有しており、この走行目標生成部は、移動ロボットの接地点より上にある移動ロボットの位置及び傾きの線形和である特徴点の時系列値を、前記複数の時系列目標値に対する移動条件を満たすように生成するようになっている。
特開２００７－１６８６０３号公報 特開２００７－３１９９９１号公報
　倒立振子型移動体はその底面積が小さいことから、人間が搭乗する移動体として利用するケースや、サービスロボットなど物を運搬するための台車として理由するケースが考えられる。このように、倒立振子型移動体は人間と共存しながらあらゆる環境で活躍することが期待されている。
　人間の近傍で動作する倒立振子型移動体は曲がり角からの飛び出しや接触、段差乗越え時の衝撃などによる姿勢の乱れに対して速やかに姿勢を回復させ、最短距離で停止して安全を確保することが重要である。
　しかしながら特許文献１記載の技術では駆動輪の最大トルクが考慮されておらず、倒立振子型移動体が必ずしも最大加速度に達しないため、最短距離で止まることは難しい。また、過大な加速度が要求された場合は転倒に至る可能性もある。
　また特許文献２記載の技術では最大トルクを考慮した移動計画を行っているが、姿勢の乱れた状態で加減速中からの計画を行うことができない。したがって、移動中の速度変更が難しく、緊急停止等の姿勢の乱れが予想されるような状況下では活用できない可能性がある。また、最大加速度を維持しない為、最短距離で止まることも難しい。
　本発明の目的は、加減速中や段差を踏み越え中に姿勢が乱れたような状況からも、安全な緊急停止を含む速度変更を可能にする倒立振子型移動体を提供することにある。
　上記目的は、移動体本体に懸架されて床面を走行面とし、進行方向に垂直に備えられた一対の車輪と、この車輪を回転させる駆動機構と、前記移動体本体の重心位置と前記一対の前記車輪の車軸を結ぶ直線と鉛直方向のなす上体角速度を計測する姿勢計測手段と、前記駆動機構を制御することで移動体本体の倒立状態を維持する機構制御部とを備えた倒立振子型移動体において、前記移動体の移動中に速度変更の必要が生じた際において、前記上体角速度と上体角度が二次関数の関係になるように移動計画を行う速度計画装置を備えたことにより達成される。
　また上記目的は、前記移動体の移動中に速度変更の必要が生じた際において、一定の車輪駆動力を発生させた後に、上体角速度と上体角度が二次関数の関係になるように移動計画を行う速度計画装置を備えることが好ましい。
　また上記目的は、上体角度目標値が非零かつ、上体角速度を零である目標値を一定時間計画する速度計画装置を備えることが好ましい。
　また上記目的は、速度変更の際に、速度変更終了まで上体角度が零かつ、上体角速度が零を目標値として出力しない速度計画装置を備えることが好ましい。
　本発明によれば、加減速中や段差を踏み越え中に姿勢が乱れたような状況からも、安全な緊急停止を含む速度変更を可能にする倒立振子型移動体を提供できる。
本発明が対象とする倒立振子型移動体の概略構成図である。 本発明が対象とする倒立振子型移動体のシステム構成を表すフロー図である。 本発明の実施例に係る行動決定部の処理を説明するフロー図である。 本発明の実施例に係る行動決定部の障害物との接触判定を説明する図である。 本発明の実施例に係る速度計画部の処理を説明するフロー図である。 本発明の実施例に係る倒立振子型移動体のモデル化の例を示す図である。 本発明の実施例に係る速度計画部の緊急停止動作処理を表すフロー図である。 本発明の実施例に係る速度計画部の緊急停止動作処理を説明する図である。 本発明の実施例に係る効果のシミュレーション結果を表すグラフ図である。
　以下、本発明の一実施例を図に沿って説明する。
　本実施例の倒立振子型移動体１の機構構成を図１を参照しながら説明する。　
　図１(a)は本実施例の倒立振子型移動体１の機構構成を説明する側面図であり、図１(b)は倒立振子型移動体１の正面図である。
　図１(a)(b)において、倒立振子型移動体１は大きく移動機構１０と上体２により構成される。移動機構１０は左右１対から成る同軸上の車輪１３R、Lと機構制御部１１とモータ12R、Lを備え、車輪１３R、Lは其々独立のモータ１２R、Lに接続されている。モータ１２R、Lは機構制御部１１に接続され、所定のトルクを発生させるように制御される。車輪１３R、Lの回転角度と回転角速度はモータ１２R、Lを介して機構制御部１１によって取得される。
　上体２は移動機構１０の上部に備えられている。上体２は後述する演算を行うための計算機３と、周囲の状況を測定するための環境センサ４と、車輪１３R、Lを除く倒立振子型移動体１の重心位置と車軸を結んだ直線と鉛直方向のなす傾斜角度θ、傾斜角速度ｄθを測定するための姿勢センサ５を備えている。計算機３と移動機構１０とは情報交換ができる様に接続されている。
　次に図２を用いて倒立振子型移動体１のシステム構成について述べる。　
　図２は本発明が対象とする倒立振子型移動体のシステム構成を表すフロー図である。
　図２において、計算機３は障害物認識部２１、行動決定部２２、経路計画部２３、速度計画部２４、倒立制御部２５を備えている。計算機３は環境センサ４、姿勢センサ５、移動機構１０に接続されている。環境センサ４は倒立振子型移動体１外部の状況を観測するためのセンサであり、例えば、レーザスキャナを用いることで、平面または立体で周囲の物体までの距離を測定することができる。また、例えば、ステレオカメラを用いることでも同様に周囲の物体までの距離を認識することができる。姿勢センサ５は例えば、ジャイロセンサを用いることで構成され、これにより上体２の傾斜角度θや傾斜角速度ｄθを計測することができる。障害物認識部２１は環境センサ４から得られる周囲の物体までの距離を、姿勢センサ５より得られる傾斜角度θを用いてｃｏｓθを乗じて補正し、障害物の位置と速度と形状を算出する。
　障害物の算出手法としては例えば以下の方法（例えば、特開２００８-６５７５５号公報の方法）がある。
　この方法ではまず、ある時刻tにレーザスキャナから得られた距離値の角度に対する急激な変化点を検出し、連続する点の纏まりごとにデータ列を分割してセグメントとする。これによって、時間tにおける各セグメントの重心などの代表位置、形状などの特徴量を得る。次に、時刻ｔ＋Δｔで同様に計算し、各セグメントの特徴量を得る。ここで、時刻tで得られたセグメントの特徴量と、時刻ｔ＋Δｔで得られたセグメントの特徴量を比較し、特徴量の近いセグメント同士は同一の障害物であると認識し、代表位置の変化量から、障害物の速度を算出する。また同一の障害物と認識したデータ列より形状を、得られた形状に内接する円の中心座標を算出する。また、移動速度がほぼ０である障害物は静止障害物とみなし、レーザスキャナによる各データ点を幅０の障害物とみなす。また上記の処理はセグメントごとに実行されるため、複数の障害物に対しても認識できる。以上の処理を行うことにより障害物認識部２１は単数または複数の障害物の位置と速度と形状を算出する。
　行動決定部２２は障害物認識部２１から所得する障害物の位置と速度と形状、及び姿勢センサ５から取得する傾斜角度θ、傾斜角速度ｄθ、経路計画部２３から取得する経路計画、速度計画部２４から取得する速度計画から倒立振子型移動体１の挙動を決定する。
　以下、行動決定部２２での行動決定処理を図３、図４で説明する。　
　図３は本発明の実施例に係る行動決定部の処理を説明するフロー図である。　
　図４は本発明の実施例に係る行動決定部の障害物との接触判定を説明する図である。　
　図３において、S２２_１では行動決定部２２は予め設定されている、またはプログラムにより決定される、または人間によって入力される移動目標を取得する。移動目標は移動先位置pr、移動先旋回方向ωｒ、最大移動速度ｖｒ、モータ最大トルクτｒを含む。
　S２２_２において行動決定部２２は経路計画部２３に移動目標を検討命令と共に送信し、経路計画部２３及び速度計画部２４から経路計画と速度計画を取得する。経路計画部２３と速度計画部２４の内部処理については後述する。
　S２２_３において行動決定部２２は姿勢センサ５より傾斜角度θ及び傾斜角速度ｄθを、倒立制御部２５から倒立振子型移動体１の現在位置pを速度計画部２４を介して取得する。
　S２２_４において行動決定部２２は数式１を用いて倒立振子型移動体１の姿勢が崩れていないかを判断する。
Figure JPOXMLDOC01-appb-M000001
　ただし、θ１u、ｄθu、θ２uは倒立振子型移動体１の特性により決定される定数であり、θ１u>０、ｄθu>０、θ２u>０である。数式１が真である場合、処理をS２２_１０へ進め、それ以外の場合はS２２_５へ処理を進める。
　S２２_５において行動決定部２２は障害物認識部２１より障害物の位置poと速度vo、形状を円で近似し半径rを取得する。
　S２２_６において行動決定部２２はS２２_５で取得した障害物の情報とS２２_２で取得した経路計画及び速度計画から倒立振子型移動体１と障害物が接触するかを判定する。接触を判定するには、例えば、図４に示すように倒立振子型移動体１の時刻ｔにおける速度をv(t)、位置を経路計画に沿い、現在時刻からt0秒後までの速度の積分引数にする関数ｐ（∫ｖ（ｔ）ｄｔ)とし、障害物と同様に倒立振子型移動体１の形状を半径ｗの円とすると数式２により倒立振子型移動体１と障害物との接触を予測する。
Figure JPOXMLDOC01-appb-M000002
　数式２が偽の場合、倒立振子型移動体１と障害物との接触は無いと判定し行動決定部２２は処理をS２２_７に進める。数式２が真の場合、行動決定部２２は処理をS２２_１０に進める。
　S２２_７において、行動決定部２２はS２２_１において移動目標を取得した後に移動命令を発行しているかを判定し、移動命令が発行されていない場合はS２２_８において行動決定部２２は経路計画部２３と速度計画部２４に対しS２２_２で発行した移動目標と移動命令を発信し、さらに処理をS２２_９へ進める。移動命令が発信済みの場合、行動決定部２２は処理をS２２_９に進める。
S２２_S２２_９において行動決定部２２はS２２_１で取得した移動目標とS２２_３で取得した現在位置pを数式３を用いて比較する。
Figure JPOXMLDOC01-appb-M000003
　ただし、εは予め設定された定数である。数式３が真の場合、行動決定部２２は移動決定処理を終了する。数式３が偽の場合、行動決定部２２は処理をS２２_３へ戻り繰り返す。
　S２２_１0において、行動決定部２２は経路計画部２３に緊急停止命令を発行し処理を終了する。
経路計画部２３は行動決定部２２より移動目標を取得し、経路を生成する。経路生成方法には走行計画部２４を介し移動機構１０より取得した現在位置p、移動先位置prを直線で結ぶ。まはた、移動先位置prと現在位置pにおいて目標とする旋回方向ωｒと現在の旋回方向ωが異なる場合は現在位置pと移動先位置ｐを滑らかに接続するベジェ曲線やスプライン曲線を経路としても良い。また、経路計画部は生成した経路情報を経路上にある点の位置座標データ群として、行動決定部２２と速度計画部２４に送信するとともに、行動決定部２２から取得した検討命令や移動命令も速度計画部２４に送信する。
　速度計画部２４は行動決定部２２より移動目標を、経路計画部２３より経路計画を、姿勢センサ５より上体の傾斜角度θと傾斜角速度ｄθを、移動機構10より倒立制御部25を介して車輪速度ｄφを取得し速度計画を生成する。また生成した速度計画と取得した経路計画を倒立制御部２５に送信する。
　以下、速度計画部２４での速度計画処理を図５のフローチャートを用いて述べる。また、図５フローチャート中の処理S２４_７の詳細については図７のフローチャートを用いて述べる。　
　図５は本発明の実施例に係る速度計画部の処理を説明するフロー図である。　
　図６は本発明の実施例に係る倒立振子型移動体のモデル化の例を示す図である。
　図５のS２４_１において、速度計画部２４は行動決定部２２より移動目標と移動命令または検討命令を取得する。S２４_２において、速度計画部２４は経路計画部２３より経路情報を取得する。S２４_３において、速度計画部２４は取得した経路情報から移動距離lを算出する。移動距離lの算出は、経路情報をｎ点からなる位置座標データ群であるので、数式４に従って算出する。
Figure JPOXMLDOC01-appb-M000004
　ただしhi(i=１..n)は経路情報個々の点を表す。
　S２４_４において、速度計画部２４は姿勢センサ５より上体の傾斜角度θと傾斜角速度ｄθを、倒立制御部２５より車輪角速度ｄφと現在位置を取得する。
　S２４_５において速度計画部２４は行動決定部２２より緊急停止命令を取得しているかを判断する。緊急停止命令を取得していない場合は処理をS２４_６へ進め、取得している場合は処理をS２４-７に進める。
　S２４_６において速度計画部２４は移動目標から抽出する最大移動速度ｖｒ、モータ最大トルクτｒ、及び算出した移動距離ｌ、姿勢センサから取得した傾斜角度θ、傾斜角速度ｄθをもとに速度計画を生成する。速度計画は倒立振子型移動体１の運動方程式に従って生成され、運動方程式算出のためのモデルを図６に、算出された運動方程式を数式５に示す。
Figure JPOXMLDOC01-appb-M000005
　車輪１３の半径をR、質量をM０、慣性モーメントをJ０、倒立振子型移動体１の車輪以外の部分の質量をM１、慣性モーメントをJ１、重心と車輪１３の車軸までの距離をL、重心と車軸を含む直線からの車輪１３の回転角度をφ、回転角速度をｄφ、重力加速度をｇとしている。
　数式５は図６に示したモデルよりラグランジュ方程式を介し、θ=０近傍で線形化している。また、式を簡便にするために定数項部分をS、T、U、Vで置き換えた。
　さらに数式５に対して媒介変数ｑを用いることにより状態量である、傾斜角度θ、傾斜角速度ｄθ、車輪角度φ、車輪角速度ｄφ、モータトルクτの関係を数式６のように得る。
Figure JPOXMLDOC01-appb-M000006
　ただしｑ（ｉ）　［ｉ＝１、２、３］はｑのｉ階微分を表す。数式６より不動点ｑを設計することで全ての状態量を設計できる。倒立振子型移動体１は速度を変更する際に、その力学的特性から一旦、車輪１３を上体２より進みたい方向に対して逆方向に動かす必要がある。この時の倒立振子型移動体１全体の回転中心がｑである。ｑの設計方法として、移動開始と移動終了時の傾斜角度θと傾斜角速度ｄθを０と見なせる場合は特許文献２記載の技術が利用できる。この時ｑ（４）　は０、ｋ、－ｋ（ｋ＞０）のいずれかの値をとり、移動開始時（時刻ｔｓ）と移動終了時（時刻ｔｅ）に状態が安定していることを条件に、ｑ（ｉ）（ｔｓ）＝　ｑ（ｉ）（ｔｅ）＝０　［ｉ＝１、２、３］を導き、さらに移動距離ｌ、最大移動速度ｖｒ、モータ最大トルクτｒを用いてｑ（４）を設定し、ｑ（４）を積分し数式６の関係で変換することで全ての状態量を設計している。傾斜角度θ、傾斜角速度ｄθ、車輪角度φ、車輪角速度ｄφの時系列に沿ったデータ列を速度計画として生成する。
　S２４_８において、速度計画部２４は行動決定部２２より取得した命令が検討命令ならば処理をS２４_９へ、移動命令ならば処理をS２４_１０へ進める。
S２４_９において、速度計画部２４は行動決定部２２へ生成した速度計画を送信し終了する。
S２４_１０において、速度計画部２４は行動決定部２２と倒立制御部２５へ生成した速度計画を送信し終了する。
　S２４_７において、速度計画部２４は現状の状態量を開始地点とする緊急停止動作を実行し終了する。この場合では移動開始時(時刻ts)と移動終了時(時刻te)の状態が安定ではないため特許文献２の方法は使えない。
　そこで図７に示すフローチャートと図８を用いて停止計画を作成する。　
　図７は本発明の実施例に係る速度計画部の緊急停止動作処理を表すフロー図である。　
　図８は本発明の実施例に係る速度計画部の緊急停止動作処理を説明する図である。
　図８は横軸にq(２)、縦軸にq(３)をとったものであり、これらは倒立振子型移動体１の加速度と加速度変化率の関係を表す。
　図７、図８において、本実施例では車輪速度ｄφが正の場合から０に減速するまでを扱うが、符号を適切にすることにより車輪速度ｄφが負の場合からも０まで加速することができる。
　S２４_７_１において、速度計画部２４はkを定数とした数式７で表される減速曲線１００と数式８であらわされる減速曲線１０１を算出する。
Figure JPOXMLDOC01-appb-M000007
Figure JPOXMLDOC01-appb-M000008
　数式７及び８においてk(>０)は時不変で共通であり、q(4)の絶対値と等しく、例えば、最大モータトルクτｒと最大移動速度ｖｒと車輪１３の半径から求められるｄφと数式６の関係を用いることで算出する。q(４) はトルクが計画で直接指定されていない場合はｋ、-k(k>0)のいずれかの値とする。また、数式6よりｋつまりq(4)の絶対値が決定されているならば最大モータトルクτｒは最大上体傾斜角度θの一次関数の関係である。これを利用して、要求するモータトルクの絶対値が最大となる、θの絶対値が最大となるq(３)＝０の場合について前述の一次関数を解くことでCが算出できる。本減速曲線に沿った計画に追従することで、倒立振子型移動体１は任意の姿勢から、最大モータトルクでモータ１２を駆動した後でも、最大の減速を行い停止することができる。
　S２４_７_２において、速度計画部２４は、姿勢センサ５より傾斜角度θと傾斜角速度ｄθを取得し、数式６の関係を用いてそれぞれq(２)とq(３) に変換し、図8における減速曲線１００の右にあるか左にあるかを判定する。右の場合は処理をS２４_７_３に進め、減速曲線１００の近傍にある場合はS２４_７_４に処理を進め、十分に左にある場合はS２４_７_５に進める。
　S２４_７_３において、速度計画部２４は倒立制御部２５にモータトルクτがモータ最大トルクτｒとなるように指示を送り、S２４_７_２へ処理を戻す。S24_7_３の間は図8に示すように倒立振子型移動体1の状態は左下方向に動く。
　S２４_７_４において、速度計画部２４は、姿勢センサ５と機構制御部１１より倒立制御部２５を介して車輪角速度ｄφ、傾斜角度θと傾斜角速度ｄθを取得し、数式６の関係を用いてこれら状態量を変換し、q(i) [i=１、２、３]を生成する。得られたq(i)に対してq(４)=kとし数式６の関係とq(４)を積分することで減速曲線１００に沿った速度計画を生成し、傾斜角速度ｄθ= q(3)=０となる部分までを倒立制御部２５に送信する。傾斜角速度ｄθ= q(3)=０となった場合は処理をS24_7_7に進める。S24_7_４の間は図8に示すように倒立振子型移動体1の状態は減速曲線100に沿って動く。
　S２４_７_５において、速度計画部２４はモータトルクτが０となるように倒立制御部２５に指示を送り、倒立振子型移動体１を転倒させ移動を終了させる。
　S２４_７_６において、速度計画部２４は、S２４_７_４と同様に減速曲線１００に従って減速し、減速曲線１００と減速曲線１０１の交点でq(４)=-kとして減速曲線101に沿ってさらに減速を続ける計画において、q(２)=q(３)=０となった場合において車輪角速度ｄφが０となるかを判定する。判定結果が偽の場合は判定結果が真となるまで目標傾斜角度、目標傾斜角速度を維持することで減速を続け、ループ処理を行う。判定結果が真の場合はS２４_７_７に処理を進め判定結果が真となった減速計画を倒立制御部２５へ送信し終了する。
　倒立制御部２５は速度計画部２４から送られた計画またはトルク指令と移動機構１０から取得した車輪１３R、Lの回転角度φR、Lと回転角速度ｄφR、Lに基づいてモータ１２R、Lを制御するための指示を機構制御部１１に送信する。計画として受信している場合では計画の時系列に従い、現在時刻に対応する目標値を使用する。また、倒立振子型移動体１の現在位置ｐ(t0)を算出する。現在時刻ｔ０における位置の算出方法は数式９による。
ｔは時刻を表し車輪角度φR、Lと回転角速度ｄφR、Lはｔの関数で記述される。またｆは車輪１３Rと１３Lの間隔である。
速度計画部２４から計画が状態量を目標として取得された場合は数式１０に従った並進用モータトルクτｓを算出する。
Figure JPOXMLDOC01-appb-M000010
　ただし、K(i) [i=１、２、３、４]はスカラー量であり、ヒューリスティックに、またLQRやＨ∞制御論により設計できる。またθr、ｄθr、φ、ｄφはそれぞれ上体角度目標値、上体角速度目標値、車輪角度目標値、車輪角速度目標値である。またφは車輪回転角度φR、Lの平均、ｄφは車輪回転角速度ｄφR、Lの平均である。緊急停止動作時のように車輪回転角度φｒが指定されない場合は車輪回転角度に関する部分は無視する。速度計画部２４より最大トルク指示、または転倒指示を受け取った場合は取得したモータトルク目標値をそのまま機構制御部１１に送信する。
経路計画に追従するための旋回モータトルクτｃの算出法について述べる。経路計画は前述のように経路上の点群データでありｈ[i]で表記する(i=1、2、…ｎ）。倒立制御部２５は現在位置に最も近い経路計画上の点ｈ[j]と点ｈ[j]までの距離Hを算出する。次に経路計画上の点ｈ[j]の前後の点ｈ[j+1]、ｈ[j-1]を用いて点ｈ[j]での目標旋回方向ωｒとその旋回半径ηを算出する。次に現在の目標移動速度ｖrを旋回半径ηで除算し目標旋回角速度ｄωｒを得る。以上を用いて数式１１に従い旋回用モータトルクτｃを算出する。
Figure JPOXMLDOC01-appb-M000011
ただし、K(i) [i=５、６、７]はスカラー量であり、ヒューリスティックに、または前述のような各種制御理論により設計できる。倒立制御部２５は機構制御部１１に対してモータ１２Lに出力させるためのトルクとしてτｓとτｃの和を、モータ１２Rに出力させるためのトルクとしてτｓとτcの差を送信する。
移動機構１０が停止命令を受信した場合は、機構制御部１１はモータ１２R、Lがトルクを出力しないように制御する。また車輪１３R、Lの角度と角速度をモータ１２R、Lを介して計測し、倒立制御部２５に送信する。
　以上のように構成された倒立振子型移動体１は姿勢が乱れた状態からでも最大加速度＝最大モータトルクで停止することができる。また、途中で加速度が０にならない為に目標値に上体の傾斜角度と上体の傾斜角速度が０になる場面が計画終了の点以外に存在しない。本実施例で構成された倒立振子型移動体１の緊急停止動作実施時のシミュレーションによる挙動を図９に示す。
　図９は本発明の実施例に係る効果のシミュレーション結果を表すグラフ図である。　
　図９において、倒立振子型移動体１が加速中に本実施例の緊急計画を行った際の例を実線で、従来例として、緊急停止計画を単に車輪角速度ｄφと傾斜角度θと傾斜角速度ｄθを０としたものを破線で表している。従来手法では急激な目標値変化に対応できず、転倒に至っているが、本実施例の手法では速度変更に成功し、停止に成功している。
　実施例１では、前進移動中に速度を０とする緊急停止を示したが、後進移動中に速度を０とする場合、または任意の速度に現状の速度から変更する場合にも用いることができる。また、実施例１の例えば、S２４_７_２やS２４_７_６では取得した現在の情報をもとに処理を進めたが、計画開始時点での状況をもとにシミュレートを行い、タイミングを決定し、全ての計画を予め生成しても良い。
　実施例１では、経路計画部２３において経路を生成していたが、行動決定部２２と同様に予め設定されている経路や、外部より直接経路情報を取得しても良い。また、経路計画を速度計画部２４に送信するに際し、経路計画情報を座標のデータで送信せず、時刻をパラメータとした関数や時刻と座標を組みとするデータ群で送信しても良い。
　実施例１の緊急停止動作生成では最大加速度で停止するとしたが、変更後速度が変更前である現在の速度との差が小さい場合においては最大加速度を用いず、適当に小さな加速度を用いても良い。また、減速曲線に姿勢を動かすS２４_７_３においても最大トルクを用いなくとも良い。
　実施例１では姿勢センサから上体の傾斜角度を取得するとしたが、姿勢センサは傾斜角速度のみを測定し、計算機が傾斜角速度を取得後に積分することで傾斜角度を得ても良い。
　１・・・倒立振子型移動体、２・・・上体、３・・・計算機、４・・・環境センサ、５・・・姿勢センサ、１０・・・移動機構、１１・・・機構制御部、１２・・・モータ、１３・・・車輪。

Claims (4)

  1. 　移動体本体に懸架されて床面を走行面とし、進行方向に垂直な同一平面上に備えられた一対の車輪と、この車輪を回転させる駆動機構と、前記移動体本体の重心位置と前記一対の前記車輪の車軸を結ぶ直線と鉛直方向のなす上体の傾斜角速度を計測する姿勢計測手段と、前記駆動機構を制御することで移動体本体の倒立状態を維持する機構制御部とを備えた倒立振子型移動体において、
    　前記移動体の移動中に速度変更の必要が生じた際において、前記上体の傾斜角速度と上体の傾斜角度が二次関数の関係になるように移動計画を行う速度計画装置を備えたことを特徴とする倒立振子型移動体。
  2. 　請求項１の倒立振子型移動体において、
    　前記移動体の移動中に速度変更の必要が生じた際において、一定の車輪駆動力を発生させた後に、上体の傾斜角速度と上体の傾斜角度が二次関数の関係になるように移動計画を行う速度計画装置を備えたことを特徴とする倒立振子型移動体。
  3. 　請求項１または２のいずれかに記載の倒立振子型移動体において、
    　上体の傾斜角度目標値が非零かつ、上体の傾斜角速度を零である目標値を一定時間計画する速度計画装置を備えたことを特徴とする倒立振子型移動体。
  4. 　請求項１または２のいずれかに記載の倒立振子型移動体において、
    　速度変更の際に、速度変更終了まで上体の傾斜角度が零かつ、上体の傾斜角速度が零を目標値として出力しない速度計画装置を備えたことを特徴とする倒立振子型移動体。
PCT/JP2011/066010 2011-06-28 2011-07-13 速度計画装置を備えた倒立振子型移動体 WO2013001658A1 (ja)

Priority Applications (4)

Application Number Priority Date Filing Date Title
KR1020137034762A KR101567456B1 (ko) 2011-06-28 2011-07-13 속도 계획 장치를 구비한 도립 진자형 이동체
JP2013522670A JP5871925B2 (ja) 2011-06-28 2011-07-13 速度計画装置を備えた倒立振子型移動体
US14/125,192 US9180926B2 (en) 2011-06-28 2011-07-13 Inverted pendulum type moving body having velocity planning device
DE112011105397.5T DE112011105397B4 (de) 2011-06-28 2011-07-13 Beweglicher Körper vom Typ inverses Pendel mit Geschwindigkeitsplanungsvorrichtung

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
JPPCT/JP2011/064832 2011-06-28
JP2011064832 2011-06-28

Publications (1)

Publication Number Publication Date
WO2013001658A1 true WO2013001658A1 (ja) 2013-01-03

Family

ID=47423596

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/JP2011/066010 WO2013001658A1 (ja) 2011-06-28 2011-07-13 速度計画装置を備えた倒立振子型移動体

Country Status (4)

Country Link
US (1) US9180926B2 (ja)
KR (1) KR101567456B1 (ja)
DE (1) DE112011105397B4 (ja)
WO (1) WO2013001658A1 (ja)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2014170747A3 (en) * 2013-04-18 2015-01-08 Toyota Jidosha Kabushiki Kaisha Moving body control device, moving body control method, and control program
JP2021096501A (ja) * 2019-12-13 2021-06-24 株式会社日立ビルシステム ロボット充電システム
WO2022044389A1 (ja) * 2020-08-27 2022-03-03 オムロン株式会社 制御装置、および制御方法

Families Citing this family (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP6220763B2 (ja) * 2014-11-06 2017-10-25 本田技研工業株式会社 倒立振子型車両
DE102016104147A1 (de) * 2016-03-08 2017-09-14 Rolf Strothmann Von Hand bewegbare Transportvorrichtung, insbesondere Golfcaddy
CN105758587B (zh) * 2016-04-27 2018-05-29 小煷伴（深圳）智能科技有限公司 机器人重心感知反馈系统
US10858027B2 (en) * 2017-06-08 2020-12-08 Segway Inc. Control of a power-assisted payload carrying vehicle using operator-applied force sensing
DE202020105169U1 (de) 2020-09-08 2021-12-09 Rolf Gseller Vorrichtung mit einem Behälter und einem Antriebsmodul

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH05181537A (ja) * 1992-01-07 1993-07-23 Japan Steel Works Ltd:The スライディングモード制御方法および装置
JP2000052985A (ja) * 1998-08-17 2000-02-22 Sumitomo Metal Ind Ltd 鉄道車両の振動抑制方法及び装置
JP2010047095A (ja) * 2008-08-20 2010-03-04 Equos Research Co Ltd 車両

Family Cites Families (20)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US5355064A (en) * 1992-03-04 1994-10-11 Honda Giken Kogyo Kabushiki Kaisha Control system for legged mobile robot
US7090040B2 (en) * 1993-02-24 2006-08-15 Deka Products Limited Partnership Motion control of a transporter
US6799649B2 (en) * 1999-03-15 2004-10-05 Deka Products Limited Partnership Control of a balancing personal vehicle
US7275607B2 (en) 1999-06-04 2007-10-02 Deka Products Limited Partnership Control of a personal transporter based on user position
US7900725B2 (en) * 2002-06-11 2011-03-08 Segway Inc. Vehicle control by pitch modulation
JP2004215350A (ja) * 2002-12-27 2004-07-29 Sony Corp 駆動制御装置およびその方法と２輪車
US20060063137A1 (en) * 2004-02-21 2006-03-23 Robbins Alan R Wheeled vehicles and control systems and methods therefor
JP2006136962A (ja) * 2004-11-11 2006-06-01 Hitachi Ltd 移動ロボット
JP2007168603A (ja) 2005-12-21 2007-07-05 Matsushita Electric Works Ltd 同軸二輪移動台車
JP5107533B2 (ja) 2006-06-01 2012-12-26 株式会社日立製作所 移動ロボット
JP5112666B2 (ja) 2006-09-11 2013-01-09 株式会社日立製作所 移動装置
US7762569B2 (en) * 2007-09-27 2010-07-27 Fraser Douglas B Cycle having unique balancing capabilities
US20110264350A1 (en) * 2008-10-22 2011-10-27 Equos Research Co., Ltd. Vehicle
US8170780B2 (en) * 2008-11-06 2012-05-01 Segway, Inc. Apparatus and method for control of a vehicle
CN102362233B (zh) * 2009-01-26 2013-11-06 爱考斯研究株式会社 车辆
CN101590323B (zh) * 2009-07-08 2012-10-31 北京工业大学 独轮机器人系统及其控制方法
JP5386282B2 (ja) * 2009-09-18 2014-01-15 本田技研工業株式会社 歩行補助装置
US8219308B2 (en) * 2010-02-02 2012-07-10 Leeser Karl F Monowheel type vehicle
CN102811901B (zh) * 2010-02-26 2015-01-14 塞格威股份有限公司 车辆控制的装置和方法
TWI392619B (zh) * 2010-03-10 2013-04-11 Nat Univ Tsing Hua 一種人力驅動與電力輔助平衡之個人載具

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH05181537A (ja) * 1992-01-07 1993-07-23 Japan Steel Works Ltd:The スライディングモード制御方法および装置
JP2000052985A (ja) * 1998-08-17 2000-02-22 Sumitomo Metal Ind Ltd 鉄道車両の振動抑制方法及び装置
JP2010047095A (ja) * 2008-08-20 2010-03-04 Equos Research Co Ltd 車両

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2014170747A3 (en) * 2013-04-18 2015-01-08 Toyota Jidosha Kabushiki Kaisha Moving body control device, moving body control method, and control program
JP2021096501A (ja) * 2019-12-13 2021-06-24 株式会社日立ビルシステム ロボット充電システム
WO2022044389A1 (ja) * 2020-08-27 2022-03-03 オムロン株式会社 制御装置、および制御方法
JP7420015B2 (ja) 2020-08-27 2024-01-23 オムロン株式会社 制御装置、および制御方法

Also Published As

Publication number Publication date
US9180926B2 (en) 2015-11-10
US20140114559A1 (en) 2014-04-24
DE112011105397T5 (de) 2014-04-24
KR20140031339A (ko) 2014-03-12
DE112011105397B4 (de) 2021-08-19
KR101567456B1 (ko) 2015-11-11

Similar Documents

Publication Publication Date Title
WO2013001658A1 (ja) 速度計画装置を備えた倒立振子型移動体
US7649331B2 (en) Mobile robot
US10265855B2 (en) Wall-climbing aerial robot mechanism and method of controlling the same
US20220050467A1 (en) Unobtrusive driving assistance method and system for a vehicle to avoid hazards
US11465617B2 (en) Vehicle path planning
JP6594589B1 (ja) 走行計画生成装置および自動運転システム
KR20150038776A (ko) 인프라 센서를 이용한 자동주차 시스템
JP2007161198A (ja) 走行装置及びその制御方法
JP2011043884A (ja) 半自律型無人車両の遠隔操縦システム
US20220057804A1 (en) Path determination method
Mulgaonkar et al. The tiercel: A novel autonomous micro aerial vehicle that can map the environment by flying into obstacles
JP5382793B2 (ja) 半自律型無人車両の遠隔操縦システム
JP5366702B2 (ja) 無人車両遠隔操作システム
US11046373B2 (en) Mobile device and mobile device management system
JP6468127B2 (ja) 全方位移動体、その制御方法及びプログラム
WO2021074660A1 (ja) 物体認識方法及び物体認識装置
JP5871925B2 (ja) 速度計画装置を備えた倒立振子型移動体
KR102178992B1 (ko) 차량 동작 계획방법 및 그를 위한 장치
WO2020136977A1 (ja) 経路決定装置、ロボット及び経路決定方法
JP2021077003A (ja) 走破可否判定装置及び移動体の制御装置
Jordan et al. Real-time model based path planning for wheeled vehicles
JP2019043191A (ja) 車両制御装置
US20230391350A1 (en) Systems and methods for hybrid open-loop and closed-loop path planning
TWI715221B (zh) 自適應軌跡生成方法及系統
JP5463628B2 (ja) 位置推定装置

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 11868751

Country of ref document: EP

Kind code of ref document: A1

ENP Entry into the national phase

Ref document number: 2013522670

Country of ref document: JP

Kind code of ref document: A

WWE Wipo information: entry into national phase

Ref document number: 14125192

Country of ref document: US

ENP Entry into the national phase

Ref document number: 20137034762

Country of ref document: KR

Kind code of ref document: A

WWE Wipo information: entry into national phase

Ref document number: 1120111053975

Country of ref document: DE

Ref document number: 112011105397

Country of ref document: DE

122 Ep: pct application non-entry in european phase

Ref document number: 11868751

Country of ref document: EP

Kind code of ref document: A1