JP5276931B2 - Method for recovering from moving object and position estimation error state of moving object - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To construct a mobile object capable of recovering to a state in which a position prediction is enabled by itself when run into the state where the position prediction error occurs. <P>SOLUTION: Using an environmental map and moving information of a vehicle body, sensor data, to be captured when actually measured by the sensor, is predicted, by comparing the sensor data by this prediction and the sensor data by the actual measurement, when the position prediction error is determined to have occurred, turning on the spot is executed, by predicting the position using the sensor data and the map captured in the state where the sensor is not shielded after the turning, the position prediction is recovered from the error state. <P>COPYRIGHT: (C)2010,JPO&amp;INPIT

Description

  The present invention relates to a moving body that returns to a state in which position estimation can be performed by itself when it falls into a position estimation error state and a method for returning from the position estimation error state.

  In Patent Document 1, when a position estimation error occurs, an RFID tag installed in the environment is read, and an approximate position is recognized using information relating the ID and position of the RFID tag registered in advance. Furthermore, a robot that returns to a state in which position estimation is possible from a position estimation error state by recognizing surrounding landmarks with a stereo camera has been proposed.

JP 2007-249735 A

  The method of Patent Document 1 requires a large number of RFID tags installed in the environment in advance and the cost of measuring their coordinates.

  An object of the present invention is to provide an apparatus and a method capable of returning a mobile object from a position estimation error state to a state in which position estimation can be performed without installing an RFID tag.

  In the present invention, when a position estimation error is detected by a mobile body, the mobile body is swung on the spot, and the environment using the sensor is measured after the turn. By performing the position estimation using, the mobile body is returned from the position estimation error state to a position estimation possible state.

  The present invention turns on the spot when a position estimation error is detected.

  In the present invention, when it is determined that there is an error in position estimation, the distance from the sensor to an obstacle existing in the surrounding environment is calculated using the estimated position and the map information before the position estimation error occurs. Turn on the spot in the direction of increasing.

  The present invention estimates the position and orientation of the current moving body from the position and posture on the map of the moving body estimated at the previous timing when the matching processing result is not within a predetermined range, and measures this time Assuming that the results were obtained from the position and orientation of the moving body this time, the difference between the measurement results and the map is detected to detect areas that do not match, and clustering based on the distance between the areas of the areas that do not match Process to detect the posture of the largest cluster among the obtained clusters as a posture that is not included in the sensing range of the sensor, and out of the postures that are not included in the sensing range of the sensor, Based on a simulation of sensor data obtained when the vehicle turns, sensor data with a large distance from the moving object to the obstacle present in the environment is obtained. Posture detected as posture of the moving body after pivoting of when.

  According to the present invention, it is possible to return a mobile object in a position estimation error state to a position capable of position estimation without previously installing in the environment a device that assists recovery from a position estimation error, such as an RFID tag. It becomes possible.

  Here, an embodiment of a moving body that returns to a state in which position estimation can be performed by itself even if a position estimation error occurs due to interruption of measurement by the sensor and performs autonomous movement will be described.

  The structure of the moving body used in this embodiment is shown in FIG. The moving body 1001 includes a processor 1002, a memory 1003, a keyboard 1004, a video card 1005, a display 1006, a communication line 1007, a laser distance sensor 1008, a moving mechanism 1009, an encoder 1010, a camera 1011, and a storage device 1012. In this embodiment, the moving mechanism is assumed to be a moving mechanism that includes two drive wheels and casters, and that allows the mobile body to turn on the spot by controlling the rotational angular velocity and direction of rotation of the drive wheels. However, as long as the same effect can be obtained, the form of the moving mechanism such as an endless track, a leg, a ship, an aircraft, and various robots is not limited. In this embodiment, as a sensor for measuring the geometric shape of the environment, the distance to the obstacle is calculated by measuring the time when the irradiated laser light is reflected and returned from the obstacle while scanning the laser in the horizontal direction. Suppose a laser distance sensor that measures the geometric shape of an object in a two-dimensional horizontal plane. However, as other sensors, for example, a laser distance sensor that measures a three-dimensional geometric shape of an object by scanning other than in the horizontal direction, and a stereo camera that measures the three-dimensional geometric shape of an object by triangulation of image features Alternatively, the form of the sensor is not limited as long as the geometric shape can be measured, such as an ultrasonic sensor capable of measuring the geometric shape. In this embodiment, it is assumed that each component is connected by a wired communication line, but may be wireless as long as communication is possible. In addition, only specific elements may be physically remote among the components of the moving body in the present embodiment. The sensing range by the sensor may be a limited angle (for example, 180 °) instead of 360 ° around the moving body.

  The storage device 1012 includes an OS 1013, an overall control unit 1014, a laser distance sensor value acquisition unit 1015 that acquires geometric data using a laser distance sensor 1008, and an encoder that acquires wheel rotation data by an encoder 1010 attached to the moving mechanism. A value acquisition unit 1016, a moving mechanism control unit 1017 that performs wheel rotation control for path following and turning operations, and estimates the position and orientation (orientation) of a moving body that reaches the next time from the rotation speed of the wheel at a certain time. The estimated arrival position estimation unit 1018, the current position estimation unit 1019 that obtains the position and orientation of the moving object using the geometric data by the laser distance sensor 1008, and the route to the destination by using the current position / orientation and map. Route planning unit 1020 to calculate, sensor is obstructed by people etc. and position estimation error occurs A position estimation error determination unit 1021 for determining whether there is a map, a map update unit 1023 for updating a map of the environment using the position estimation result, and a display for displaying an image of a map or a position of a moving object on the display 1006 via a video card 1005 A part 1024 and map data 1025 used for position estimation are stored. In the present embodiment, it is assumed that the processor 1002 loads the program in the storage device 1012 into the memory 1003 and executes it as a process. However, if the processor 1002 performs an equivalent function, an FPGA (Field Programmable) is used. You may implement | achieve by programmable hardware, such as Grid Array) and CPLD (Complex Programmable Logic Device). Further, the above-described 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. The above hardware and software may be selected according to the embodiment.

  It is assumed that the moving body configured as described above autonomously moves in the environment. FIG. 2 is a diagram illustrating an environment in which a moving body moves and a moving route thereof. The moving body 2009 uses the laser distance sensor 2010 to measure obstacles 2001 such as peripheral shelves and walls, perform position estimation, and autonomously move from the initial position 2009 to the destination 2002. During this movement, if the human 2003 that is present in the environment interrupts the measurement of the moving object by the laser distance sensor 2010 and an error occurs in the position estimation, it returns to a state in which position estimation is possible, and autonomous movement FIG. 3 shows a series of processes performed by the moving body 2009 when continuing.

  When a program is started by an input from the keyboard 1004 (3001), first, map data 1024 is read by an overall control unit 1014 that supervises processing of the entire program (3002). The map data 1024 represents the geometric shape of the environment as indicated by reference numeral 4001 in FIG.

  Subsequently, the laser distance sensor control unit 1015 acquires the laser distance sensor data at the initial position 2011 (3003). In the laser distance sensor data, coordinates of a point where the laser spot light hits an obstacle when the laser is scanned are recorded. For example, if the laser scanning 2014 is performed at the position 2004, the coordinate value in the sensor coordinate system of the point group 2013 reflected by the spot light is obtained.

  The moving body 2009 estimates its own initial position 2011 by matching the laser distance sensor data at the initial position 2011 with the map data 4001 (3004). Matching here refers to calculating the position and orientation at which the geometric features of the laser distance sensor data and the map data 4001 best overlap. In this embodiment, it is assumed that ICP (Iterative Closest Point) is used as a matching method, but any matching method can be used as long as the same effect can be obtained. By this matching, the position / posture of the moving body 2009 at the initial position 2011 is calculated. This calculation of the position and orientation of the moving body is hereinafter simply referred to as position estimation. In addition, although the two-dimensional position estimation is assumed as the position estimation in the present embodiment, the three-dimensional position estimation is performed using data from a sensor capable of three-dimensional measurement, and the position estimation value is obtained. It may be used as

  Subsequently, a route plan to the destination 2002 is performed using the obtained initial position 2011 and map data 4001 (3005). At this time, it is assumed that the human 2003 is not in the environment and the path 2008 is obtained.

  In order to move along the path 2008, the moving body 2009 moves by moving the wheel by the moving mechanism control unit 1015 (3006). Based on the control information of the moving mechanism at this time, the moving body estimates the position and posture that the moving body will reach when the laser distance sensor data is acquired next time (3007). Hereinafter, the position and posture of the moving body that are scheduled to reach are combined and simply referred to as the scheduled position of arrival.

  Next, sensor data obtained when the moving body performs measurement by the laser distance sensor at the estimated arrival position from the estimated arrival position and map data 4001 are calculated (3008).

  Next, measurement by the laser distance sensor 2010 is performed at the position where the moving body 2009 has moved and actually reached (3009). At this time, odometry data based on measurement by the encoder 1010 attached to the moving mechanism is also acquired simultaneously with the laser distance sensor data. Odometry is a method of calculating the translation speed of the moving body and the angular velocity of the moving body from the rotational speeds of the left and right wheels in a moving body or robot equipped with a wheel moving mechanism, and integrating them to determine the position and orientation of the robot. is there. The odometry here is not the absolute position / posture from the initial value of the moving body, but the amount of movement (relative position / posture) from the previous position estimation using the laser distance sensor data. Shall.

  Next, it is determined whether or not a position estimation error has occurred due to the mobile body sensor being shielded by a person or the like (3012). This determination is performed by matching the laser distance sensor data at the planned arrival position calculated by the process 3008 with the laser distance sensor data acquired at the actual arrival position by the process 3009. The matching method is not limited, but here it is assumed that matching is performed using the above-described ICP. By matching using ICP, the relative position and orientation of the sensor data and the matching evaluation value when the geometric features overlap most are obtained. Here, the matching evaluation value is a value representing the degree of overlapping of geometric features between sensor data. Here, the sum of the distances between points in the nearest point group defined by ICP is used as the matching evaluation value, and the smaller the evaluation value, the better the matching. Now, when there is sensor data 5002 obtained by actually measuring the environment with the moving body at the position 2004 while the sensor is shielded by the human 2003 and the sensor data 5001 predicted by the processing 3009, matching using ICP By 5003, a matching result 5004 is obtained. By this matching, a matching evaluation value is obtained as an amount indicating how much the sensor data does not overlap. If this matching evaluation value is smaller than a preset threshold value (within a range where there is no position estimation error), it is determined that no position estimation error has occurred, and the process proceeds to position estimation processing 3013, which is larger than the threshold value. If it is outside the range where there is no position estimation error, it is determined that a position estimation error has occurred, and the process proceeds to a return operation 3018 (3011). When it is larger than the threshold value, the return operation 3018 may be performed after the movement of the moving body is stopped.

  First, the position estimation 3012 performed when it is determined in the process 3011 that the matching evaluation value is smaller than the threshold will be described. Here, as with the initial position estimation 3004, matching between the laser distance sensor data and the map data 4001 is performed, and the position / orientation of the moving object is obtained (3013). Further, the laser distance sensor data is coordinate-converted to the position / orientation of the moving body, that is, the map is updated by converting the sensor coordinate system data into the map coordinate system and writing it into the map data 4001 (3014). The updated map, the position of the moving object, the planned route, the destination, and the like are generated as images by the display unit 1024 and displayed on the display 1009 via the video card 1008. Subsequently, it is determined whether the mobile object has arrived at the destination from the position of the mobile object (3015). If it is determined that the mobile object has arrived, the program ends (3016). A route plan from the current position to the destination is performed (3005), and the movement is repeated until the destination is reached.

  Next, when it is determined in processing 3011 that the matching evaluation value is larger than the threshold value, that is, when it is determined that there is an error in position estimation, this is performed to return the mobile body to a state in which position estimation is possible. Processing 3017 will be described. Processing 3017 is performed by the return operation planning unit 1022. Hereinafter, the series of processes will be described. If the matching evaluation value is larger than the threshold value, it is determined here that a position estimation error has occurred because the sensor is shielded by an obstacle that is not on the map, such as a person, and the position can be estimated from this position estimation error state. To return to the correct state. For this reason, first, by adding the relative movement amount based on the odometry data in the processing 3009 to the estimated position using the laser distance sensor data at the previous timing (predetermined time) in the periodic timing, the approximate amount based on the odometry data is obtained. Is estimated (3018). Subsequently, based on the estimated position / orientation, the measured laser distance sensor data is converted into a map coordinate system, and the difference is obtained by subtracting the overlapping area with the map from the laser distance sensor data (that is, sensor data). And the map and the map are identified) (3019). The difference may be expressed by coordinates of a position where the two do not match. Subsequently, clustering based on the distance between the regions is performed on the region remaining due to the difference between the laser distance sensor data and the map (that is, the region where the sensor data does not match the map) (3020). As the clustering method, any method such as a self-organizing map or a K-average may be used. Among the obtained clusters, the cluster having the maximum distribution, that is, the turning and turning at the current position obtained in the processing 3018 so that the region where the person who is shielding the sensor is supposed to exist does not enter the sensing range. Simulation of laser distance sensor data that is sometimes obtained. Using the sensor data obtained by simulation and the sensor data obtained for each attitude obtained by simulation, the sensor data when the average value of the distance from the sensor to the obstacle is maximum is obtained, and the attitude of the transport vehicle 2009 corresponding to this is obtained. 2005 is determined. A turning direction 2006 and a turning angle with a smaller turning angle when turning to the posture 2005 are obtained from the posture of the transport vehicle obtained in the processing 3018 (3021), and the vehicle body is set to be the turning angle and the turning direction. Is turned on the spot (3022).

  Note that the calculation of the turning direction with the smaller turning angle is not essential. That is, the turning direction such as clockwise may be determined in advance. In addition, it is preferable that the area where humans who are shielding the sensor are present does not enter the sensing range, but the matching evaluation is performed even if the area where humans are present is within the sensing range. There may be any posture whose value is smaller than the threshold value.

  Note that the turning of the vehicle body here may be any of super turning, believing turning, turning such that the vehicle body draws a circle or an arc, and the like. After turning, the sensor is expected to be out of the state where it was obstructed by humans. Therefore, the laser distance sensor data is obtained again, the position is estimated using that data, and the position estimation error state Return from. If it is possible to recover from the position estimation error, the process returns to a series of processes for moving to the destination.

  In the above embodiment, the laser distance sensor 2010 of the moving body 2009 detects that the sensor is shielded. However, as a sensor other than the laser distance sensor, for example, the sensor is detected from an image acquired by the camera 1011. You may detect having been shielded.

  In the above embodiment, it is determined whether or not to turn on the spot by determining the position estimation error. However, the mobile body whose position is completely unknown can be turned on by turning off the power. For example, when it is possible to drive the sensor and the vehicle body, the initial position estimation can be performed based on the data obtained by turning the superstrate regardless of whether the position estimation is correct or incorrect. Good.

  Further, in the above embodiment, turning was performed on the spot only when it was determined as a position estimation error, but as a result of turning at a plurality of points in the environment regardless of the presence or absence of a position estimation error, By using the swivel to improve the matching accuracy of the sensor data sets and the accompanying map creation accuracy by apparently expanding the sensor measurement range as one set of sensor data group obtained at the time of turning Also good.

  According to this embodiment, even if position estimation becomes difficult due to being surrounded by a person or the like, it is possible to realize a mobile body that returns to a state where position estimation is possible and continues autonomous movement. In addition, the movement that the moving body performs to return to a state in which position estimation is possible is only turning on the spot and does not involve movement, so it is possible to safely estimate the position without collision with people or objects There is an effect that it is possible to return to a proper state.

The figure which shows the structure of an apparatus. The figure which shows the mode of the environment and autonomous movement. The figure which shows the flow of a process. The figure which shows map data. The figure which shows the matching of the sensor data by prediction, and the sensor data by actual measurement.

Explanation of symbols

  DESCRIPTION OF SYMBOLS 1001 ... Mobile body, 1002 ... Processor, 1003 ... Memory, 1004 ... Keyboard, 1005 ... Video card, 1006 ... Display, 1007 ... Communication line, 1008 ... Laser distance sensor, 1009 ... Moving mechanism, 1010 ... Encoder, 1011 ... Camera, DESCRIPTION OF SYMBOLS 1012 ... Memory | storage device, 1013 ... OS, 1014 ... Overall control part, 1015 ... Laser distance sensor value acquisition part, 1016 ... Encoder value acquisition part, 1017 ... Movement mechanism control part, 1018 ... Expected arrival position estimation part, 1019 ... Current position Estimating unit, 1020 ... route planning unit, 1021 ... position estimation error determining unit, 1022 ... return operation planning unit, 1023 ... map updating unit, 1024 ... display unit, 1025 ... map data

Claims (2)

The environment around the moving object is periodically measured, and the position and orientation of the moving object on the map are estimated by comparing the measurement result with a map stored in advance. Based on the estimated result, the destination In a moving body that moves to
A processing unit for performing a matching process between the measurement result and the map;
When the matching processing result is within a predetermined range, the position and orientation of the moving body on the map are estimated based on the matched map, and the matching processing result is not within the predetermined range In addition, the position and posture of the moving body is estimated from the position and posture of the moving body on the map estimated at the previous timing, and the current measurement result is the position and posture of the moving body. As a result, the difference between the measurement result and the map is detected to detect a region that does not match, and the clustering process is performed based on the distance between the regions that do not match. Are detected as postures that are not included in the sensing range of the sensor, and the current movement of the postures that are not included in the sensing range of the sensor is detected. The moving body after turning the posture when sensor data having a large distance from the moving body to the obstacle existing in the environment is obtained based on the simulation of the sensor data obtained when turning at the position and posture A processing unit that detects the posture of
And a processing unit that instructs the mobile body to turn to the detected posture of the mobile body. The environment around the moving object is periodically measured, and the position and orientation of the moving object on the map are estimated by comparing the measurement result with a map stored in advance. Based on the estimated result, the destination In the recovery method from the position estimation error state of the moving body moving to
Perform a matching process between the measurement result and the map,
When the result of the matching process is within a predetermined range, the position and orientation of the moving body on the map are estimated based on the matched map,
When the matching processing result is not within a predetermined range, the position and posture of the moving body is estimated from the position and posture on the map of the moving body estimated at the previous timing, and the current measurement result Is obtained from the position and orientation of the moving body this time, by taking the difference between the measurement result and the map, a region that does not match is detected, and clustering is performed based on the distance between the regions that do not match Processing, detecting the posture of the largest cluster among the obtained clusters as a posture not included in the sensing range of the sensor, and among the postures not included in the sensing range of the sensor, the position and posture of the current moving body The distance from the moving body to the obstacle present in the environment is large based on the simulation of sensor data obtained when turning at It detected as posture of the moving body posture after turning when the sensor data is obtained,
A method for returning from a position estimation error state of a moving body, wherein the moving body is instructed to turn to the detected posture of the moving body.

Images