JP5802423B2 - Moving body - Google Patents

Description

  The present invention relates to a technique for providing a moving body that conveys a conveyed product.

  As a background art relating to a moving body that transports a transported object, Patent Document 1 states that “the restriction of the route on which an autonomous mobile body travels can be reduced and a route with the shortest distance can be set, and a plurality of autonomous mobile bodies travel simultaneously. There is a disclosure of “to prevent collisions and traffic jams from occurring”.

Patent Document 2 also states that “the direction estimation means estimates and calculates the direction of the moving object based on the output of the optical fiber gyro”. For the output signal from this optical fiber gyro, been the error of the azimuth orientation between the actual moving body is disclosed for very small that is "it.

JP 2005-242489 A JP 07-083673 A

  A robot that has been turned into an abnormal state due to an abnormal condition, such as a robot that is in an abnormal state, waits for the controller OS to start up, reads various data, Preparations such as operation check are necessary, and it takes a lot of time. In the present invention, the time required for these preparations is shortened, and it is an object to speed up the return of the mobile body (hereinafter referred to as not only the mobile body but also a robot) to a workable state. .

  In order to solve the above-mentioned problem, when the robot is restarted, the state of the controller or sensor is determined, and the controller or sensor that is not in an abnormal state is not restarted, thereby reducing the number of devices to be restarted. Try to reduce startup time.

  Here, the contents described above will be further described with different expressions.

  A distance sensor unit that measures the distance between the object and the moving body, a position and orientation estimation unit that obtains a signal from the distance sensor unit and estimates the position or orientation of the moving body, and the position and orientation estimation unit Obtaining a signal, generating a path for moving the moving body, and having a path following control unit that controls the moving body to move, and path information that the position and orientation estimation unit and the path following control unit refer to An external storage unit, a moving mechanism unit that moves the moving body, the distance sensor unit, the position and orientation estimation unit, the path tracking control unit, an external storage unit, a power supply unit that supplies power to the moving mechanism unit, A distance sensor unit, the position / orientation estimation unit, the path tracking control unit, an external storage unit, a moving mechanism unit, and a housing that supports a power supply unit, and the movement that moves under the control of the path tracking control unit In the body, said A first restart system for restarting the separation sensor unit and the position / orientation estimation unit and a second restart system for restarting the path following control unit are separately restarted, and the distance sensor unit And a first abnormality detection unit for detecting an abnormality of the position and orientation estimation unit, and when the first abnormality detection unit detects an abnormality, the distance sensor unit by a first restart system, When the restart processing of the position / orientation estimation unit is executed, a second abnormality detection unit that detects an abnormality of the path following control unit is provided, and when the second abnormality detection unit detects an abnormality, The restart processing of the path follow-up control unit is executed by the restart system of 2.

  Further, a calibration structure is provided on the casing in a range that can be detected from the distance sensor unit, and a preset value corresponding to the position of the distance sensor unit and the calibration structure, When the difference from the detection value obtained by detecting the position of the calibration structure by the distance sensor unit is larger than a predetermined value, an abnormality of the distance sensor unit is detected.

  Further, a calibration structure is provided on the casing in a range that can be detected from the distance sensor unit, and a preset value corresponding to the position of the distance sensor unit and the calibration structure, A correction value for correcting the value of the detection signal from the distance sensor unit is obtained from the difference from the detection value obtained by detecting the position of the calibration structure by the distance sensor unit.

  In addition, the moving body is provided with a timer capable of measuring a time elapsed by a count value that changes every predetermined time, and the external storage unit stores map data of a range in which the moving body moves, and the position The posture estimation unit includes a map reading unit that reads the map data, estimates the position of the moving body from the output value from the map reading unit and the detection value from the distance sensor unit, and from the distance sensor unit The detected value is stored in the external storage unit as distance sensor data corresponding to the time, and after the position / orientation estimation unit is restarted, the position / orientation estimation unit sets the time before restarting to the external storage unit. The difference between the time read from the external storage unit and the time corresponding to the count value obtained from the timer is compared with the count value obtained from the timer after restarting Is smaller than the range of the map reading unit reads from the map data to be less than the total range stored in the map data.

  The external storage unit stores map data of a range in which the moving body moves, and the position and orientation estimation unit includes a map reading unit that reads the map data, and an output value from the map reading unit The position of the moving body is estimated from the detection value from the distance sensor unit, the detection value from the distance sensor unit is stored in the external storage unit as distance sensor data, and after restarting the position and orientation estimation unit, The position and orientation estimation unit reads the distance sensor data before being restarted from the external storage unit, and compares the detected value from the distance sensor unit after the restart with the distance read from the external storage unit When the difference between the sensor data and the detected value from the distance sensor unit after restart is smaller than a predetermined value, the range that the map reading unit reads from the map data is recorded in the map data. It is to be less than the total range is.

The range read from the map data by the map reading unit is set to a range corresponding to a range that can be detected by the distance sensor unit.

According to the present invention, it is possible to provide a mobile body or a robot that shortens the time required for activation by reducing the number of devices to be reactivated than before .

Functional configuration diagram of an embodiment of the present invention Hardware and software configuration diagram of an embodiment of the present invention The figure which shows the external appearance of the robot of the Example of this invention. The figure which shows the operation | movement of the robot of the Example of this invention, and the example of laser distance sensor data The figure which shows the process in the robot at the time of automatic driving | running | working of the Example of this invention. The figure which shows the process in the robot at the time of abnormality detection of the Example of this invention The figure which shows the process at the time of starting of the position and orientation estimation controller of the Example of this invention The figure which shows the process at the time of starting of the path | route following control controller of the Example of this invention. The figure which shows the process in the preparation operation | movement of the robot of the Example of this invention. The figure which shows the process of the setting of the reading range of the map data of the Example of this invention

  Embodiments relating to the present invention will be described below with reference to the drawings. In the following description, what is described as a robot is an example of the above-described moving body.

  In the above description, information, data, signals, and the like that are output, input, received, transmitted, referenced, stored, and set between the units are expressed differently depending on the description. It is not limited by the wording. For example, even if it is described as `` value (setting value, detection value, correction value, output value, etc.) '', it is not limited only to data, but it also has the same meaning when treated and expressed as information or signal . The same applies to the following description.

  Here, we will first describe the configuration of the robot functions and the basic processing flow assumed in this example, and then work after the robot collides with an obstacle while moving and falls into an abnormal state. The processing performed by the robot will be described through an example of returning to a proper state.

The configuration of the robot function is shown in FIG. The robot 0101 includes a position / orientation estimation controller unit 0102 (corresponding to the above-described position / orientation estimation unit), a path following control controller unit 0103 (corresponding to the above-described path following control unit), and an external storage unit 0128. , Distance sensor unit 0104, distance sensor calibration jig unit 0130, abnormality detection sensor unit 0105, movement mechanism unit 0116, restart control unit 0117, power supply unit 0118 (corresponding to the power supply unit described above), and these It is comprised from the housing | casing (not shown) which supports.

Although not shown in FIG. 1, a timer is also included in the configuration, and is used, for example, as a reference when generating a control signal. Here, it is assumed that the timer has a function capable of measuring the time elapsed by a count value that changes at regular intervals. Note that this timer, or the power of part of the structure is cut features of the robot, be restarted without stopping, the count value of the timer is updated continuously, can time measurement Suppose there is.

First, the process performed at the time of automatic traveling after completion of the robot activation will be described. Robot 0101, by the distance sensor control unit 01 10, and controls the distance sensor unit 0104 capable distance measurement to the object in the environment, to obtain the distance data from the distance sensor to the object of the environment. This distance data is data to which the direction of the obstacle viewed from the sensor that has performed the measurement and the time at which the measurement was performed is added. It is memorize to.

Therefore, when it is stopped, turned off, or restarted due to an abnormality, etc., the distance sensor from the distance sensor immediately before it is stopped, turned off, or restarted to the environmental object distance data, the direction and time of performing the measurements of the obstacle seen from the sensor performing the measurement is to be memorize.

  It is assumed that the distance data obtained from the distance sensor unit 0104 includes the result of measurement by the distance sensor calibration jig unit 0130. The distance sensor calibration jig unit 0130 is an object whose distance, direction, and shape from the distance sensor unit 0104, and the posture of the jig itself are known and can be measured by the distance sensor, and are within the measurement range of the distance sensor unit 0104. It is installed in the robot casing. By using this measurement result, an abnormality of the distance sensor unit 0104 is detected by the following process.

In the position / orientation estimation device abnormality detection unit 0111, from the distance sensor unit 0104 recorded in the distance sensor calibration data 0123 read in advance when the position / orientation estimation controller unit 0102 is activated to the distance sensor calibration jig unit 0130. And the distance obtained by actually measuring the distance sensor calibration jig unit 0130 from the distance sensor unit 0104, and whether or not the magnitude of the distance difference falls within the threshold value The presence / absence judgment is performed.

  Here, the distance sensor calibration jig unit 0130 corresponds to the calibration structure described above.

  In addition, the position / orientation estimation device abnormality detection unit 0111 detects an abnormality even when an abnormality is detected in the processing of the position / orientation estimation controller initialization unit 0106 and the distance sensor device check unit 0107, and restarts. It shall be able to output the detection to the control unit.

  More specifically. Now, the distance from the distance sensor unit 0104 recorded in the distance sensor calibration data 0123 to the distance sensor calibration jig unit 0130 is referred to as a reference distance. This is because the distance from the distance sensor unit 0104 to the jig unit 0130 is obtained by another sensor with higher accuracy than the distance sensor unit 0104, or the arrangement of the distance sensor unit 0104 and the distance sensor calibration jig unit 0130 is determined. Assume the design value obtained when designing.

  It is assumed that distance data obtained by detection and measurement by the distance sensor unit 0104 can be associated with the reference distance. For example, when the distance sensor is calibrated, the distance sensor unit 0104 can detect the distance sensor calibration jig unit 0130 or the calibration structure and measure the distance and position. For example, the direction of the sensor unit 0104 may be changed or adjusted.

  However, simply, the initial position detected by the distance sensor may be a position where the distance sensor calibration jig unit 0130 is detected and the distance and position can be measured. More specifically, for example, a case where the distance sensor is installed in the housing by rotating the range of 180 degrees and the distance or the like is detected will be described.

  When the direction of 90 degrees within the rotation angle of the distance sensor is the forward direction of the casing, the direction of 0 degrees (the direction perpendicular to the forward direction) is on the casing. In addition, the distance sensor calibration jig unit 0130 or the calibration structure is provided. In this way, if the initial position detected by the distance sensor is 0 degree, the distance sensor calibration jig unit 0130 or the calibration structure can be detected and the position and distance can be measured. It becomes like this.

  Without being limited to the above, the reference distance may be associated with the information detected from the distance sensor unit 0104 by various methods.

  If the difference between the reference distance and the distance obtained by the actual measurement by the distance sensor unit 0104 is larger than the threshold value, the distance sensor unit 0104 has a failure because the difference between the assumed distance and the actually measured distance is large. Is detected as an abnormality.

  The reference distance is set to a preset value corresponding to the position of the distance sensor unit and the calibration structure.

  The distance obtained by actual measurement by the distance sensor unit 0104 is a detection value obtained by detecting the position of the distance sensor calibration jig unit 0130 (or the calibration structure) by the distance sensor unit 0104. .

  If the magnitude of the distance difference is within a predetermined value or threshold value, the distance sensor unit 0104 is assumed to be operating normally because the expected distance can be measured, and the process is performed. Proceed next. At this time, the distance sensor calibration jig unit 0130 is measured by the distance sensor unit 0104 from the distance from the distance sensor unit 0104 to the distance sensor calibration jig unit 0130 recorded in the distance sensor calibration data 0123. A value obtained by subtracting the distance is passed to the next process together with the distance data as a correction value. Also, the abnormality detection result of the distance sensor unit 0104 here is output to the restart control unit 0117 and stored.

  Subsequently, the distance sensor data correction corresponding position / orientation estimation unit 0112 performs position / orientation calculation processing in the environment of the robot 0101. Here, first, the above-mentioned correction is regarded as a distance measurement error of the distance sensor unit 0104, and this measurement error is added to the distance data to correct the distance data and at the same time obtain the laser irradiation direction and measurement. By obtaining the position where the laser hits the object in each direction from the obtained distance information, the distance data is converted into the geometric data of the environment. Subsequently, the map reading unit 0113 performs matching with the map data 0125 in which the geometric shape of the environment read in advance is recorded.

  For example, when the distance sensor unit 0104 is a laser distance sensor that scans a plane parallel to the floor surface, the ratio of the geometric shape data of the environment on the scan plane and the map data based on the image that records the geometric shape of the environment overlap. The position / posture of the geometric shape data on the map data when the maximum value is obtained, that is, the position / posture of the laser distance sensor is obtained.

  The position / orientation of the distance sensor unit 0104 in the map data 0125 can be obtained. However, when representing the position / orientation of the robot, any position of the robot housing may be used as a reference, and therefore the robot can be determined using the obtained position / orientation of the laser distance sensor. The position / posture is 0101. In addition, time is added to the position / posture estimation result, and the time / position estimation data log 0124 is recorded.

  The route planning unit 0114 reads route graph data 0126 in which the route that the robot can pass in the environment and how the partial route is connected and the attributes of this route, for example, the length and width of the partial route are recorded. The route to the destination is obtained based on the estimated position / orientation obtained by the distance sensor data correction corresponding position / orientation estimation unit 0112.

  Then, the movement mechanism control unit 0115 obtains how much the position / orientation of the robot is deviated from the path the robot is trying to follow, and controls the movement mechanism unit 0116 to reduce this deviation, that is, the wheel. The rotation speed and steering angle of the steering wheel are obtained, and instructions to the motor are issued at any time so that the robot body follows the path.

  An abnormality is detected by a position / orientation estimation device abnormality detection unit 0111 (corresponding to the first abnormality detection unit described above) and an abnormality detection sensor unit 0105 (corresponding to the second abnormality detection unit described above). While not detected, the robot automatically travels to the destination according to the above processing.

  In the following, the flow of processing until the robot that is running automatically detects an abnormality, stops, restarts, etc., and returns to a running state will be described. Assume that an abnormality is detected by the abnormality detection sensor unit 0105 of the robot during automatic traveling. Here, as an example, a situation is considered in which a bumper sensor that detects contact with an obstacle is provided as the abnormality detection sensor unit 0105, thereby detecting a collision of the robot with the obstacle.

Detection result of the presence or absence of abnormality by the abnormality detection sensor unit 0105 are transmitted to the transfer motive configuration control unit 0115 when the anomaly is detected, the moving mechanism from the transfer motive structure controller 0115 in order to suppress the damage of an accident by the robot A stop signal is issued to the unit 0116. Moving mechanism control section 0115 also, on the other hand distance sensor unit 0104 and the position and orientation of the abnormality presence or absence of estimation controller unit 0102 detection result is sent to the position-and-orientation estimation device abnormality detecting unit 0111, wherein the abnormality detection Select the device to restart according to the result.

  More specifically, if an abnormality of the distance sensor unit 0104 is detected by the position / orientation estimation device abnormality detection unit 0111 and an abnormality is also detected by the abnormality detection sensor unit 0105, the restart instruction line 0120 (described above) To the second restart system) and restart instruction line 0121 (corresponding to the first restart system described above), the restart control unit 0117 instructs the restart. The control signal to be output is output and restarted.

  In addition, when the abnormality of the distance sensor unit 0104 is not detected and the abnormality is detected only by the abnormality detection sensor unit 0105, the restart instruction line 0120 in which the distance sensor unit 0104 and the position / orientation estimation controller unit 0102 are connected. A control signal for instructing restart is not output, and a control signal for instructing restart only for the restart instruction line 0121 connected to the path following control controller 0103 is output.

Also, if the abnormality only by the position and orientation estimation controller unit 0102 has been detected, the distance sensor unit 0104 and the position and orientation estimation controller unit 0102 is a control signal for an instruction to restart the restart indication line 0120 that is connected Is output only.

Although not shown in FIG. 1, a detection unit that detects an abnormality of the path tracking control controller unit 0103 (which corresponds to the above-described path tracking control unit) is provided separately from the abnormality detection sensor unit 0105. When an abnormality is detected, the detection is output to the restart control unit 0117 after detection, and a control signal instructing restart is output from the restart control unit 0117, and the path tracking control controller unit 0103 is restarted. Activation may be performed.

However, re-activation of the path tracking controller unit 0103, the detection of the abnormality of the detection unit for detecting an abnormality of the path tracking controller unit 0103 (assumed to correspond to the second abnormality detection section above) Then, it may be restarted so as to correspond to both of the detection of the abnormality from the abnormality detection sensor unit 0105.

  As described above, unnecessary stop / restart of the distance sensor unit 0104 and the position / orientation estimation controller unit 0102 that are operating normally is avoided, and the time until the robot returns to a state where it can perform normal operations such as automatic running is shortened. Is planned.

  For example, it is assumed that a laser distance sensor is used for the distance sensor unit 0104. In this case, a mechanism for rotating the laser irradiator for scanning is generally provided, and it may take time for the rotation to stabilize, which may cause a slow return to the normal state in which the robot can work.

  In addition, in order to suppress measurement errors due to temperature changes of components such as chips and transistors in the sensor, waiting time until the temperature change settles, communication check with the inside of the laser distance sensor and controller, and laser irradiation unit cover A waiting time such as a dirt check is also a factor for delaying the return.

  In addition to the distance sensor unit 0104, if the position / orientation estimation controller unit 0102 is equipped with an OS, the startup time, various programs, and data reading time are required, which also causes the return of the robot to be delayed. When there is no abnormality in the distance sensor unit 0104, as described above, the distance sensor unit 0104 and the position / orientation estimation controller unit 0102 are not stopped / restarted, so The time is reduced, and as a result, it is possible to shorten the time from when an abnormality occurs in the robot until it returns to a state where normal work can be performed.

  Hereinafter, the flow of processing when the position / orientation estimation device abnormality detection unit 0111 detects no abnormality of the distance sensor unit 0104 and when it is detected will be described in order.

  If no abnormality is detected in the abnormality detection result of the distance sensor unit 0104 recorded in the restart control unit 0117, a restart instruction for connecting the distance sensor unit 0104 and the position / orientation estimation controller unit 0102 is performed. A control signal for instructing restart is not output to the line 0121, but a control signal for instructing restart to the restart instruction line 0120 connected to the path following control controller unit 0103 is output.

In the path following control controller unit 0103, first, the path following control controller initialization unit 0108 reads the BIOS and starts up the OS, and each part of functions used in the path following control controller during automatic driving, specifically, the path The planning unit 0114 and the movement mechanism control unit 0115 are activated. Subsequently, the movement mechanism device check unit 0109 performs communication check with the movement mechanism unit 0116, transmission of a brake signal, and the like. Each of the path following control controller initialization unit 0108 and the movement mechanism device check unit 0109 confirms whether there is an abnormality, and the confirmation result is recorded in the movement mechanism control unit 0115.

  In the activated route planning unit 0114, route graph data in which the route that the robot can pass in the environment and how the partial route is connected and the attributes of this route, for example, the length and width of the partial route are recorded. The vehicle body shape information for checking whether the robot can pass or not with respect to the road width, or the movement mechanism control unit 0116 is recorded with the movement mechanism specifications and settings such as the maximum movement speed and the maximum rotation angular velocity. The mechanism calibration data 0127 is read, and these data are transmitted to the moving mechanism control unit 0115.

Since the distance sensor unit 0104 and the position / orientation estimation controller unit 0102 continue to operate, the return to the state in which the robot can perform normal work is completed.

If an abnormality is detected in the abnormality detection result of the distance sensor unit 0104 recorded in the restart control unit 0117, the distance sensor unit 0104 and the position / orientation estimation controller unit 0102 are stopped and restarted. In the position / orientation estimation controller unit 0102, first, the position / orientation estimation controller initialization unit 0106 reads the BIOS and starts up the OS, and each part of functions used in the position / orientation estimation controller during automatic driving, specifically distance The sensor control unit 0110, the position / orientation estimation device abnormality detection unit 0111, the distance sensor data correction corresponding position / orientation estimation unit 0112, and the map reading unit 0113 are activated.

  Subsequently, the distance sensor device check unit 0107 performs a communication check with the distance sensor unit 0104, a contamination check of the laser irradiation unit cover, and the like. Each of the position / orientation estimation controller initialization unit 0106 and the distance sensor device check unit 0107 confirms whether there is an abnormality, and the confirmation result is recorded in the position / orientation estimation device abnormality detection unit 0111.

  In the activated position / orientation estimation device abnormality detection unit 0111, the distance from the distance sensor unit 0104 to the distance sensor calibration jig unit 0130 recorded in the distance sensor calibration data 0123, that is, the reference distance described above. The data is read.

  Also, the map reading unit 0113 reads the position / posture and time data at which an abnormality is detected and time from the position / posture estimation data log 0124 with time, finds the difference from the current time, and determines the magnitude of the time difference. Is within the threshold value, the robot has detected an abnormality and stopped, and it has not passed until it starts, so it is unlikely that the robot has moved, and it still stops at the previously estimated position / posture The map corresponding to the measurement range of the distance sensor unit 0104 is read from the map data 0125 with the position / posture as the center. Thereby, the reading range of the map data 0125 is narrowed, and the waiting time for reading the map data 0125 is shortened.

  As described above, the activation of the distance sensor unit 0104 and the position / orientation estimation controller unit 0102 is completed, and the return to a state in which the robot can perform normal work is completed with the activation of the path following control controller activated in parallel. Become.

  In the above description, the embodiment has been described in which the time data is read from the timed position / orientation estimation data log 0124. However, the present invention is not limited to this.

  For example, the above-described timer may be used to obtain the time when the operation stopped abnormally or the timer counter value as described above.

  The above is the outline of the processing performed for each function constituting the robot. Next, through the example of returning to a workable state after falling into an abnormal state where the robot collided with an obstacle while moving, more specific hardware configuration and software This will be explained based on the above process.

FIG. 2 shows the configuration of the hardware of the robot 0101 and the software stored therein. The robot includes an external storage device 0208, a laser distance sensor 0209, a bumper sensor 0210, a restart control controller 0211, a moving mechanism 0212, a display 0213, and an input device 0214 in addition to the position and orientation estimation controller unit 0102 and the path following control controller unit 0103 described above. The communication line 0204 is used for communication between these devices. In FIG. 2, only communication lines related to the processing flow are basically shown, and the power supply shown in FIG. 1 is omitted.

  Here, a laser distance sensor 0209 is used as the distance sensor unit 0104. This sensor has a laser irradiation unit that obtains the distance from the sensor to the object by measuring the time from when the laser is irradiated to the object until the irradiated laser is reflected by the object and returns to the sensor, Furthermore, while rotating this laser irradiation unit, the distance from the sensor to the object is measured for each direction that forms a certain rotation angle, thereby scanning a certain angle range and the geometry of the object in the plane forming the scan plane. It is a sensor that measures the shape.

  In this embodiment, it is assumed that the angle range to be scanned is 180 degrees, and the laser is irradiated every 0.5 degrees in this angle range and the distance to the object is measured, but the angle range to be scanned and the laser are irradiated. The step size of the angle and the maximum measurement range of the distance may be different. FIG. 3 shows how an object in the environment is measured by a laser distance sensor 0301 mounted on the robot 0310.

  The figure shows a state where the body of the robot corresponding to the robot 0101 is looked down from above (a plan view). In this figure, the laser 0304 scans an angle range 0302 of 180 degrees, and in this range, for example, a distance 0305 to the object 0306 in the environment is set as the distance from the viewpoint 0303 of the laser irradiation unit of the sensor to the object in the environment. It shall be measured.

  In the present embodiment, the sensor as described above is assumed. However, the sensor method may be different as long as the sensor can measure the geometric shape of the object. For example, a stereo camera or a depth camera that can measure the distance to an object for each pixel by irradiating the object with infrared rays in a plane may be used.

  As the moving mechanism unit 0116, a robot 0310 shown in FIG. 3 is assumed. The robot 0310 is provided with a caster 0311 at the front and a driving wheel 0312 at the rear, and can be moved straight by turning a difference in rotational angular velocity of the driving wheel. .

  However, other moving mechanisms such as a vehicle having an endless track, a robot having legs, a ship, an aircraft, and an airship may be used as long as the effect of moving in the environment can be obtained. Further, in this embodiment, an example in which the robot automatically travels is shown, but the robot may be operated by a person boarding or by remote communication.

The abnormality detection sensor unit 0105 here assumed emergency stop button. In addition to detecting the pressing of the emergency stop button, the abnormality detection sensor unit 0105 detects a collision, a bumper sensor detects a collision, a current sensor detects an overcurrent, a distance sensor detects an obstacle around the robot, a moving mechanism Various abnormalities can be detected, such as detecting slips by comparing encoder data and acceleration sensor data, and detecting car body shakes and inclinations using tilt sensors. However, abnormalities can be defined and detected according to the operation of the robot. As long as it is a sensor, the method, the position / number of installation, etc. may be different.

The restart control part 0117 where although the restart controller 0211, here receives the emergency stop button by down abnormality detection result or the position and orientation estimation device abnormality detection program 0219 abnormality detection result due, it Assume a microcomputer equipped with a program for controlling the output of a signal that prompts the device to restart, and having a dedicated power source in addition to the power source for driving the robot, controller, and sensor.

In addition, the position / orientation estimation controller unit 0102 performs communication check with the processor 0201, the memory 0202, the storage device 0203, the OS 0215, the position / orientation estimation controller initialization program 0216 for reading the BIOS and starting the OS, and the laser distance sensor 0209. Laser distance sensor device check program 0217, laser distance sensor control program 0218 for acquiring distance data from the laser distance sensor 0209, position / orientation estimation device abnormality detection program 0219, and matching of laser distance sensor data and map data for position / orientation It comprises a calculated laser distance sensor data correction corresponding position and orientation estimation program 0220 and a map reading program 0221 for setting a map reading range.

Similarly, the path tracking controller unit 0103, a processor 0205, a memory 0206, storage device 0207, OS0222, path tracking controller initialization program 0223 is used to start the BIOS read or OS, Ya communication check with the mobile Organization 0 212 A moving mechanism device check program 0224 for transmitting a brake signal, a route planning program 0225 for calculating a route to reach the destination based on the route graph data 0230, and a vehicle wheel so that the vehicle body moves along the route. It comprises a moving mechanism control program 0226 for calculating the rotation speed and the like.

  In addition, the external storage device 0208 has a time-distance sensor data log 0228 in which the map data 0227 used for matching by the laser distance sensor data correction corresponding position / orientation estimation program 0220 and the distance data obtained from the laser distance sensor 0209 are recorded together with the time. The position / orientation estimation data log 0229 with time, in which the position / orientation estimation result of the robot obtained by the laser distance sensor data correction corresponding position / orientation estimation program 0220 is recorded together with the time, and the length and width of the path that the robot can pass in the environment. Recorded path graph data 0230, laser distance sensor calibration data 0231 recorded with a reference distance that is a distance from the laser distance sensor 0301 to the laser distance sensor calibration jig 0309, maximum moving speed / rotational angular speed Moving mechanism calibration data 0232 to set the specifications and the like standard moving speed movement mechanism is recorded is recorded.

  Here, it is assumed that the laser distance sensor calibration jig 0309 corresponds to the distance sensor calibration jig unit 0130 (or the calibration structure) described in FIG.

It is assumed that the program and data shown in FIG. 2 are loaded into the memory and then processed by the processor. However, if the program and data are equivalent to this, an FPGA (Field Programmable Grid Array), You may implement | achieve with programmable hardware, such as CPLD (Complex Programmable Logic Device).

Further, the program and data may be transferred from a storage medium such as a CD-ROM, or may be downloaded from another device via a network. In addition, although it is assumed here that the devices constituting the robot 0101 such as a processor, a storage device, and a moving mechanism communicate with each other via a wired communication line 0204, they may be wireless and can communicate with each other. In this case, the position / orientation estimation controller unit 0102, the path following control controller unit 0103, the external storage device 0208, the display 0213, and the input device 0214 may be physically remote. The above hardware and software may be selected according to the embodiment.

  Next, the flow of processing when the robot has been activated and performing normal operation will be described, and then the flow of processing when an abnormality is detected and restarted will be described.

  First, the flow of processing when the robot has been started and normal operation is being performed, more specifically, measurement is performed by the distance sensor unit 0104 in FIG. A flow from the device abnormality detection unit 0111, the map reading unit 0113, the distance sensor data correction corresponding position / orientation estimation unit 0112, and the route planning unit 0114 until the vehicle mechanism is controlled by the moving mechanism control unit 0115 will be described.

Now, the robot is activated, and the position / orientation estimation controller unit 0102 and the path following control controller unit 0103 have completed preparatory operations at the time of activation such as reading of the BIOS and OS, and the automatic traveling program unit (distance sensor control unit 0110, Laser distance sensor control program 0218 as a program group corresponding to a position / orientation estimation device abnormality detection unit 0111, a distance sensor data correction compatible position / orientation estimation unit 0112, a map reading unit 1013, a route planning unit 0114, and a movement mechanism control unit 0115). It is assumed that the position / orientation estimation device abnormality detection program 0219, the laser distance sensor data correction corresponding position / orientation estimation program 0220, the map reading program 0221, the route planning program 0225, and the moving mechanism control program 0226) are activated.

That is, in the position / orientation estimation controller unit 0102, the OS0215 is read by the position / orientation estimation controller initialization program 0216, the communication check with the laser distance sensor 0209 is performed by the laser distance sensor device check program 0217, and then the laser distance sensor Laser distance sensor control program 0218 for capturing measurement results, position / orientation estimation device abnormality detection program 0219 for detecting an abnormality of a device such as a controller or sensor, and laser distance sensor data correction corresponding position / orientation estimation program 0220 for estimating the position / orientation of a robot , in a state where the map reading program 0221 to set the map loads range is activated, also the path tracking controller unit 0103, path following controller initialization The OS 0222 is read by the program 0223, the communication check with the moving mechanism 0212 is performed by the moving mechanism device check program 0224, and then the route planning program 0225 for obtaining the route to the destination and the moving mechanism control for obtaining the rotational speed of the wheel, etc. Assume that the program 0226 is activated. Up to this point, preparations for the robot to perform a normal operation are completed, and from this point, the robot operates according to the process flow of FIG.

When the programs of the position / orientation estimation controller unit 0102 and the path following control controller unit 0103 are activated as described above and the preparation operation is completed (0501), a confirmation screen asking whether or not to finish setting the destination is displayed on the display 0213. Is displayed.

  The operator selects the setting to be performed by the input device 0214 when moving the robot to the robot by carrying a package or the like (0502), and if it is not necessary to move, selects the end and immediately terminates the program. (0503). If it is assumed that the destination is set, the process proceeds to process 0504.

Here, the operator selects a destination from the map displayed on the display 0213 and the list of transport destination candidates, and sets the destination using the input device 0214 (0505). Here, it is assumed that the destination of the robot 0406 is set in a state where the map data 0401 of the environment is displayed as shown in FIG.

When the movement start position of the robot and 0402, the moving path 0404 for the purpose Cima determined from the position 0402 (0505). This assumes that the shortest route search is performed using the route graph data 0230 read when the route tracking control controller unit 0103 is activated.

  As a method of searching for the shortest route, there is a method of searching by the Dijkstra method based on the distance of partial routes in the route graph and connection information between the partial routes, but the method may be different. Further, although it is assumed here that a route having the shortest distance is obtained as a reference for route search, other criteria such as obtaining a route having the longest distance may be used.

Then obtain the distance data by the laser distance sensor 0209 by the laser distance sensor control program 02 18. For example robot Ri by scanning range by the laser distance sensor 0407 when to have been moved to the position of FIG. 4, the distance data is obtained (0506).

  The obtained distance data is sent to the position / orientation estimation device abnormality detection program 0219, where abnormality detection of the laser distance sensor 0209 is performed (0507). This abnormality detection is performed using a laser distance sensor calibration jig 0309 attached to the scan range of the laser distance sensor 0301.

  Now, the distance from the viewpoint 0303 of the laser irradiation unit to the laser distance sensor calibration jig 0309 is recorded in the laser distance sensor calibration data 0231, and which data of the distance data is the laser distance sensor calibration jig. It is assumed that the data for measuring 0309, that is, the direction of the laser distance sensor calibration jig 0309 is also recorded in the laser distance sensor calibration data 0231.

  Further, it is assumed that the position / orientation estimation device abnormality detection program 0219 reads this data at startup. Using this, the difference between the measured distance to the jig 0309 sent from the laser distance sensor control program 0218 and the reference distance recorded in the laser distance sensor calibration data 0231 is obtained, and the magnitude of this difference. Is within the threshold value, the laser distance sensor is considered to be operating normally, and if the threshold value is exceeded, it is considered that an abnormality has occurred.

Detection results of the abnormality is sent to the restart controller 0211, it is memorize in the controller. A value obtained by subtracting the distance obtained by actually measuring the jig 0309 from the distance recorded in the laser distance sensor calibration data 0231 here is sent to the next process together with the distance data as a sensor data correction value. Note that the jig 0309, in FIG. 3, are installed only one left sensor may be a plurality of installation.

  Subsequently, the laser distance sensor data is first corrected by the laser distance sensor data correction corresponding position / orientation estimation program 0220 (0508). Here, the measurement error of the laser distance sensor 0209 obtained by the measurement of the jig 0309 is corrected by adding the sensor data correction value sent from the previous process to the distance value in each direction included in the distance data. . Also, as described above, the distance data is converted into two-dimensional geometric shape data 0403 and passed to the next processing.

Subsequently, matching is performed between the obtained geometric shape data 0403 and map data 0401 (0227 in FIG. 2) (0509). Here superimposed map data 0401 expressing the same image shape and geometry data 040 3 expressing imagewise, the pixels representing the presence of an obstacle in the geometry data of the presence of obstacles on the map data 0401 The position / posture of the geometric shape data on the map data when the ratio of overlapping with the pixel to be represented becomes the maximum, that is, the position / posture of the laser distance sensor is obtained.

  Here, the position / orientation of the laser distance sensor is the position / orientation of the robot. In addition to this, among the points forming the sensor data and the map data, the matching is performed by associating the points with the shortest distance between the points so that the sum of the distances between the points becomes small, that is, The use of a method called ICP (Iterative Closest Point), which finds the relative position and orientation between sensor data and map data when the geometrical characteristics of sensor data and map data most overlap, can be considered. Any method can be used if it can be obtained.

  Next, it is determined whether or not the robot has arrived at the destination (0510). This is performed by determining whether or not the position obtained by the above-described position and orientation estimation is within a certain distance from the coordinates of the destination.

  If the robot is still arriving at the destination, the route planning program 0225 calculates the rotation speed of the wheel for moving the vehicle body to the route obtained by the route plan based on the current position / posture. (0512).

  Subsequently, the moving mechanism control program 0226 issues a current value instruction to the motor of the moving mechanism 0212 so that the rotational speed of the wheel becomes a predetermined value (0511).

  If it is determined that the robot has moved and arrived at the destination (0510), the process returns to the destination setting confirmation process 0502. The above is the flow of processing when the robot is performing normal operation.

  Next, the flow of processing when an abnormality is detected and restarted will be described. When the robot starts normal operation, as described above, abnormality detection is performed by the position / orientation estimation device abnormality detection program 0219 and the result is transmitted to the restart control controller 0211. Based on this result and the state of the emergency stop button for abnormality detection connected separately, the restart control controller 0211 performs processing in accordance with the flow of FIG.

  Specifically, when the program in the restart control controller 0211 is started (0601), after confirming the stop of the controller (0602), first, acquisition of push-down data of the emergency stop button (0603) and position / posture estimation Acquisition (0604) of abnormality detection data sent from the device abnormality detection program 0219 is performed.

Next, it is determined whether the restart of each device in accordance with the state of the state and the emergency stop button of the laser distance sensor 0209. First, when an abnormality in both the emergency stop button and the laser distance sensor 0209 is detected (0605), more specifically, there are significant errors in the measurement by the laser distance sensor 0209, and an emergency stop button If the depression of the article is confirmed, except the restart controller, the position and orientation estimation and both controllers path following control, to restart the laser distance sensor 020 9 (0606).

If an error only in any of the laser distance sensor 0209 or the emergency stop button is detected, how restart varies depending respectively. First, when an abnormality is detected only by the laser distance sensor 0209 (0609), since an abnormality has occurred only in the laser distance sensor, the laser distance sensor 0209 and the position / orientation estimation controller unit 0102 that controls the abnormality are re-established. A control signal for instructing activation is output, and only these are restarted (0607).

Also very when the stop abnormality in only button is detected (0611), a control signal for an instruction to restart the position and orientation estimation controller unit 0102 and the laser distance sensor 0209 which operates normally is not output Then, a control signal for instructing only the path following control controller unit 0103 to be restarted is output (0610).

  Here, no sensor is connected to the path following control controller unit 0103. However, when a device such as a sensor or actuator that takes time to start is further connected to the path tracking control controller unit 0103, the movement mechanism control unit 0115 determines the state. These devices may be monitored, and those devices may be restarted in accordance with the restart of the path following control controller unit 0103.

As described above, it takes time for the laser distance sensor 0209 and the position / orientation estimation controller unit 0102 to be operable, but as described above, by avoiding restart of a device that is operating normally, Reduce the time it takes for the robot to be able to operate normally.

FIG. 7 shows the processing flow of the restarted position / orientation estimation controller unit 0102. When the position / orientation estimation controller unit 0102 is activated (0701), as initialization operation (0702) by the position / orientation estimation controller initialization program 0216, BIOS reading (0705), OS reading (0706), OS and this An abnormality of the related service software is confirmed (0707).

  Note that the abnormality confirmation result is temporarily recorded in the memory. Subsequently, as an automatic running program in the position and orientation controller, a laser distance sensor control program 0218, a position and orientation estimation device abnormality detection program 0219, a laser distance sensor data correction corresponding position and orientation estimation program 0220, and a map reading program 0221 are started. (0708).

Subsequently, the communication check with the laser distance sensor 0209 connected to the position / orientation estimation controller unit 0102 is performed by the laser distance sensor device check program 0217 (0709), and the check result by the laser distance sensor itself is acquired. (0710). This is to receive the check result of the cover contamination of the laser irradiation unit performed by the laser distance sensor itself.

  The abnormality detection result at the initialization of the controller (0702) already recorded in the memory and the abnormality detection result in the processing 0709 and 0710 are sent together to the position / orientation estimation device abnormality detection program (0711). The process ends (0704).

FIG. 8 shows the flow of processing of the route follow-up control controller unit 0103 restarted in the same manner as the position / orientation estimation controller unit 0102. When the path following control controller is activated (0801), as the initial operation (0802) by the path following control controller initialization program 0223, BIOS reading (0805), OS reading (0806), OS and related services A software error is confirmed.

Note that the abnormality confirmation result is temporarily recorded in the memory. Subsequently, the route planning program 0225 and the moving mechanism control program 0226 are started as the automatic running program in the route following control controller (0808). The movement mechanism device check program 0224 performs a communication check with the movement mechanism 0212 connected to the path following control controller unit 0103 (080 9 ).

  Also, the check result by the moving mechanism itself is acquired (0810). This is for receiving a brake operation check result and the like performed by the moving mechanism 0212 itself.

  The abnormality detection result at the initialization of the controller (0802) already recorded in the memory and the abnormality detection result at the processing 0809 and 0810 are sent together to the moving mechanism control program (0811), and the program is terminated ( 0804).

  Subsequently, preparations are made by the started automatic running program group. The flow of processing related to this preparation is shown in FIG.

  As a preparatory operation for the automatic running program group, route graph data 0230 is first read.

  Subsequently, the map data 0227 is read. The processing flow at this time is shown in FIG. When the map reading program 0221 is started (1001), first, distance data, that is, distance data after restarting the robot is acquired from the laser distance sensor 0209 (1002). Subsequently, the latest distance data is acquired from the laser distance sensor data log with time 0228 before the robot is restarted (1003).

  Next, the difference between the time of the distance data after restarting the robot and the time of the distance data before restarting is obtained (1004).

Next, matching is performed between the distance data after restarting the robot and the distance data before restarting (1005 −1 ). This is similar to the matching process performed by the laser distance sensor data correction corresponding position / orientation estimation program 0220. In the laser distance sensor data correction-compatible position / orientation estimation program 0220, the ratio of the geometric data overlapped with the map data 0227 is calculated. Here, the distance data before and after the restart is converted into the geometric data. Above, the ratio which these overlap is calculated | required (1005-2) .

The map data reading range is set according to the time difference obtained so far and the matching ratio. First, it is the difference in time is less than or equal to the threshold, by that time until the robot is restarted is stopped is short, or the matching ratio of geometric data after a reboot and restart the previous geometry data When it is determined that the robot is not moving because the surrounding environment as seen from the laser distance sensor 0 209 is not changed (1006), the operation is stopped from the timed position / orientation estimation data log 0229. The position / orientation estimation result recorded immediately before is acquired (1007).

In this position / posture, the maximum range that can be measured by the laser distance sensor 0209 is set as a read range, and the corresponding range is read from the map data 0227. In this way, the reading range of the map data 0227 is narrowed down to the vicinity of the robot stop position, and the time required for reading is shortened. In process 1006, if the size KuKatsu matching ratio from the difference between the threshold time is smaller than the threshold value read map data 0227 for the entire movement of the robot is assumed, after completion reading the map data 0227, the map reading program 0213 Auto Since the map data 0227 is read in accordance with the movement of the robot during traveling, the vehicle is in a standby state until the start of automatic traveling.

  When the reading of the map data 0227 is completed, the distance data is acquired by the laser distance sensor control program 0218 (0904).

  Subsequently, the initial position / posture, that is, the first position / posture after the robot is started is estimated using the distance data. This process is the same as the process 0509 related to position / posture estimation. Thus, the preparation operation at the time of starting the robot is completed (0906), and the robot returns to a state where the normal operation shown in FIG. 5 is possible.

  As described above, when an abnormality such as a collision with an obstacle is detected during operation of the moving object or robot, the moving object or robot is stopped immediately and immediately after the obstacle is removed. In addition, it is conceivable to restart the moving body or the robot.

  As described in the above embodiment, when this moving body or robot is restarted, the controller or sensor that is not in an abnormal state is not restarted, and the device in the abnormal state is restarted. By making it start, the mobile body or the robot can be quickly returned to work.

0101 ... Robot, 0102 ... Position and orientation estimation controller unit, 0103 ... Path following control controller unit, 0104 ... Distance sensor unit, 0105 ... Abnormality detection sensor unit, 0106 ... Position and orientation estimation controller initialization unit, 0107 ... Distance sensor device check unit , 0108... Path initialization control controller initialization unit, 0109... Movement mechanism device check unit, 0110... Distance sensor control unit, 0111... Position / orientation estimation device abnormality detection unit, 0112. ... Map reading unit, 0114 ... Route planning unit, 0115 ... Move mechanism control unit, 0116 ... Move mechanism unit, 0117 ... Restart control unit, 0118 ... Power supply unit, 0119 ... Communication during automatic driving, 0120 ... Route follow-up control controller Restart instruction line to the unit, 0121 ... Reactivation instruction line to remote sensor unit and position / orientation estimation controller unit, 0122 ... distance sensor data log with time, 0123 ... distance sensor calibration data, 0124 ... position / orientation estimation data log with time, 0125 ... map data, 0126 ... Route graph data, 0127 ... Movement mechanism calibration data, 0128 ... External storage unit, 0129 ... Communication during preparation operation, 0130 ... Distance sensor calibration jig unit

Claims (6)

A distance sensor for measuring the distance between the object and the moving body;
A position and orientation estimation unit that obtains a signal from the distance sensor unit and estimates the position or orientation of the moving body;
A path follow-up control unit that obtains a signal from the position and orientation estimation unit, generates a path for moving the moving body, and controls the moving body to move;
An external storage unit having path information referred to by the position and orientation estimation unit and the path tracking control unit;
A moving mechanism for moving the moving body ;
A power supply unit that supplies power to the distance sensor unit, the position and orientation estimation unit, the path tracking control unit, an external storage unit, and a moving mechanism unit;
A housing that supports the distance sensor unit, the position and orientation estimation unit, the path tracking control unit, an external storage unit, a moving mechanism unit, and a power supply unit;
In a moving body that moves under the control of the path following control unit,
A first restart system for restarting the distance sensor unit and the position and orientation estimation unit;
A second restart system that restarts the path following control unit is restarted separately;
A first abnormality detection unit that detects an abnormality of the distance sensor unit and the position and orientation estimation unit;
When the first abnormality detection unit detects an abnormality, a restart process of the distance sensor unit and the position and orientation estimation unit is executed by a first restart system,
A second abnormality detection unit for detecting an abnormality of the path following control unit;
When the second abnormality detection unit detects an abnormality, a restart process of the path following control unit is executed by the second restart system. The moving body according to claim 1,
A calibration structure is provided on the casing in a range where detection from the distance sensor unit can be performed,
It is larger than the predetermined value the difference between the detected value detected positions of the calibration structure by the setting value set in advance corresponding to the position of the distance sensor unit and said calibration structure before Symbol distance sensor unit In this case, the moving object, wherein the first abnormality detection unit detects an abnormality of the distance sensor unit. The mobile body according to claim 2 ,
The difference between the detected value detected positions of the calibration structure by the setting value set in advance corresponding to the position of the distance sensor unit and said calibration structure before Symbol distance sensor unit, the position and orientation estimation The unit obtains a correction value for correcting the value of the detection signal from the distance sensor unit. The moving body according to claim 1,
The moving body is provided with a timer capable of measuring the elapsed time by a count value that changes every fixed time,
The external storage unit stores map data of a range in which the moving body moves,
The position and orientation estimation unit
A map reading unit for reading the map data;
From the output value from the map reading unit and the detection value from the distance sensor unit, the position of the moving body is estimated,
The detected value from the distance sensor unit is stored in the external storage unit as distance sensor data corresponding to the time,
After restarting the position and orientation estimation unit,
The position / orientation estimation unit reads the time before restarting from the external storage unit, compares it with the count value obtained from the timer after restarting, the time read from the external storage unit, and the timer If the difference from the time corresponding to the count value obtained from is smaller than a predetermined value,
A moving body in which a range read from the map data by the map reading unit is smaller than an entire range stored in the map data. The moving body according to claim 1,
The external storage unit stores map data of a range in which the moving body moves,
The position and orientation estimation unit
A map reading unit for reading the map data;
From the output value from the map reading unit and the detection value from the distance sensor unit, the position of the moving body is estimated,
Detection values from the distance sensor unit are stored in the external storage unit as distance sensor data,
After restarting the position and orientation estimation unit,
The position / orientation estimation unit reads distance sensor data before restarting from the external storage unit, and compares the detected value from the distance sensor unit after restarting with the distance sensor read from the external storage unit When the difference between the data and the detected value from the distance sensor unit after restart is smaller than a predetermined value,
A moving body in which a range read from the map data by the map reading unit is smaller than an entire range stored in the map data. The moving body according to claim 4 or 5 ,
A moving body characterized in that a range read by the map reading unit from the map data is a range corresponding to a range that can be detected by the distance sensor unit.

Images