JP2009178788A - Mobile robot fall prevention system and fall prevention device - Google Patents

Abstract

To provide a mobile robot overturn prevention system that automatically and quickly responds to the state of a mobile robot. A mobile robot uses an environment map based on information acquired by a sensor. And a self-position estimation unit that estimates the self-position and a route generation unit that generates a movement route. The fall prevention device includes a robot position information acquisition unit 511 that acquires position information of the mobile robot, a route information acquisition unit 512 that acquires the next movement route information of the mobile robot, and the next information of the mobile robot based on these pieces of information. A follow-up position calculation unit 513 that calculates a movement position, and an operation command for causing the mobile robot to follow the hoist based on the next movement position of the mobile robot calculated by the follow-up position calculation unit 513 and an operation to the hoist 430 And a follow-up action command unit 541 for giving a command.

Description

  The present invention relates to a fall prevention system and a fall prevention device for a mobile robot.

  Development of a humanoid robot (biped robot) that autonomously walks on two legs is underway. Such a biped robot is likely to have an accident in which the movement operation is likely to be unstable and the vehicle falls over and breaks. In particular, if the drive motor is servo-off for all axes when an emergency stop is applied, the drive motor will fall like a free fall, and the damage will be enormous.

Therefore, measures are taken to prevent falls during a mobility test. For example, connect the wire hanging from above and the mobile robot, and when the mobile robot is about to fall, lift the mobile robot immediately with the wire so that the mobile robot does not fall even in the event of an abnormality Measures are taken.
In this case, the operator operates the hoist while visually observing the mobile robot, and when an abnormality is recognized, the wire is wound up and the mobile robot is lifted.
However, it is impossible for the operator to operate the hoist by following the mobile robot in this way, or to perform the lifting operation in a timely manner before the mobile robot falls over when an abnormality occurs. Therefore, an invention that has improved such a problem has been proposed (for example, Patent Document 1).

Patent Document 1 describes that a hoist follows a mobile robot. And in patent document 1, three methods are disclosed in making a hoist follow a mobile robot,
(1) A position sensor is provided for each of the mobile robot and the hoist, and the hoist is driven and controlled so that the detection values by the two position sensors are within a predetermined deviation amount.
(2) The mobile robot is imaged with a camera attached to the hoist, and the hoist is driven and controlled so that the positional deviation between the mobile robot and the hoist is within a predetermined range.
(3) It is described that the hoist is driven and controlled according to a preset movement map of the mobile robot.

JP 2004-90126 A

However, in tracking with a camera or position sensor, since the movement of the mobile robot is followed, there is a problem that the movement of the hoist is necessarily delayed with respect to the movement of the mobile robot.
If the movement of the hoist is delayed with respect to the mobile robot, the mobile robot will be pulled by the wire. Then, an unnecessary load is applied to the mobile robot, which hinders the walking motion of the mobile robot, and causes a problem of inducing a fall.
Even when the movement map is used, it is sufficient if the mobile robot moves along a predetermined route. However, this may not be applicable to an autonomous mobile robot that has been developed recently.

  An object of the present invention is to provide a mobile robot tipping prevention system that automatically and quickly responds to the state of the mobile robot.

  The fall prevention system for a mobile robot according to the present invention includes a mobile robot that autonomously walks, and a hoist provided with a wire connected to the mobile robot so that the wire can be drawn and pulled up. A fall prevention system for a mobile robot comprising: a fall prevention device for preventing the fall of the robot, and moving the hoist following the mobile robot substantially above the mobile robot; Generated by the sensor for acquiring the environmental information, a self-position estimating unit for creating a surrounding environmental map based on the information acquired by the sensor and estimating the self-position, and the self-position estimating unit A route generation unit that generates a route of movement using an environmental map, and the fall prevention device includes the movement robot from the self-position estimation unit. A robot position information acquisition unit that receives the position information of the mobile robot upon receiving the self-position estimation information of the robot, a route information acquisition unit that acquires the route information generated by the route generation unit, and the self-position estimation A tracking position calculation unit that calculates a next movement position of the mobile robot based on the information and the route information, and the hoist based on the next movement position of the mobile robot calculated by the tracking position calculation unit. And a follow-up action command unit that generates an action command for causing the mobile robot to follow and gives an action command to the hoist.

In such a configuration, the fall prevention device acquires information on a self-position and a route generated inside the mobile robot, and calculates the next movement position of the mobile robot based on this information. Then, based on the calculated next movement position of the mobile robot, an operation command is generated to control the hoist. Then, the hoist can smoothly follow the movement of the mobile robot.
Conventionally, the movement of the mobile robot is monitored by a sensor (camera, radar, etc.), and when the positional deviation between the mobile robot and the hoist exceeds a predetermined value, the hoist is moved in the direction of the mobile robot to move the mobile robot and the hoist. And the positional deviation within a predetermined value.
However, with this method, the hoist is attached after the mobile robot has moved, and smooth follow-up is impossible in principle.
Furthermore, when the follow-up of the hoist is delayed, the wire pulls the mobile robot, so that an extra force is applied to the mobile robot, which hinders the autonomous walking of the mobile robot.
In this regard, according to the present invention, since the position where the mobile robot moves next is calculated based on information generated inside the mobile robot and the hoist is controlled, the mobile robot can smoothly move without delay. The hoist can be followed.

  Note that the wire may be in the form of a long string, for example, a rope in which fibers or wires are twisted together, and a chain in which rings are connected. Also good.

  In the present invention, in addition to calculating the next movement position of the mobile robot, the follow-up position calculation unit further gives an operation command to the hoist from the acquisition of the self-position estimation information and the path information. It is preferable to calculate a follow-up position for causing the mobile robot to follow the hoist in consideration of a delay time until the hoist moves to the commanded position.

  According to such a configuration, since the next movement of the mobile robot is predicted and the time required for the follow-up operation is taken into consideration, the hoist can accurately follow the mobile robot without delay.

  In the present invention, the mobile robot includes an error signal transmission unit that detects a control error of the drive motor and transmits an error signal, and the overturn prevention device is a pulling operation that instructs the hoist to pull up the mobile robot. A command unit, and an error signal receiving unit that receives an error signal from the error signal transmission unit and transmits error information to the pulling operation command unit, and when the pulling operation command unit receives the error information, It is preferable to instruct the hoist to pull up the mobile robot.

  According to such a configuration, when there is an error in driving the motor of the mobile robot, an error signal is transmitted from the error signal transmission unit. Then, this error signal is received by the error signal receiving unit of the fall prevention device. The error signal receiving unit transmits error information to the pulling operation command unit, and the pulling operation command unit instructs the hoist to pull up the mobile robot. As a result, the mobile robot is quickly lifted when an error occurs. As a result, it is possible to reliably avoid damage to the mobile robot due to falling.

If an error occurs in the motor drive while the mobile robot is walking and the motor is servo-off (power cut-off), the mobile robot will tip over at the same momentum as a free fall, causing fatal damage. There may be a risk of receiving it. Conventionally, when the mobile robot is monitored by a camera or the like and the fall of the mobile robot is recognized by image recognition or by human observation, the pulling operation is started.
Patent Document 1 discloses a method for determining whether a mobile robot has fallen. In Patent Document 1, the determination of a mobile robot to fall is based on (a) a detected value of the tilt angle of the robot, and (b) a mobile robot and a hoist. (C) abnormality in wire tension, and (d) acceleration detection value of a mobile robot.

Also, for example, in Japanese Patent Application Laid-Open No. 2006-224202, in a configuration in which a mobile robot is imaged by an imaging unit provided in a wire expansion / contraction mechanism and the wire expansion / contraction mechanism follows the mobile robot based on the captured image, In the case of falling, it is described that the moving robot is judged to fall using the fact that the size of the image of the mobile robot is larger than a predetermined threshold in the captured image.
Conventionally, since the lifting operation was performed after the mobile robot had fallen like this, the lifting was not in time, and the mobile robot was often severely damaged.

  In this regard, in the present invention, when there is a risk of servo-off due to detection of a motor drive error, the pulling operation is performed quickly, so that it is possible to reliably avoid a situation where the mobile robot suddenly falls down and breaks. can do.

  In the present invention, the overturn prevention device determines whether the mobile robot is in a fall state while determining whether the mobile robot is in a fall state and a pulling operation command unit that commands the hoist to pull up the mobile robot. A fall detection unit that determines a direction of the fall and outputs a signal to the pulling operation command unit according to the judgment result, and the pulling operation command unit receives the signal from the fall detection unit. Accordingly, it is preferable to move the hoist in the direction in which the mobile robot falls when the hoist is instructed to pull up the mobile robot.

In such a configuration, the fall detection unit determines whether or not the mobile robot is in a fallen state. A method for specifying whether or not the mobile robot is in a fallen state is not particularly limited, and the mobile robot may be automatically recognized from an attitude image of the mobile robot or an image of the mobile robot. When the mobile robot is in a fall state, the direction in which the mobile robot is falling is specified by the fall detection unit.
When the mobile robot is in a fall state, the fall detection unit notifies the pulling operation command unit that the mobile robot is in a fall state and also informs the direction in which the mobile robot is falling. When the lifting operation command unit instructs the hoist to lift the mobile robot, the lifting operation command unit also moves the hoist in the direction in which the mobile robot is falling. Thereby, the mobile robot which is falling down can be pulled up smoothly smoothly.
When pulling up the mobile robot obliquely with respect to the axis of the mobile robot body, a large external force acts on the mobile robot body from an undesired direction, which may damage the airframe. This makes it possible to lift the mobile robot straight and smoothly without worrying about damaging the mobile robot body.

  In the present invention, the mobile robot includes an attitude angle detection unit that detects an inclination angle of the mobile robot, and an angle signal processing unit that processes a signal from the attitude angle detection unit and acquires angle information. The fall prevention device includes a pulling operation command unit that commands the hoist to pull up the mobile robot, an attitude angle information receiving unit that receives angle information from the angle signal processing unit, and determines whether the mobile robot is overturned And an angle threshold for the mobile robot is detected by the pulling motion command unit when the angle of inclination of the mobile robot detected by the posture angle detection means exceeds the angle threshold. A fall determination unit that outputs a signal indicating that the angle threshold has been exceeded, and when the pulling operation command unit receives the signal from the fall determination unit, the hoist It is preferred to direct the pulling operation of the dynamic robot.

According to such a configuration, the posture angle of the mobile robot can be constantly monitored, and the pulling operation can be automatically and quickly started when the inclination of the mobile robot exceeds a predetermined threshold. For example, even when the mobile robot stumbles, the mobile robot can be quickly lifted to avoid damage. In many cases, the image recognition by the imaging camera and the human visual judgment are not in time, but according to the present invention, the mobile robot can be pulled up quickly and reliably to avoid the damage caused by the fall.
The angle threshold value may be a threshold value for determining that the mobile robot has completely entered the fall state, or may be set to an angle threshold value that may cause a fall from the viewpoint of danger avoidance.

  In the present invention, the fall prevention device is based on the angle information received by the posture angle information receiving unit when the fall determining unit determines that the tilt angle of the mobile robot has exceeded the angle threshold. And a tilt direction determining unit that determines the tilt direction of the mobile robot and outputs the determined tilt direction to the pulling operation command unit. The pulling operation command unit moves the hoist during the pulling operation of the mobile robot. It is preferable to move in the determined inclination direction.

In such a configuration, when the fall determination unit determines that the attitude angle of the mobile robot exceeds the angle threshold, the fall direction determination unit determines the tilt direction of the mobile robot. For example, by determining the direction in which the inclination angle of the mobile robot exceeds the angle threshold, it is possible to determine the direction in which the mobile robot is falling or the direction in which the mobile robot is likely to fall. Then, the inclination direction of the mobile robot is output from the fall direction determination unit to the pulling operation command unit. The pulling operation command unit moves the hoist in the tilting direction of the mobile robot at the same time when instructing the mobile robot to pull up. Then, the hoist pulls up the mobile robot while moving in the inclination direction of the mobile robot. As a result, it is possible to smoothly lift the mobile robot that is falling down vertically.
When pulling up the mobile robot at a slant with respect to the axis of the mobile robot body, a large external force may be applied to the mobile robot body from an undesired direction, which may damage the airframe. This makes it possible to lift the mobile robot straight and smoothly without worrying about damaging the mobile robot body.

  An overturn prevention device according to the present invention has a hoist provided with a wire connected to a mobile robot so that the wire can be unwound and rolled up, and moves the hoist following the mobile robot on the approximate head of the mobile robot. A fall prevention device for preventing the mobile robot from falling by pulling up the wire, a robot position information acquisition unit for acquiring self-position estimation information generated internally by the mobile robot, and internally generated by the mobile robot Calculated by a route information acquisition unit that acquires route information, a follow-up position calculation unit that calculates a next movement position of the mobile robot based on the self-position estimation information and the route information, and the follow-up position calculation unit And generating an operation command for causing the mobile robot to follow the hoist based on the next movement position of the mobile robot. Characterized in that both and a follow-up operation command unit giving an operation command to the hoist.

  According to such a fall prevention device, a fall prevention device suitable for the fall prevention system can be provided, and the same effects as the fall prevention system can be achieved.

  According to the present invention, since the position where the mobile robot moves next is calculated based on information generated inside the mobile robot and the hoist is controlled, the mobile robot can smoothly follow the hoist. .

The best mode for carrying out the present invention will be described below with reference to the drawings.
1st Embodiment which concerns on the fall prevention system of the mobile robot of this invention is described.
FIG. 1 is a diagram showing an overall configuration of the first embodiment.
The mobile robot overturn prevention system 100 includes a self-standing bipedal mobile robot 200 and a overturn prevention device 300 for preventing the mobile robot 200 from overturning.

The mobile robot 200 is a legged mobile robot having two legs. Each mobile robot 200 has an actuator 271 (not shown in FIG. 1) necessary for a walking motion on each axis, and information on the surrounding environment necessary for autonomous walking. It has a sensor 210 for acquisition.
The mobile robot 200 generates an environmental map based on the sensor information and simultaneously recognizes its own location information (SLAM: simultaneous localization and mapping). Then, the mobile robot 200 autonomously determines a movement route based on the generated map information and self-location information, and performs walking movement along the route.

FIG. 2 is a block diagram illustrating the configuration of the mobile robot 200.
The mobile robot 200 includes a sensor 210, an attitude angle detector 220, a robot control unit 230, and a drive unit 270.
The sensor 210 acquires ambient environment information. The sensor 210 is constituted by, for example, an imaging camera or laser transmission / reception means.
The posture angle detector 220 is provided at the waist of the mobile robot 200 and detects the tilt angle and tilt direction of the mobile robot 200.
Various known gyroscopes can be used as the attitude angle detector 220.
The robot control unit 230 controls the operation of the mobile robot.
The drive unit 270 includes a motor 271 provided on each axis, and is driven and controlled by the robot control unit 230.

The robot control unit 230 includes a state data processing unit 240, an operation command generation unit 250, a drive control unit 260, and an error signal transmission unit 261.
The state data processing unit 240 processes state data from the sensor 210 and the attitude angle detector 220.
The motion command generation unit 250 generates a walking motion command for the mobile robot 200 based on the data obtained by processing by the state data processing unit 240.
The drive control unit 260 controls the drive of the motor 271 of the drive unit 270 based on the operation command from the operation command generation unit 250.
The error signal transmitter 261 detects a control error of the drive controller 260 and transmits an error signal.

The state data processing unit 240 includes a position data processing unit 241 and an angle signal processing unit 244.
The position data processing unit 241 processes the sensor signal from the sensor 210.
The angle signal processing unit 244 processes the angle signal from the attitude angle detector 220.
The position data processing unit 241 includes a self-position estimation unit 242 and a route generation unit 243.
The self-position estimation unit 242 grasps the surrounding environment shape and creates a map, and simultaneously estimates the self-position.
The route generation unit 243 generates a movement route using the map generated by the self-position estimation unit 242.

  Here, when the drive control unit 260 performs feedback control of each motor 271, the error signal transmission unit 261 constantly monitors the control operation state of the drive control unit 260. When a failure state occurs such that the motor control by the drive control unit 260 becomes impossible due to an excessive deviation or the like, the error signal transmission unit 261 detects such a state as an error and transmits an error signal.

  The operation of each functional unit of the mobile robot will be described later with reference to the flowchart of FIG.

Next, the configuration of the overturn prevention device 300 will be described.
The overturn prevention device 300 includes a hoist mechanism 400 and a hoist controller 500.
The hoist mechanism 400 includes a hoist 430 that is provided so as to be able to move two-dimensionally over the head of the mobile robot by travel rails 410 and 420. The hoist control unit 500 controls the operation of the hoist mechanism unit 400 according to the state of the mobile robot 200.

The hoist mechanism unit 400 includes two Y-axis rails 410 and 410, an X-axis rail 420, and a hoist 430.
The two Y-axis rails 410 and 410 are arranged in parallel with each other at a predetermined interval on the head of the mobile robot 200.
The X-axis rail 420 is provided on the two Y-axis rails 410 and 410 so as to be slidable on the Y-axis rail.
The hoist 430 is configured to be slidable on the X-axis rail 420 and to be capable of feeding and pulling a wire connected to the head or shoulder of the mobile robot 200.

FIG. 3 is a block diagram illustrating a configuration of the hoist control unit 500.
The hoist control unit 500 includes a follow-up control unit 510, a fall prevention control unit 520, and an operation command unit 540.
The follow-up control unit 510 obtains a position for following the mobile robot 200 based on information from the mobile robot 200.
The fall prevention control unit 520 performs a control operation for preventing the mobile robot 200 from falling.
The operation command unit 540 gives an operation command to the hoist 430 based on signals from the follow-up control unit 510 and the fall prevention control unit 520.

The tracking control unit 510 includes a robot position information acquisition unit 511, a route information acquisition unit 512, and a tracking position calculation unit 513.
The robot position information acquisition unit 511 receives the self position estimation information from the self position estimation unit 242 and acquires the position information of the mobile robot 200.
The route information acquisition unit 512 receives the route information generated by the route generation unit 243 and acquires the next movement route information of the mobile robot 200.
The follow-up position calculation unit 513 calculates a next movement position for the hoist 430 to follow the mobile robot 200 in consideration of a time delay at the time of an operation command from the hoist control unit 500 to the hoist 430.
The follow-up position calculation unit 513 calculates the current position (x _r , y _r , z _r ) of the mobile robot 200 and further takes into account the delay time τ, and the position of the mobile robot after this delay time ( _{x r ', y r',} z r ') is calculated (see point _{P r} in FIG. 4).
In addition, the position of the hoist 430 corresponding to the position (x _r ′, y _r ′, z _r ′) of the mobile robot 200 after this delay time and the amount of feeding (x _h , y _h , z _h ) of the wire 440 are set. Calculate (point P _{h in} FIG. 4).

The fall prevention control unit 520 includes an error signal reception unit 521 and a fall detection unit 530.
The error signal receiver 521 receives the error signal from the error signal transmitter 261 and transmits error information to the operation command unit 540.
The fall detection unit 530 receives angle information from the angle signal processing unit 244 and monitors the fall of the mobile robot 200.

The fall detection unit 530 includes an attitude angle information reception unit 531, a fall determination unit 532, and a fall direction determination unit (tilt direction determination unit) 533.
The posture angle information receiving unit 531 receives angle information from the angle signal processing unit 244.
The fall determination unit 532 determines the fall of the mobile robot 200 based on the angle information received by the posture angle information reception unit 531.
The fall direction determination unit (tilt direction determination unit) 533 determines the fall direction of the mobile robot 200 when the fall determination unit 532 determines that the mobile robot 200 has fallen.

The posture angle information receiving unit 531 receives the angle signal from the angle signal processing unit 244 and constantly monitors the posture angle of the mobile robot 200. Then, the angle signal is sent to the fall determination unit 532.
In the fall determination unit 532, an angle threshold for determining the fall of the mobile robot 200 is set in advance. The fall determination unit 532 always compares the angle information input from the posture angle information reception unit 531 with the angle threshold, and determines that the mobile robot 200 has entered the fall state when the angle information exceeds the angle threshold. When the fall determination unit 532 determines the fall of the mobile robot 200, the fall information is output to the fall direction determination unit 533 and the operation command unit 540.

  When the fall direction determination unit 533 receives the fall information from the fall determination unit 532, the fall direction determination unit 533 receives angle information from the posture angle information reception unit 531 and obtains a direction in which the mobile robot 200 falls. For example, the direction in which the posture angle exceeds the angle threshold can be confirmed, and the direction can be predicted to be the direction in which the mobile robot 200 falls. The direction is obtained, for example, as θ degrees on the right side or the left side with respect to the traveling direction of the mobile robot 200. The fall direction obtained by the fall direction determination unit 533 is output to the operation command unit 540.

The operation command unit 540 includes a follow-up operation command unit 541 and a pulling operation command unit 542.
The follow-up operation command unit 541 outputs a command to move the hoist 430 to the follow-up position calculated by the follow-up position calculation unit 513.
The pulling operation command unit 542 commands the hoist mechanism unit 400 to perform the pulling operation of the mobile robot in response to the determination result of the overturn determination unit 532 and the overturn direction determination unit 533 or the reception of an error signal.

  In addition, operation | movement of the hoist control part 500 is later mentioned with reference to the flowchart of FIG.

The operation of the first embodiment having such a configuration will be described with reference to FIGS.
FIG. 5 is a flowchart showing an operation procedure of the mobile robot 200.
The operation of the mobile robot will be described with reference to FIG.
First, the mobile robot 200 acquires sensor information from the sensor 210 in ST100. If the sensor 210 is a camera, surrounding image data is acquired.
Next, in ST101, sensor information is processed to process position data.
This process is executed by the self-position estimation unit 242 and the route generation unit 243 of the position data processing unit 241. That is, the information of the sensor 210 is processed to create a surrounding environment map, and the current position of the mobile robot itself is estimated by the self-position estimation unit 242. At the same time, a movement path of the mobile robot 200 is generated. For example, a path that can be walked while avoiding obstacles is generated.

  Next, in ST102 and ST103, position information and route information are transmitted. That is, the position information estimated by the self-position estimation unit 242 and the route information generated by the route generation unit 243 are transmitted to the robot position information acquisition unit 511 and the route information acquisition unit 512 of the hoist control unit 500.

  Then, an operation command for driving the mobile robot 200 is generated by the operation command generation unit 250 based on the self-position and route information obtained in ST101 (ST104). For example, a command for the rotation angle and rotation speed of the servo motor for each axis is generated. The operation command generated in this manner is output to the drive control unit 260, and a control signal for each motor 271 is generated by the drive control unit 260 and output to the motor 271 (ST105). Then, the motor 271 is driven and the mobile robot 200 walks and proceeds (ST106).

  As described above, when the drive control of the motor 271 is performed by the feedback control by the drive control unit 260, the error of the motor control is monitored by the error signal transmission unit 261, and the presence or absence of an error is checked (ST107). For example, a communication failure between the drive control unit 260 and the motor 271 or an excessive deviation between the motor position and the command results in an error. In such a case, the drive control unit 260 stops the control signal to the motor, and the emergency stop state It becomes. The presence or absence of such an error is checked, and if there is an abnormality in the motor drive control by the drive controller 260 (ST107: YES), an error signal is output from the error signal transmitter 261 to the error signal receiver 521. Then, the operation of the mobile robot 200 is ended by an end process (ST113).

  If there is no error (ST107: No), the attitude angle information is acquired (ST108). Then, the angle signal detected by the attitude angle detector 220 is processed by the angle signal processing unit 244 and converted into angle information (ST109). For example, the direction and angle at which the mobile robot 200 is tilted are obtained. The angle information obtained in this way is output from the angle signal processing unit 244 to the attitude angle information receiving unit 531 (ST110).

  Next, it is determined whether or not the end condition is met (ST111). If the end condition is satisfied (ST111: YES), the end process is executed and the walking of the mobile robot is ended. An example of the end condition is that the mobile robot 200 has reached a destination point. On the other hand, if the termination condition is not satisfied, the control operation is continued by returning to ST100.

Next, the operation of the hoist control unit 500 will be described.
FIG. 6 is a flowchart showing an operation procedure of the hoist control unit 500.
The operation of the overturn prevention device will be described with reference to FIG.
When driving of the mobile robot 200 is started, communication between the mobile robot 200 and the hoist control unit 500 is started. First, in ST200, the self-position information of the mobile robot 200 estimated by the self-position estimation unit 242 is acquired by the robot position information acquisition unit 511. Subsequently, in ST201, the route information acquisition unit 512 acquires the route information of the mobile robot 200 generated by the route generation unit 243.
When the information on the self position and the movement route of the mobile robot 200 is acquired in this way, the movement position for the hoist 430 to follow the mobile robot 200 is calculated by the tracking position calculation unit 513 in ST202. That is, the movement position for the hoist 430 to follow the mobile robot 200 without delay in consideration of the command to the hoist 430 and the drive time delay is calculated.

The position of the mobile robot at time t is P _r (t) = (x _r (t), y _r (t), z _r (t))
In this case, the follow-up position calculation unit 513 gives the target movement position S (t) of the hoist at time t based on P (t + τ) in consideration of the delay time τ.

  The tracking position of the hoist 430 calculated in this way is output from the tracking position calculation unit 513 to the tracking operation command unit 541, and is output from the tracking operation command unit 541 toward the hoist 430 (ST203). Then, the operation control in which the hoist 430 follows the position of the mobile robot 200 without delay is executed (ST204).

Next, in ST205, the presence / absence of an error signal is checked.
When an abnormality occurs in the motor drive by the drive control unit 260 of the mobile robot 200, an error signal is output from the error signal transmission unit 261 to the error signal reception unit 521. The error signal reception unit 521 determines whether there is an error signal. To check.

  If there is no error signal (ST205: No), then a fall detection process is executed. That is, in ST206, the posture angle information is acquired by the posture angle information reception unit 531 and the posture angle information of the mobile robot 200 processed by the angle signal processing unit 244 is acquired. The information on the posture angle acquired in this way is output to the fall determination unit 532, and the angle threshold set in advance and the acquired posture angle information are compared in the fall determination unit 532 so that the mobile robot 200 is in the fall state. Whether or not the vehicle falls is determined (ST207). When the posture angle information is equal to or smaller than the angle threshold value, it is determined that the vehicle is not in a fall state (ST208: NO).

  Then, in ST209, it is determined whether or not the end condition is met. If the end condition is satisfied (ST209: YES), the end process is executed and the hoist control is ended. An example of the end condition is that the mobile robot 200 has reached the destination point and the mobile robot has ended its operation. On the other hand, if the termination condition is not satisfied, the control operation is continued by returning to ST200.

Here, in ST205, when an error signal is transmitted from the error signal transmitting unit 261 to the error signal receiving unit 521 (ST205: YES), the motor driving of the mobile robot 200 is servo-off, and the mobile robot 200 falls over. The risk is extremely high.
In this case, the error signal receiving unit 521 outputs that the pulling operation command unit 542 has an error signal. Upon receiving this error signal, the pulling operation command unit 542 commands the hoist 430 to pull up the mobile robot 200 (ST220). Then, the hoist 430 winds up the wire 440 and performs the lifting operation of the mobile robot 200 (ST221).
FIG. 7 is a diagram illustrating a state where the mobile robot 200 is pulled up.
When the lifting of the mobile robot 200 is completed, a termination process is performed, and the control of the hoist 430 is terminated (ST222).

In ST208, when it is determined that the attitude angle of mobile robot 200 exceeds the angle threshold value and mobile robot 200 is in a fall state (ST208: YES), fall determination unit 532 includes pull-up operation command unit 542. A signal that informs the fall direction determination unit 533 of the fall state is output. Then, the fall direction determination unit 533 determines in which direction the mobile robot 200 is to be tilted from the posture angle information (ST230).
For example, the falling direction of the mobile robot 200 is specified by specifying the direction in which the posture angle exceeds the threshold. The specified falling direction is output to the pulling operation command unit 542 (ST231).

The pulling operation command unit 542 winds the wire 440 and pulls up the mobile robot 200, and at the same time moves the hoist 430 in the designated falling direction (ST232). Then, the mobile robot 200 that is falling is smoothly pulled up in the vertical direction.
FIG. 8 is a diagram illustrating a state where the mobile robot 200 that is falling is being lifted.
When the pulling-up operation of the mobile robot 200 is completed, an end process is performed, and the control of the hoist 430 is ended (ST233).

According to this embodiment, the following effects can be achieved.
(1) Since position data (self position and path) generated inside the mobile robot 200 is sent to the hoist control unit 500 and drive control of the hoist 430 is performed based on these data, smooth automatic follow-up becomes possible.
(2) The follow-up position calculation unit calculates not only the current position of the mobile robot but also the follow-up position commanded to the hoist by taking into account the delay time until the hoist 430 actually operates. Since the delay time is compensated in this way, the mobile robot can follow very smoothly.

(3) The error signal transmission unit constantly monitors the motor drive error, and if an error has occurred, the mobile robot can be quickly lifted based on the error information.
If the motor is servo-off, the mobile robot will fall sharply, so there is a possibility that it will not be in time for the pull-up operation after recognizing a fall. However, according to this embodiment, fatal damage to the mobile robot Can be prevented.
(4) A fall direction determination unit is provided, and when the mobile robot falls, the fall direction is specified, and the hoist is moved in the fall direction when the mobile robot is pulled up.
As a result, the mobile robot that is falling can be pulled straight up smoothly without applying an extra load, and damage to the mobile robot can be prevented.

In addition, this invention is not limited to the said embodiment, The deformation | transformation in the range which can achieve the objective of this invention, improvement, etc. are included in this invention.
In the above embodiment, the communication means between the mobile robot and the hoist control unit has not been specifically described. However, such a communication means is not particularly limited, and may be wired communication or wireless communication. It is. Similarly, the communication means between the hoist control unit and the hoist is not particularly limited.
The mobile robot is not limited to the autonomous bipedal walking type as long as it is a legged type, but it is not limited to the bipedal walking type that has a high risk of falling and has a large damage when falling. The invention is highly effective.

The figure which shows the whole structure of 1st Embodiment which concerns on the fall prevention system of the mobile robot of this invention. The block diagram showing the structure of a mobile robot in the said 1st Embodiment. The block diagram which shows the structure of a hoist control part in the said 1st Embodiment. The figure which shows a mode that a hoist follows a mobile robot in the said 1st Embodiment. 4 is a flowchart showing an operation procedure of the mobile robot in the first embodiment. The flowchart which shows the operation | movement procedure of a hoist control part in the said 1st Embodiment. The figure which shows a mode that the mobile robot is pulled up in the said 1st Embodiment. The figure which shows a mode that the mobile robot which is falling is pulled up in the said 1st Embodiment.

Explanation of symbols

DESCRIPTION OF SYMBOLS 100 ... Fall prevention system, 200 ... Mobile robot, 210 ... Sensor, 220 ... Attitude angle detector, 230 ... Robot control part, 240 ... State data processing part, 241 ... Position data processing part, 242 ... Self-position estimation part, 243 ... path generation unit, 244 ... angle signal processing unit, 250 ... operation command generation unit, 260 ... drive control unit, 261 ... error signal transmission unit, 270 ... drive unit, 271 ... motor, 300 ... fall prevention device, 400 ... hoist Mechanism unit, 410 ... Y-axis rail, 420 ... X-axis rail, 430 ... hoist, 440 ... wire, 500 ... hoist control unit, 510 ... follow-up control unit, 511 ... robot position information acquisition unit, 512 ... path information acquisition unit, 513 ... Following position calculation unit, 520 ... Fall prevention control unit, 521 ... Error signal receiving unit, 530 ... Fall detection unit, 531 ... Attitude angle information Shin portion, 532 ... fall determining unit, 533 ... falling direction determining section, 540 ... operation command section, 541 ... tracking operation command section, 542 ... operation command section.

Claims (7)

A mobile robot that autonomously walks,
A wire connected to the mobile robot has a hoist provided so that the wire can be unwound and rolled up, and the device is provided with a tip-over prevention device that prevents the mobile robot from tipping over by pulling up the wire. A mobile robot overturn prevention system that moves following the mobile robot at
The mobile robot is
A sensor that acquires ambient environmental information;
A self-position estimation unit that creates a surrounding environmental map based on information acquired by the sensor and estimates a self-position;
A route generation unit that generates a route of movement using the environmental map generated by the self-position estimation unit,
The fall prevention device is
A robot position information acquisition unit that receives the position information of the mobile robot by receiving the self position estimation information of the mobile robot from the self position estimation unit;
A route information acquisition unit that acquires the route information generated by the route generation unit;
A follow-up position calculation unit that calculates a next movement position of the mobile robot based on the self-position estimation information and the route information;
A follow-up operation command unit that generates an operation command for causing the hoist to follow the mobile robot based on a next movement position of the mobile robot calculated by the follow-up position calculation unit; A system for preventing the fall of a mobile robot. In addition to calculating the next movement position of the mobile robot, the follow-up position calculation unit further gives an operation command to the hoist from the acquisition of the self-position estimation information and the route information, and the hoist is instructed. 2. The mobile robot overturn prevention system according to claim 1, wherein a follow-up position for causing the mobile robot to follow the hoist is calculated in consideration of a delay time until the mobile robot moves to another position. The mobile robot includes an error signal transmission unit that detects a control error of the drive motor and transmits an error signal;
The fall prevention device is
A pulling operation commanding unit for commanding the hoisting operation of the mobile robot;
An error signal receiving unit that receives an error signal from the error signal transmitting unit and transmits error information to the pulling operation command unit, and
The mobile robot overturn prevention system according to claim 1 or 2, wherein when the error information is received, the pull-up operation command unit instructs the hoist to perform a pull-up operation of the mobile robot. The fall prevention device is
A pulling operation commanding unit for commanding the hoisting operation of the mobile robot;
Determine whether or not the mobile robot is in a fall state, and if the mobile robot is in a fall state, determine the direction of the fall and output a signal to the pulling operation command unit according to the judgment result A detection unit;
The pulling operation command unit moves the hoist in the direction in which the mobile robot falls when instructing the hoist to perform a pulling operation of the mobile robot according to the signal from the fall detection unit. The fall prevention system of the mobile robot according to any one of claims 1 to 3. The mobile robot is
Attitude angle detection means for detecting the inclination angle of the mobile robot;
An angle signal processing unit for processing the signal from the posture angle detection means to obtain angle information,
The fall prevention device is
A pulling operation commanding unit for commanding the hoisting operation of the mobile robot;
An attitude angle information receiving unit that receives angle information from the angle signal processing unit;
An angle threshold for determining whether the mobile robot falls is set, and when the inclination angle of the mobile robot detected by the attitude angle detection means exceeds the angle threshold, the pulling operation command unit A fall determination unit that outputs a signal that the tilt angle of the mobile robot exceeds the angle threshold,
4. The mobile robot overturn according to claim 1, wherein the pull-up operation command unit instructs the hoist to perform a pull-up operation of the mobile robot according to the signal from the overturn determination unit. 5. Prevention system. The fall prevention device is
When the fall determination unit determines that the tilt angle of the mobile robot exceeds the angle threshold, the tilt direction of the mobile robot is determined based on the angle information received by the posture angle information reception unit. And an inclination direction determination unit that outputs the determined inclination direction to the pulling operation command unit,
6. The mobile robot overturn prevention system according to claim 5, wherein the pulling operation command unit moves the hoist in the determined inclination direction during the pulling operation of the mobile robot. A wire connected to the mobile robot has a hoist provided so that the wire can be unwound and rolled up. The hoist is moved substantially following the mobile robot following the mobile robot and lifted up the wire. A fall prevention device for preventing falls,
A robot position information acquisition unit for acquiring self-position estimation information generated internally in the mobile robot;
A route information acquisition unit for acquiring route information generated internally by the mobile robot;
A follow-up position calculation unit that calculates a next movement position of the mobile robot based on the self-position estimation information and the route information;
A follow-up operation command unit that generates an operation command for causing the hoist to follow the mobile robot based on a next movement position of the mobile robot calculated by the follow-up position calculation unit; A fall prevention device characterized by comprising.

Images