JP5107533B2 - Mobile robot - Google Patents

Description

  The present invention relates to a mobile robot, and is particularly suitable for an inverted pendulum type mobile robot.

  As an inverted pendulum type mobile robot, those described in Japanese Patent Application Laid-Open No. 63-305082 (Patent Document 1) and Japanese Patent Application Laid-Open No. 2004-345030 (Patent Document 2) are known.

These mobile robots include a pair of wheels, an axle laid between the two wheels, an upper body supported by the axle, a wheel drive device, and a control device that controls the wheels. The inclination of the mobile robot is detected by the body inclination angle measuring means, and the rotation angle of the wheel is detected by the wheel rotation angle detection means. The wheel drive device calculates the drive torque by substituting the detected inclination angle of the body and the rotation angle of the wheel into a preset control input equation, and controls the wheel drive motor to invert the wheel drive device. Do. Calculation of the control input expression, based on the equation of motion of the mobile robot, various control theory (e.g., LQR, LQG, H∞ control theory, H ₂ control theory, .mu. design method) is determined by.

JP 63-305082 A JP 2004-345030 A

  In order to utilize a mobile robot in a human living space, agility and stability that do not give stress to human beings and compactness that does not get in the way are important. An example of a mobile robot that satisfies these conditions is an inverted pendulum. Inverted pendulum type mobile robots have the advantage that the bottom area of the mobile robot can be reduced while realizing agile movement because the center of gravity is actively changed to move.

  However, an inverted pendulum generally has a characteristic of moving in the opposite direction when it starts and stops. In the conventional technology, the speed or position is controlled by the control using the speed or position as the target input. However, in such control, the amount of movement of the inverted pendulum cannot be predicted, so the area where the mobile robot can move is limited. It was difficult to control the location so that it was within the area.

  In addition, since the motor driving torque cannot be predicted, it is difficult to keep the motor driving torque within the limits of the motor. For this reason, there is a problem in that only the target input with a margin can be given to the output torque and the movable region, and the original performance of the mobile robot is limited.

  In addition, when there is or may be an obstacle moving on the path of the mobile robot, if the behavior of the mobile robot is not predicted accurately, the target will be greatly increased to avoid the obstacle. This leaves room for input, limiting the original performance of the mobile robot.

  Further, when the mobile robot performs work while moving, if the inclination of the upper body can be predicted, the positional relationship between the mobile robot and the work target is clarified, and work instructions are facilitated. However, in the conventional method for specifying the movement to the inverted pendulum, the moving speed / acceleration cannot be predicted. Therefore, the body angle closely related to the moving speed / acceleration cannot be predicted, and it is difficult to give a work instruction during movement.

The object of the present invention is to specify the movement distance, the maximum speed during movement, and the maximum motor drive torque during movement, and the mobile robot stops the position, speed, body angle, body angle speed, and motor drive torque from the start of movement. It is to provide a technique for obtaining an operation plan of a target value that can be followed without deviation .

In order to achieve the above-described object, the present invention provides a moving mechanism having left and right wheels and a travel motor for rotationally driving the wheels, a body supported rotatably by the moving mechanism, and the moving mechanism. In an inverted pendulum type mobile robot having a control device to control, the control device generates a movement target value that is a movement target that is a movement target of the mobile robot, and a movement target value in time series The motion planning unit acquires the travel distance, the maximum speed, and the maximum motor drive torque that are the travel conditions from the movement target generation unit, and the position and body angle of the mobile robot. By representing each of the movement conditions, that is, the movement distance, the maximum speed, and the maximum driving torque of the movement motor, using the feature point that is a linear sum of the above and the time derivative of the feature point up to the fourth floor as variables. The conditions of the moving distance, the maximum speed, and the maximum driving torque of the moving motor are treated as the constraint conditions to be satisfied for the feature point and the time derivative up to the fourth floor of the feature point, and the values along the time series of the fourth derivative of the feature point are By designing so as to satisfy the constraint conditions, the linear sum of the feature point along the time series that satisfies the movement condition and the time derivative of the feature point up to the fourth floor is calculated as the position, speed, body angle of the mobile robot, It is generated as a movement target value along the time series consisting of torque. Furthermore, the fourth-order time differentiation of the feature points along the time series is planned with all the time differentiation values up to the third floor of the feature points. It is planned to stop from the stop of the mobile robot by designing it to be 0 at the start and 0 after reaching the target moving distance of the moving condition .

A preferred specific configuration example of the present invention is as follows.
(1) An obstacle sensor and an obstacle behavior prediction unit are provided, and a collision is predicted from the generated movement target value along the time series and the obstacle prediction value from the obstacle behavior prediction unit.

According to the inverted pendulum type mobile robot of the present invention, it is possible to provide an operation plan of a target value that can be followed without deviation when moving, and to remove the restriction of the movable area and widen the movable area of the mobile robot. Can do .

  A mobile robot according to an embodiment of the present invention will be described below with reference to FIGS.

  First, the mechanism configuration of the mobile robot 1 of this embodiment will be described with reference to FIG. FIG. 1A is a front view for explaining the mechanism configuration of the mobile robot of this embodiment, and FIG. 1B is a side view of the mobile robot of FIG. 1A.

  The mobile robot 1 is an inverted pendulum type mobile robot, and is largely divided into a moving mechanism 2 and an upper body 3. The moving mechanism 2 includes left and right wheels 4 and 5 and travel motors 6 and 7 that rotationally drive these wheels. The upper body 3 is rotatably supported on the upper part of the moving mechanism 2. At the top of the moving mechanism 2 is provided a posture direction sensor 8 that detects the inclination of the upper body 3 with respect to the vertical direction. On the upper body 3, work devices such as a manipulator 9 for work and a head 10 having an interpersonal interface function are mounted. Further, the body 3 has a built-in control device 11 for controlling the entire robot, and an obstacle sensor 12 for measuring the position and shape of the obstacle in the running direction existing in the running area. .

  Next, the configuration of the control system of the mobile robot 1 will be described with reference to FIG. FIG. 2 is a control system configuration diagram of the mobile robot of FIG.

  The control device 11 includes a movement target generation unit 22, an operation planning unit 20, a route control unit 21, a travel target generation unit 17, an obstacle behavior prediction unit 18, a collision prediction unit 19, and left and right motor drive devices 13 and 14. .

  The movement target generation unit 22 generates an arrival position, a movement time, a movement speed, a maximum movement acceleration, a maximum motor driving torque, and the like, which are movement targets of the mobile robot 1.

  The motion planning unit 20 receives the arrival position, movement time, movement speed, maximum movement acceleration, and maximum motor drive torque from the movement target generation unit 22 as targets, and the target position, target speed, target inclination along the time series of the mobile robot. Angle, moving motor driving torque is generated. Hereinafter, the target position, the target speed, the target inclination angle, and the moving motor driving torque are collectively referred to as a moving target value. A method for generating the movement target value will be described later.

  Further, the route control unit 21 acquires the arrival position from the movement target generation unit 22, generates a route to the arrival position, and calculates a turning angle target value and a turning angular velocity target value on the route. Hereinafter, the turning angle target value and the turning angular velocity target value are collectively referred to as a turning target value.

The travel target generation unit 17 acquires the movement target value from the motion planning unit 20, acquires the turning target value from the route control unit 21, acquires the wheel speeds v ₁ and v ₂ from the left and right encoders 15 and 16, and the posture. The inclination angle θ ₁ and the inclination angular acceleration dθ ₁ / dt of the upper body 3 are acquired from the sensor 8, and the target motor driving torques τ _1r and τ _2r are designated to the left and right motor driving devices 13 and 14 based on them.

  The obstacle sensor 12 attached to the upper body 3 detects the position and speed of the obstacle, and based on this information, the obstacle behavior prediction unit 18 predicts how the obstacle moves.

  The collision prediction unit 19 acquires the movement target value from the motion planning unit 20, acquires the turning target value from the route control unit 21, acquires the predicted value of the obstacle from the obstacle behavior prediction unit 18, and based on them. The presence or absence of contact with an obstacle is determined, and if it is predicted that a collision will occur, a signal is output to the travel target generator 17 so as to immediately stop the movement.

In the left and right motor drive devices 13 and 14, the target motor drive torques τ _1r and τ _2r are acquired from the travel target generator 17, and the wheel speeds v ₁ and v ₂ are acquired from the left and right encoders 15 and 16, respectively. Control is performed so that the motor drive torques τ ₁ and τ _{2 of the} motors 5 and 6 are equal to the target motor drive torques τ _1r and τ _2r .

  Next, the operation of the control device 11 performing the inverted pendulum control of the mobile robot 1 will be described with reference to FIG.

  First, how the driving target generator 17 calculates the motor driving torque for keeping the mobile robot 1 standing will be described.

The mobile robot 1 is considered in a two-dimensional manner and is assumed to be composed of a wheel drive mechanism 2 and an upper body 3. The wheel drive mechanism 2 includes wheels 4 and 5, travel motors 5 and 6, and an axle that connects the left and right wheels 4 and 5 and the travel motors 5 and 6, and has a mass m ₀ and an inertia moment around the axle. Let _J0 . The upper body 3 is the other part, and is represented by a mass m ₁ , a moment of inertia j ₁ around the center of gravity, and a mass point where the distance between the axle and the center of gravity is l. In addition, the radius of the wheel is r, and the viscous resistance between the wheel drive unit and the upper body 3 is d. These parameters m ₀ , m ₁ , J ₀ , J ₁ , l, r, and d may be measured with actual machines or calculated from design values. The rotation angle between the upper body 3 of the wheels 4 and 5 is θ _0, and the inclination of the upper body from the vertical direction is θ ₁ . Further, when the output torque of the motors 5 and 6 is τ, the equation of motion is expressed as the following equation (1).

In order to simplify the design of the control system, linearization of the equation (1) is performed assuming that the nonlinear term θ ₁ is sufficiently small. Thereby, the simplified equation of motion is expressed as the following equation (2).

  The following equation (3) shows a simplified equation of motion of equation (2) expressed in state space.

The controlled object represented by the expression (3) can be stabilized by calculating the state feedback gain based on various known control theories and performing the state feedback. Briefly, as shown in the following equation (4), θ ₀ , dθ ₀ / dt, θ ₁ , dθ ₁ / dt are multiplied by state feedback gains K ₁ , K ₂ , K ₃ , and K ₄ , respectively. By designating the added value as τ, it is possible to maintain the position while maintaining the position and maintaining the speed 0.

  Further, the position (x, y) and direction (φ) of the mobile robot performed by the travel target generation unit 17 are calculated as follows.

Using the left and right wheel angular velocities v ₁ and v ₂ obtained from the left and right encoders 15 and 16, the position of the mobile robot 1 at the time t is represented by p (t) = (x, y) and the speed v (t ) = (Dx / dt, dy / dt), bearing is (φ), and turning speed is (dφ / dt) as shown in equations (5) and (6).

  Here, the mobile robot 1 can be moved by rewriting the current position to a deviation between the target position and the current position, or by changing the current speed to a deviation between the target speed and the current speed. .

However, the mobile robot system represented by Expression (3) cannot follow a target position or target speed whose current position or current speed is simple, such as an input on a step, a trapezoidal speed pattern, or a trapezoidal acceleration pattern without deviation. In a conventional simple target designation method, when following a target position or target speed, the robot moves in the opposite direction immediately after overshooting or following tracking. This means that the mobile robot 1 performs an action that is not given as a target, which may cause a problem such as a collision that did not occur at the time of planning. In addition, when the mobile robot performs an operation that deviates from the target, it can be known at the stage of specifying the target position and target speed whether the motor drive torque is within the motor capacity when the mobile robot is actually run. In order to check whether the specified target position or target speed is excessively demanding the motor, it must be confirmed once using simulation or an actual machine, and the result is the state feedback gain K ₁ , K ₂ , K ₃ , and K ₄ are different depending on the size.

Therefore, in order to solve the above problem, the motion planning unit 20 of the present embodiment sets the movement distance, the maximum speed, and the movement motor maximum drive torque, and then the inverted pendulum type mobile robot starts moving. A movement target value is generated along a time series that can follow without deviation until the movement stops.

Generates a movement target value along the time series that the inverted pendulum type mobile robot can follow without any deviation from the movement start to the movement stop so that the motion planning unit 20 satisfies the movement distance, maximum speed, and movement motor maximum drive torque. Means for performing this will be described with reference to FIG. FIG. 3 is a flowchart showing a procedure for obtaining a movement plan of the mobile robot of FIG.

First, the motion planning unit 20 acquires a moving condition including three of the moving distance L, the maximum speed Vmax, and the moving motor maximum drive torque τmax from the moving target generator (S1). Next, as shown in Expression (7) using the moving distance L, the maximum speed V _max , and the maximum driving motor torque τ _max , an acceleration time t ₁ that is a parameter of a fourth-order differentiation of a feature point q (t) described later. The constant velocity time t ₂ and the maximum absolute value k are obtained (S2).

  Based on the calculated parameters, a feature point q (t) along the time series is obtained by the following equation (8) (S3).

Finally, the position θ ₀ r (t), the velocity dθ _0r / dt (t), the body angle θ _1r (t) of the mobile robot, which are the movement target values along the time series, using the formula (9), The angular velocity dθ _1r / dt (t) and the torque τ _r (t) are obtained from the feature point q (t) along the time series (S4).

  The above description will be given below.

  First, formula (10) is obtained by arranging and formulating formula (2) assuming d = 0.

  Here, the feature point q is defined as in Expression (11).

The feature point q is a point that exists above the ground contact point of the mobile robot. By using the feature point q, the position θ _0r , the velocity dθ _0r / dt, the body angle θ _1r , and the body angular velocity dθ _1r of the movement target value are used. / Dt and torque τ _r can be expressed as in equation (9). If it is designed to satisfy the movement distance L, the maximum speed V _max , and the movement motor maximum drive torque τ _max targeted for the feature point q from the equation (9), the movement target value position θ _0r , speed dθ _0r / dt, The target values along the time series of the body θ _1r , the body angular velocity dθ _1r / dt, and the torque τ _r are obtained. Further, assuming that the movement target value is designated from the stop to the stop, the fourth-order differentiation of q is as shown in Expression (8). By designating the movement target value from stop to stop, the wheel speed dθ _0r / dt and its derivative take zero at the start and end of movement. In addition, the body angle θ _1r is also 0 when stopped. From here, the first-order differential, second-order differential, and third-order differential value of q must be 0 at the start and end of movement. Furthermore, there are three design degrees of freedom for simultaneously satisfying the movement distance L, the maximum speed V _max , and the movement motor maximum drive torque τ _max . The fourth-order derivative of q defined by Equation (8) is a value that depends on time t and has three degrees of freedom: acceleration time t ₁ , constant velocity time t ₂ , and maximum absolute value k. Satisfy all. By using a movement condition consisting of three, that is, a movement distance L, a maximum speed V _max , and a maximum drive motor torque τ _max , the parameter acceleration time t ₁ , constant speed time t ₂ , and maximum absolute value k of the fourth order differential of q are It can be obtained analytically by combining equations (8) and (9). Since the acceleration time t ₁ , the constant velocity time t ₂ , and the maximum absolute value k are real numbers, the analytical solution obtained is given by Equation (7).

  Examples of the obtained movement target value, the feature point q, and the time series values up to the fourth order differentiation thereof are shown in FIGS.

As described above, the motion planning unit 20 sets the movement distance, the maximum speed, and the maximum driving motor driving torque, and generates a movement target value along a time series that can be followed without deviation of the inverted pendulum type mobile robot. To do. In order to make the mobile robot follow the movement target value, as shown in the following equation (12), the current position in equation (4) is set to the difference between the current position and the target position, and the current velocity is converted to the current velocity and the target velocity. the difference between the current upper body angle difference between the current upper body angle and desired body angle, inverted by the respective variable puff Rukoto the difference between the current upper body angular velocity and the desired body angular velocity of the current body angular velocity Motor drive torque τs is calculated.

Next, collision prediction will be described. The obstacle behavior prediction unit 18 records the position and size of the obstacle detected by the obstacle sensor 12. Recording is performed every scanning time e seconds of the obstacle sensor. When the position of one obstacle o recorded at time t is represented by p _o (t) in the x, y plane, the speed of the obstacle v _o (t) is expressed as the following equation (13). Further, the obstacle is considered to be a cylinder having a radius R _o around the position p _o (t) of the obstacle.

The processing procedure of the collision prediction unit is shown in the flowchart of FIG. First, the collision prediction unit 19 acquires the obstacle position p _o (t), the velocity v _o (t), and the radius Ro from the obstacle behavior prediction unit 18 (S11), and the following equation (14): _Is used to obtain the predicted position p _op (t + k) of the obstacle (S12).

Next, the collision prediction unit 19 acquires the movement plan of the mobile robot 1 from the motion planning unit 20, and acquires the predicted speed v _p (t) expressed by the following equation (15) and the movement route from the route control unit 21 ( S13).

Assuming that the mobile robot follows the movement plan without deviation on the path of the obtained mobile robot, a point where the integration from the time t to the time (t + k) of the predicted speed v _p (t) and the path length are equal to each other. The predicted position is p _p (t + k) (S14). Judgment of the collision is made by using the following equation (16) whether or not the predicted position p _op (t + k) of the obstacle is included in the range of the radius R centering on the predicted position p _p (t + k) of the mobile robot ( S15). Here, Rr is a radius when the mobile robot is a cylinder, and Rp is an error range of the predicted position of the mobile robot.

  If the expression (16) is established, it is determined that a collision has occurred, and a movement stop is designated to the travel target generator 17 (S16).

In the conventional method, in order to move the robot is unable to follow the deviation without relative movement plan, it is necessary to increase the error range R _p of the predicted position of the mobile robot. However, it is possible to reduce the error range R _p of the predicted position of the mobile robot by using the deviation without tracking possible movement plan shown above, the conventional method had to be stopped to determine mobile robot collision Even in the scene, the mobile robot can correctly determine that it does not touch an obstacle, and can move without colliding or stopping.

  According to the present embodiment, as described above, the motion planning device can provide a motion plan with a target value that can follow the inverted pendulum type mobile robot without deviation during movement. In the motion plan obtained from the motion planning device of the present embodiment, the mobile robot follows the motion plan without deviation, so that the restriction of the movable area can be removed and the movable area of the mobile robot can be widened. In addition, it is possible to provide a mobile robot that can pass through the vicinity of the obstacle and perform smooth movement by increasing the accuracy of collision prediction with the obstacle.

It is a front view explaining the mechanism structure of the mobile robot which concerns on one Example of this invention. It is a side view of the mobile robot of FIG. 1A. It is a control system block diagram of the mobile robot of FIG. It is a flowchart which shows the procedure which acquires the movement plan of the mobile robot of FIG. It is a figure which shows an example of the time series value to the feature point of the mobile robot of FIG. 1, and its 4th-order differentiation. It is a flowchart which shows the procedure which performs the collision prediction of the collision prediction apparatus of FIG.

Explanation of symbols

DESCRIPTION OF SYMBOLS 1 ... Mobile robot, 2 ... Moving mechanism, 3 ... Upper body, 4,5 ... Wheel, 6, 7 ... Traveling motor, 8 ... Attitude sensor, 9 ... Manipulator, 10 ... Head, 11 ... Control device, 12 ... Obstacle Object sensor.

Claims (2)

A moving mechanism having left and right wheels and a travel motor that rotationally drives the wheels;
An upper body rotatably supported by the moving mechanism;
In an inverted pendulum type mobile robot provided with a control device for controlling the moving mechanism,
The control device includes a movement target generation unit that generates a movement condition that is a movement target of the mobile robot, and an operation planning unit that generates a movement target value along a time series,
The motion planning unit acquires the movement distance, maximum speed, and maximum motor driving torque that are the movement conditions from the movement target generation unit , and is a feature point and feature that is a linear sum of the position and body angle of the mobile robot By representing each of the movement distance, maximum speed, and movement motor maximum drive torque, which are the movement conditions, using the time differentiation of the points up to the fourth floor as variables, the movement distance, maximum speed, and movement motor maximum drive, which are the movement conditions, are expressed. By treating the torque condition as a constraint condition to be satisfied by the feature point and the time derivative of the feature point up to the fourth floor, and designing the values along the time series of the fourth derivative of the feature point so as to satisfy the constraint condition , The linear sum of the feature points along the time series satisfying the moving condition and the time differentiation of the feature points up to the fourth floor is the moving eye along the time series consisting of the position, speed, body angle, and torque of the mobile robot. In addition, the fourth-order time differentiation of the feature points along the time series is 0 for all the time differentiation values up to the third floor of the feature points at the start of planning, and the target movement of the movement condition A mobile robot characterized by planning from stop to stop of the mobile robot by designing it to be 0 after reaching the distance . The mobile robot according to claim 1, further comprising an obstacle sensor and an obstacle behavior prediction unit, and predicting a collision from the generated movement target value along the time series and the obstacle prediction value from the obstacle behavior prediction unit. A mobile robot characterized by

Images