JP2008076252A - Own position recognition system - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To perform accurate own position recognition with reduced operation loads in a complicated moving environment using wall information for the own position recognition in an own position recognition system. <P>SOLUTION: The system comprises a map information storage means 2, an own position storage means 3, a laser radar 4 which senses an obstacle, and an operation means 5 which operates and processes the obtained obstacle information and map information. The operation means 5 acquires the obstacle information (first distance data) based on the map information to be sensed using the laser radar 4 at a currently recognized position, and actual obstacle information (second distance data) by the laser radar 4, forms first and second angle histograms by determining occurrence frequency of the angle of a line segment connecting data points for each of the distance data, and determines the correction angle in the rotation direction, based on a cross correlation function of the two angle histograms. It also extracts two angles in the first angle histogram, defines two translational axes, and determines the correlation distance in the translational direction. <P>COPYRIGHT: (C)2008,JPO&INPIT

Description

  The present invention relates to a self-position recognition system used in an autonomous mobile vehicle or the like.

  Conventionally, autonomous moving bodies such as vehicles and robots have been developed and put into practical use. In order for an autonomous mobile body to move autonomously, it is important to detect an obstacle, avoid it, and recognize its own position. Since the map information can be referred to in advance by recognizing the self position, it is easy to detect an obstacle. The recognition of the self position requires both recognition of the posture (orientation) of the self and recognition of the position coordinates of the current location. For such self-position recognition, for example, in a vehicle, a so-called dead in which a moving distance and a moving direction are determined using internal data such as the rotation speed of a wheel and a steering angle obtained from an encoder attached to a motor of a driving wheel. Many reckoning methods are used.

  When the above-described encoder information is used, errors accumulate and increase due to the occurrence of wheel slip and the like, so that position correction by map matching is usually combined with dead reckoning. In map matching, for example, a storage medium storing the input map information and a horizontal laser radar are provided, and the position recognized by dead reckoning is determined based on the difference between the map information and the obstacle position information acquired by the laser radar. to correct.

  The laser radar acquires distance data to the object surface by reflected light. Obstacle position information acquired by laser radar includes the position information of the object surface related to the measurement. In addition to moving obstacles other than the self in the moving space, the position of the objects that make up the environment such as walls and pillars Contains information. The map information includes information that can be compared with the position information. In an environment where people are present, unexpected obstacles are often placed, and there are often obstacles that are not included in the input map information.

  In order to perform the above-described map matching efficiently and accurately, it is conceivable to use a surface position such as a plane wall where the measurement data of the distance data is a straight line. When an autonomous mobile body passes through a corridor or the like while traveling in a corridor indoors, walls orthogonal to each other exist as environmental information. When a wide horizontal visual field range is scanned horizontally by the laser radar, orthogonal measurement point sequences arranged in a straight line in the scan plane are obtained. Under such circumstances, by efficiently extracting and using the characteristics of the data point sequence using the histogram, the direction of the point sequence arranged in a straight line is detected, and the arrangement of the point sequence is determined. A method of matching with the arrangement of walls in map information is known (for example, see Patent Document 1).

In addition, an example in which the map matching method using the above-described histogram is applied to a steering assist system for an electric wheelchair is known (see, for example, Patent Document 2).
“A map Based on laserscans without geometric interpretation” by Weiss and Puttkamer, Intelligent Autonomous Systems 4, 403-407 1995 Takeshi Goto "Electric wheelchair steering assist system using laser range finder", Nara Institute of Science and Technology, Graduate School of Information Science, Master's thesis, NAIST-IS-MT0051038, 2002

  However, in the map matching as shown in Patent Documents 1 and 2 described above, it is assumed that the walls are orthogonal, and there is no mention of utilizing the information of walls that are not orthogonal. In an actual environment where an autonomous mobile body moves, there are many environments where the walls are not orthogonal, so that such non-orthogonal wall information can be used effectively for map matching to enable position correction in self-position recognition. This is important for accurate self-localization in complex mobile environments.

  The present invention solves the above-described problem, and can perform position correction using non-orthogonal wall information for self-position recognition, and realizes accurate self-position recognition with reduced calculation load in a complicated moving environment. An object of the present invention is to provide a self-position recognition system that can be used.

  In order to achieve the above object, the invention of claim 1 includes a map information storage means for storing pre-input map information and a self-position memory for storing the position of the autonomous mobile body recognized on the map information. Means, a laser radar for sensing an obstacle existing around the autonomous mobile body, the obstacle information obtained by the laser radar, and the surroundings at the self position currently stored in the self position storage means A self-position recognition system comprising: a calculation means for correcting the self-position by calculating and processing the map information stored in the map information storage means; wherein the calculation means is the laser at a position recognized as a current position. Obstacle information (referred to as first distance data) based on the map information that should be sensed using a radar, and actual information sensed by the laser radar. Obstacle information (referred to as second distance data) is obtained, and for each of the first distance data and the second distance data, the angle of the line segment connecting the data points is obtained, and the obtained angle Autonomous mobile object that is currently recognized based on the result of calculating the cross-correlation function of the first and second angle histograms by calculating the appearance frequency and forming the first angle histogram and the second angle histogram To obtain a new angle data obtained by rotating the first distance data by the correction angle, and obtaining either the first angle histogram or the second angle histogram. In FIG. 2, two angles having high appearance frequency (these are called the first angle and the second angle) are extracted, and the new first distance data is extracted from the second data. An axis in a direction orthogonal to the direction of the first angle (referred to as the first translation axis) and an axis in a direction orthogonal to the direction of the second angle (referred to as the second translation axis) so as to overlap the separation data The position of the autonomous mobile body currently recognized by the translation movement along the first and second translation axis directions is corrected.

  According to a second aspect of the present invention, in the self-position recognition system according to the first aspect, the calculation means stores data on the first translation axis for each of the new first distance data and the second distance data. Projecting points to form a first position histogram and a second position histogram consisting of the frequency of appearance of data points with respect to the first translation axis, and projecting data points onto the second translation axis Forming a first position histogram and a second position histogram composed of the frequency of appearance of data points with respect to the translation axis, calculating a cross-correlation function of the first and second position histograms with respect to the first translation axis, And obtaining distance data for correcting the position of the autonomous mobile body currently recognized based on the first translation axis direction, and the first and second positions relative to the second translation axis It is to recognize its own position by obtaining the distance data for correcting the position of the second translational axis of the autonomous moving body that are currently known on the basis of the calculated cross-correlation function of the histogram results.

  According to a third aspect of the present invention, in the self-position recognition system according to the first or second aspect, the direction of the first or second angle is parallel to one of coordinate axes of a coordinate system used for self-position recognition. In addition, both the new first distance data and the second distance data are rotated, and the corrected distances in the first and second translational axis directions are obtained using the rotated distance data. .

  According to a fourth aspect of the present invention, in the self-position recognition system according to any one of the first to third aspects, the first and second angles are extracted from the first angle histogram.

  According to a fifth aspect of the present invention, in the self-position recognition system according to any one of the first to fourth aspects, when the difference between the first and second angles is close to 90 degrees, the two angles One of the angles is an angle in a direction orthogonal to the direction of the other angle.

  According to the first aspect of the present invention, two angles having a high frequency of appearance are extracted from the angle histogram, and the distance data is translated along the axis in the direction orthogonal to the direction of these angles, and is currently recognized. Since the position of each autonomous moving body in the direction of each translation axis is corrected, obstacle information (distance data) including information on two walls that are not orthogonal but have different orientations can be obtained even in a complicated moving environment where there is no information on orthogonal walls. It is possible to recognize the self position with high accuracy.

  According to the second aspect of the present invention, a histogram and a cross-correlation function are obtained for distance data including information of two walls facing in different general directions without assuming that the two walls are orthogonal to each other. By performing a numerical process using, self-position recognition can be performed with high accuracy.

  According to the invention of claim 3, since the direction of one translation axis coincides with the direction of the coordinate axis of the coordinate system used for self-position recognition, calculation for forming a position histogram becomes easy, and for self-position recognition. The calculation load can be reduced.

  According to the invention of claim 4, since the first angle histogram is based on map information that does not include a moving obstacle that becomes noise data for map matching, map matching is performed in two directions not included in the map information. Can be avoided, and accurate self-position recognition can be performed even in a complicated moving environment.

  According to the fifth aspect of the present invention, the two walls facing different directions with an angular relationship close to 90 degrees are considered to be arranged orthogonal to each other. Accordingly, two orthogonal directions can be used as translation directions, and errors in determining the angle can be eliminated, and self-position recognition with high accuracy can be performed.

  Hereinafter, a self-position recognition system according to an embodiment of the present invention will be described with reference to the drawings.

(First embodiment)
FIG. 1 shows a block configuration of the self-position recognition system, FIG. 2 shows a flowchart of self-position recognition processing in the system, and FIGS. 3 to 7 show the contents of each process. First, the outline of the self-position recognition system 1 will be described with reference to FIGS.

  As shown in FIG. 1, the self-position recognition system 1 includes a map information storage means 2 for storing pre-input map information and a self-position storage means for storing the position of the autonomous mobile body recognized on the map information. 3, a laser radar 4 that senses obstacles around the autonomous mobile body, obstacle information obtained by the laser radar 4, and information on the surroundings at the self-location stored in the self-position storage means 3. Computation means 5 for recognizing its own position by computing the map information stored in the map information storage means 2 is provided.

  As shown in FIG. 2, the calculation means 5 recognizes obstacle information (first distance data) that should be sensed using the laser radar 4 at the position recognized as the current position and stored in the self-position storage means 3. A) is acquired based on the map information stored in the map information storage means 2, and actual obstacle information (referred to as second distance data P) is acquired based on the result sensed by the laser radar 4 (S1). FIG. 3).

  Subsequently to the above, the computing means 5 obtains the angle of the line segment connecting the data points for each of the first distance data A and the second distance data P, and obtains the appearance frequency of the obtained angle to obtain the first frequency. The second angle histogram and the second angle histogram are formed (S2, see FIG. 4).

  Subsequent to the above, the calculation means 5 calculates the cross-correlation function of the first and second angle histograms and obtains a correction angle Δθ for correcting the rotational direction of the autonomous mobile body currently recognized based on the result ( S3) Further, new first distance data B obtained by rotating the first distance data A by the correction angle Δθ is acquired (S4, see FIG. 5).

  Subsequent to the above, the calculating means 5 extracts two angles of the first angle and the second angle, which appear frequently in either the first angle histogram or the second angle histogram (S5). In this case, normally, the first and second angles are extracted from the first angle histogram. This is because the first distance data A, and hence the first angle histogram, is based on map information that does not include moving obstacles that are noise for map matching. As a result, it is possible to avoid the occurrence of erroneous recognition that map matching is performed in two directions not included in the map information. Therefore, accurate self-position recognition can be performed even in a complicated moving environment.

  Note that although the above-described step S5 has been described as being performed after step S4, these two steps have no restriction on the processing order. Therefore, the processing order may be changed and the processing in step S4 may be performed after the processing in step S5.

  Subsequent to the above, the calculation means 5 sets each of the new first distance data B and second distance data P to an axis (referred to as a first translation axis u) in a direction orthogonal to the direction of the first angle. Projecting the data points to form a first position histogram and a second position histogram consisting of the frequency of appearance of the data points with respect to the first translation axis u, and an axis in the direction orthogonal to the direction of the second angle (first The data points are projected onto the second translation axis v) to form a first position histogram and a second position histogram that are composed of the frequency of appearance of the data points with respect to the second translation axis v (see S6, FIG. 6).

  Subsequent to the above, the calculation means 5 calculates the cross-correlation function of the first and second position histograms with respect to the first translation axis u, and based on the result, the first translation axis of the autonomous mobile body currently recognized. The first correction distance Δu for correcting the position in the direction is obtained (S7), and the cross-correlation function of the first and second position histograms with respect to the second translation axis v is calculated and is currently recognized based on the result. A second correction distance Δv for correcting the position of the autonomous moving body in the second translational axis direction is obtained (S8).

  As described above, the calculation means 5 of the self-position recognition system 1 currently recognizes based on the correction angle Δθ, the first correction distance Δu, and the second correction distance Δv obtained by map matching using a histogram. The rotation direction, that is, the orientation (or orientation) of the autonomous mobile body and the coordinates of the current position can be corrected, and therefore the self position can be recognized. The direction is corrected by the correction angle Δθ, and the position coordinates are corrected by obtaining Δx and Δy in FIGS. 5 and 6 from Δu and Δv (see FIG. 7 and described later).

  Details will be described below. FIG. 3 shows an arrangement example of the first distance data A, the second distance data P, and the laser radar 4 processed by the self-position recognition system 1. State the prerequisites. The laser radar 4 is provided in an autonomous mobile body such as an autonomous mobile vehicle and moves. A global coordinate system is defined as a coordinate system that defines a position in a two-dimensional region where an autonomous mobile body operates, and map information is described based on the global coordinate system. Usually, the autonomous mobile body includes a plurality of laser radars, but here, the laser radar 4 itself is an autonomous mobile body, and the self-position recognition of the laser radar 4 is self-position recognition of the autonomous mobile body. The laser radar 4 has an xy coordinate system (local coordinate system) that has a y-axis in front of the traveling direction and an x-axis to the right and moves with the laser radar 4. Determining the orientation and position of the xy coordinate system relative to the global coordinate system is self-position recognition.

  The laser radar 4 scans a visual field range in a front horizontal plane with a laser beam at a certain angle, for example, every 3 degrees, and acquires distance data of an object surface position within a predetermined distance range. The position recognized by the computing means 5 as the current position is assumed to be the position of the laser radar 4 shown in FIG. 3, and at this position, a virtual measurement that should be sensed using the laser radar 4 is performed. It is assumed that the points are data points a1 to a12 (first distance data A). These data points a1 to a12 are points on the obstacle stored in the map information storage means 2 having two walls w1 and w2. In the xy coordinate system, the angle is measured from the x-axis direction to the y-axis direction. The wall w1 is in the direction of angle α1 (the direction of arrow s), and the wall w2 is in the direction of angle α2 (the direction of arrow t).

  In FIG. 3, the measurement points on the obstacle having the two walls w1 and w2 actually measured by the laser radar 4 are shown as data points p1 to p11 (second distance data P). . In the xy coordinate system, the measured wall w1 is in the direction of the angle φ1, and the wall w2 is in the direction of the angle φ2.

  In the situation shown in FIG. 3, the position of the first distance data A, which is distance data based on the assumed self-position before confirmation, and the position of the second distance data P, which is actual measurement data, are different. Recognition is wrong. Therefore, correct self-position recognition is performed by an operation of rotating the first distance data A, further moving it in translation, and overlaying it on the second distance data P (that is, map matching).

  In FIG. 3, the arrangement of the laser radar 40 is shown at the position of the laser radar with respect to the first distance data A in correspondence with the position of the laser radar 4 with respect to the second distance data P measured. Therefore, the difference (Δθ, Δx1, Δy1) between the directions and positions of the laser radar 4 and the laser radar 40 represents a shift in self-position recognition. The processing for obtaining Δθ, Δx1, Δy1, or its equivalent amount (Δx, Δy) is self-position recognition processing.

  Next, formation of an angle histogram and calculation of a correction angle will be described. In FIG. 3, the measured data points a1 to a12 on the walls w1 and w2 are arranged in a substantially straight line under the control of the wall flatness and the distance measurement error. The data points p1 to p11 based on the map information are arranged in a substantially straight line as described above in accordance with the degree of obstacle modeling. Therefore, the inclination angle of the line segment between adjacent data points, for example, the line segment between the data points a1 and a2, is obtained as the inclination angle α1, and the appearance frequency F of the inclination angle, that is, the frequency is obtained. Then, a first angle histogram shown in FIG. 4A is obtained for the first distance data A, and a second angle histogram shown in FIG. 4B is obtained for the second distance data P. Is obtained.

  In the first and second angle histograms, frequency peaks appear at angles φ1 and α1 corresponding to the direction of the wall w1, and frequency peaks appear at angles φ2 and α2 corresponding to the direction of the wall w2. By the way, in the first and second distance data A and P, the mutual arrangement between the data points constituting each data is an invariant (conservation amount) with respect to the entire rotation and translation. Moreover, although there is a difference that the first distance data A is actually measured data and the second distance data P is model data, it is data for the same obstacle. Reflecting these points, the arrangement between the peaks in the first and second histograms is an invariant between the two histograms. That is, both histograms can be superimposed on each other by shifting in the angular direction. Shifting the histogram in the angle direction corresponds to rotating the distance data, and superimposing each other corresponds to making the linear data point sequences included in the two distance data parallel to each other.

  From the above, it can be seen that the movement angle necessary for overlaying the histograms is the correction angle itself for self-position recognition. In the actual operating environment of an autonomous mobile body, there are walls composed of a plurality of planes with different inclinations, walls with curved surfaces, moving obstacles, etc., so the second histogram shown in FIG. The waveform has a shape that includes noise. In addition, the waveform of the first histogram shown in FIG. 4A also usually includes some noise. The derivation of the angle at which these two waveforms are in the optimum superimposed state is numerically performed using a so-called cross-correlation function.

When the variable representing the angle is i and the first and second histograms are h1 (i) and h2 (i), the cross-correlation function is k (j)
k (j) = Σh1 (i) h2 (i + j),
It is expressed. Here, Σ is a symbol for calculating the sum for the angle variable i. The angle variable j giving the peak value (usually the maximum value, the same applies hereinafter) of the cross-correlation function k (j) is the correction angle Δθ. Note that Δθ, Δθ1, and Δθ2 in FIGS. 3 and 4B generally include errors and are not necessarily equal to each other, and Δθ≈Δθ1≈Δθ2.

  In FIG. 5, the first distance data A is rotated by the correction angle Δθ obtained by calculating the above-described cross-correlation function with the position of the laser radar 4 as the rotation center. First distance data B is shown. In this figure, the data points b1 to b4 and the data points p1 to p7 corresponding to the wall w1 are arranged in parallel at a distance Δu, and the data points b5 to b12 and the data points p8 to p8 corresponding to the wall w2 are arranged. p11 is arranged in parallel with a distance Δv from each other.

  Further, the laser radar 40 moves to the position of the laser radar 41 by the rotation of the correction angle Δθ. The positional deviations in the xy direction between the laser radar 4 and the laser radar 41 are the first and second correction distances Δx and Δy, which are the self-position recognition deviations in the state of FIG. In order to obtain the first and second correction distances Δx and Δy, the above-described correction distances Δu and Δv are obtained.

  In order to obtain the correction distances Δu and Δv, first, two angles of the first angle α1 and the second angle α2 having a high appearance frequency are extracted from the first histogram shown in FIG. In this example, a simplified example is shown and there are only two peaks, but in general there may be more than one. In addition, the first angle histogram is used because the first histogram is based on map information that does not include a moving obstacle or the like that causes noise for map matching. This is because accurate self-position recognition can be performed.

  When the difference between the first angle α1 and the second angle α2 is close to 90 degrees, one of the two angles is set to an angle perpendicular to the direction of the other angle. This is because it is considered that two walls facing different directions with an angular relationship close to 90 degrees are usually arranged orthogonal to each other. In this case, in accordance with reality, by making two directions orthogonal to each other as translation directions, errors in determining the angle can be eliminated and accurate self-position recognition can be performed.

  The first and second angles α1 and α2 are the new first distance data B obtained by rotating the first distance data A by the correction angle Δθ, respectively, as shown in FIG. β1 and β2. Here, β1 = α1 + Δθ and β2 = α2 + Δθ. These angles α1, α2 and therefore β1, β2 are used to determine the above-mentioned distances Δu, Δv. That is, with these two angles, as shown in FIG. 6, the axis (first translation axis u) in the direction orthogonal to the direction of the first angle β1 (the direction of the arrow s) and the direction of the second angle β2 ( An axis (second translation axis v) in a direction orthogonal to the direction of arrow t) is determined. The first distance data B is translated (translated) so as to overlap the second distance data P along these translational axes u and v, and the autonomous mobile body currently recognized by the movement distance. First and second correction distances Δu and Δv for correcting the positions in the respective translational axis directions are obtained.

  The first and second correction distances Δu and Δv described above are obtained using a histogram and a cross-correlation function in the same manner as the processing for obtaining the correction angle Δθ. As shown in FIG. 6, for each of the first distance data B and the second distance data P, data points b1 to b12 with respect to the first translation axis u are projected by projecting data points on the first translation axis u. The first position histogram composed of the appearance frequencies G1 and the second position histogram composed of the appearance frequencies G2 of the data points p1 to p11 are formed.

  Similarly, a first position histogram composed of the appearance frequencies G1 of the data points b1 to b12 with respect to the second translation axis v by projecting the data points on the second translation axis v, and the appearance frequencies G2 of the data points p1 to p11. To form a second position histogram.

  The first translation axis u of the autonomous mobile body currently recognized is calculated from the position variable that calculates the cross-correlation function of the first and second position histograms with respect to the first translation axis u and gives the peak value of the cross-correlation function. A first correction distance Δu for correcting the position in the direction is obtained. Further, the second translation of the autonomous mobile body currently recognized is calculated from the position variable that calculates the cross-correlation function of the first and second position histograms with respect to the second translation axis v and gives the peak value of the cross-correlation function. A second correction distance Δv for correcting the position in the axis v direction is obtained.

Next, with reference to FIG. 7, the correlation among the above-described correction distances Δu, Δv, Δx, Δy is obtained. In FIG. 7, a straight line bd is an auxiliary line. The angle δ is the angle formed by the data point sequence along the two walls w1 and w2, and therefore along these walls, preferably based on the first distance data A and B, and δ = β2−β1 = α2−α1. . From the geometrical consideration in FIG. 7, equations representing Δx and Δy are obtained as follows.
Δx = ((Δv · cos (β1) + Δu · cos (π−β2)) / sin (δ),
Δy = ((Δu · sin (π−β2) −Δv · sin (β1)) / sin (δ).

(Second Embodiment)
Next, another example of the self-position recognition process in the self-position recognition system 1 will be described. FIG. 8 shows a flowchart of this process, and FIG. 9 shows an example of arrangement of distance data and laser radar. The block configuration of the self-position recognition system 1 in this embodiment is the same as that of the first embodiment shown in FIG. Further, in the flowchart shown in FIG. 8, Step # 1 is the same as Steps S1 to S3 and Step S5 in the flowchart in the first embodiment shown in FIG. As described above, the order of steps S4 and S5 can be changed.

  The present embodiment is characterized by step # 2. In this step, both the distance data A and P are further rotated following the rotation process of step S4 in FIG. By the rotation of step # 2, as shown in FIG. 9, the direction of the first angle α1 (the direction of the arrow s) is parallel to the y axis of the xy coordinate system which is the coordinate system used for self-position recognition. Both the first distance data A and the second distance data P are rotated. The laser radar 4 is rotated on the spot by this additional rotation, so that the laser radar 4a is arranged. Further, the laser radar 41 corresponding to the first distance data A becomes the arrangement of the laser radar 41a by this additional rotation.

  The first distance data A becomes new first distance data B1 by the rotation of the angle ω1 = π / 2−α1. Further, the second distance data P becomes new second distance data P1 by the rotation of the angle ω2 = π / 2−α1−Δθ. By this rotation, the first translation axis becomes the x-axis direction. Further, the second translation axis (Y axis in FIG. 9) is a direction rotated by an angle δ = α2−α1 from the x axis.

  In step # 3, a histogram relating to the first translation axis x and the second translation axis Y is formed from the rotated first and second distance data B1 and P1 shown in FIG. This process is the same as step S6 in the first embodiment and the description of FIG.

  In Step # 4, as in Steps S7 and S8 in the first embodiment, the cross-correlation function is calculated for each of the translation axes x and Y, and the first and second correction distances are obtained. That is, as shown in FIG. 9, the correction distance Δx2 in the first translation axis x direction and the correction distance ΔY in the second translation axis Y direction are obtained. Further, using the correction distances Δx2, ΔY and the correction angle Δθ obtained in step # 1, the rotational direction, that is, the orientation (or orientation) of the currently recognized autonomous mobile body and the coordinates of the current position are corrected. Thus, self-position recognition is performed.

Next, the correlation between the above-described correction distances Δx2, ΔY and the correction distance before the additional rotation, that is, Δx, Δy, is obtained. Here, if the translation difference in the y-axis direction between the first distance data B1 and the second distance data P1 is Δy2, δ = α2−α1, ΔY, Δx2 is used.
Δy2 = (ΔY−Δx2 · cosδ) / sinδ,
It becomes. By the way, the first distance data B1 and the second distance data P1 are the first distance data B and the second distance data that generate the correction distances Δx and Δy from the translation differences of each other in FIG. Since both P are rotated by an angle (π−α1), the correction distance (Δx, Δy) is obtained by reversely rotating the correction distance (Δx2, Δy2) by the angle (π−α1). Therefore,
Δx = Δx2 · cos (α1) −Δy2 · sin (α1),
Δy = Δx2 · sin (α1) + Δy2 · cos (α1),
It becomes.

  In the above, it is possible to arbitrarily select which direction of the first and second angles α1 and α2 is parallel to the coordinate axis of which coordinate system. In the example of FIG. 9, the direction of the smaller angle α1 (α1 <α2) of the two angles α1 and α2 is parallel to the y axis. In addition, the first rotation and the additional rotation described above can actually be performed by one rotation without being divided into two stages.

  According to the present embodiment in which the rotation described above is performed, the direction of one translation axis coincides with the direction of one of the coordinate axes (x-axis) of the coordinate system used for self-position recognition. The calculation becomes easy and the calculation load for self-position recognition can be reduced.

(Third embodiment)
The third embodiment will be described with reference to FIGS. 10 to 12A, 12B, and 12C. In this embodiment, an example of data when the self-position recognition system 1 described in the second embodiment is applied to a real environment is shown. FIG. 10 shows a state where the autonomous mobile body 10 equipped with a self-position recognition system performs self-position recognition during movement, and FIGS. 11 (a) (b) (c) and FIGS. 12 (a) (b) (c) Shows examples of histograms and cross-correlation functions.

  The autonomous mobile body 10 having a substantially rectangular parallelepiped shape is provided with the laser radar 4 of the self-position recognition system 1 (not shown) at the front left corner, and is moving in a path surrounded by walls W. The left wall W has a recess made of a wall surface that is not orthogonal. The self-position recognition system 1 of the autonomous mobile body 10 acquires the first distance data from the map information based on the currently recognized self-position, and the front in the scannable angle range by the actual measurement using the laser radar 4. And distance data (second distance data) of the obstacle on the left is acquired. Note that measurement data cannot be obtained when the incident angle of the laser beam is close to 90 ° (when parallel to the wall surface).

  The angle histogram and the distance histogram obtained based on the first and second distance data acquired under the above situation are as shown in FIGS. 11 (a), 11 (b), and 11 (c). The deviation in the direction of the angle coordinate axis between the first angle histogram m0 corresponding to the first distance data (obtained from the map information) and the second angle histogram r0 corresponding to the second distance data (actually measured value). It is a correction angle. Further, the first axis direction and the second axis direction are determined from the angles with respect to the first two peaks of the angle histogram m0, and the correction distance is obtained from the distance histogram regarding these axes. Distance histograms m1 and m2 are obtained from the first distance data, and distance histograms r1 and r2 are obtained from the second distance data.

  When the cross-correlation function k is calculated for each of the angle histograms m0, r0, the distance histograms m1, r1, and the distance histograms m2, r2, the results are as shown in FIGS. 12 (a), 12 (b), and 12 (c). The values of the angle difference and the distance difference corresponding to the positions of the peaks c0, c1, and c2 are the correction angle and the correction distance, respectively. During automatic processing in the self-position recognition system 1, a predetermined peak is detected by changing the threshold value TH.

  The map information storage means 2, the self-position storage means 3, and the calculation means 5 of the self-position recognition system 1 in the first to third embodiments described above are a CPU, a memory, an external storage device, a display device, and an input. An electronic computer having a general configuration including an apparatus and the like, and a process or a function set on the computer can be configured. Further, the present invention is not limited to the above configuration and can be variously modified. For example, the laser radar 4 has been described as a means for acquiring the second distance data, which is environment information. However, the distance image is generated not only by the laser radar but also by using an ultrasonic array sensor or a CCD image device. You may make it acquire 2nd distance data from a distance image. In the flowcharts shown in FIGS. 2 and 8, processes that can be processed in parallel may be performed in parallel, or the order of the processes may be changed.

  In the above description, the first distance data A is rotated by the correction angle Δθ or ω1 = π / 2−α1-Δθ to obtain new first distance data B and B1. In this case, the first distance data B and B1 can be obtained by performing the rotation process in two ways. One is a method of generating a data point having a new coordinate value by rotationally converting the coordinates of each data point. The other one is a method based on the original method of acquiring the first distance data. That is, after correcting the direction based on Δθ or ω1 for the position recognized as the current position, sensing is performed using the laser radar 4 at the new position recognized as the current position after rotation. This is a method of acquiring obstacle information (referred to as first distance data) based on map information that should be obtained. According to the second method, as the angle is corrected, it becomes closer to the actually measured data, and it is considered that the matching accuracy between the two distance histograms is improved.

The block block diagram of the self-position recognition system which concerns on the 1st Embodiment of this invention. The flowchart explaining the self-position recognition process in a system same as the above. The top view which shows the data point example of a 1st and 2nd distance data processed with a system same as the above. FIG. 4A is a diagram of an angle histogram related to the first distance data shown in FIG. 3, and FIG. 4B is a diagram of an angle histogram related to the second distance data shown in FIG. The top view of the state which rotated the 1st distance data in FIG. 3 by the correction angle. The top view which shows a mode that a position histogram is formed from the 1st and 2nd distance data shown in FIG. The geometric arrangement | positioning figure for demonstrating derivation | leading-out of the correction distance in a system same as the above. The flowchart explaining the self-position recognition process in the self-position recognition system which concerns on 2nd Embodiment. The top view which shows the data point arrangement example of the 1st and 2nd distance data processed according to a flowchart same as the above. The perspective view which shows a mode that the autonomous mobile body provided with the self-position recognition system which concerns on 3rd Embodiment performs self-position recognition. (A) is a figure which shows the example of an angle histogram, (b) (c) is a figure which shows the example of a position histogram. FIG. 11A is a cross-correlation function diagram for the angle histogram of FIG. 11A, FIG. 11B is a cross-correlation function diagram for the distance histogram of FIG. 11B, and FIG. FIG. 4 is a cross-correlation function diagram for a distance histogram.

Explanation of symbols

DESCRIPTION OF SYMBOLS 1 Self-position recognition system 2 Map information storage means 3 Self-position storage means 4, 4a, 40, 41, 41a Laser radar 5 Calculation means a1-a12, b1-b12, p1-p11 Data point k Cross correlation function u, x 1st 1 translational axis v, Y 2nd translational axis A, B, B1 1st distance data F (angle) appearance frequency G, G1, G2 (position) appearance frequency P, P1 2nd distance data α1, β1 first angle α2, β2 second angle Δθ correction angle Δx first correction distance Δy second correction distance

Claims (5)

Map information storage means for storing pre-input map information, self-position storage means for storing the position of the autonomous mobile body recognized on the map information, and obstacles existing around the autonomous mobile body A radar that senses the obstacle, the obstacle information obtained by the laser radar, and the map information stored in the map information storage means around the self-position stored in the self-position storage means. A self-position recognition system comprising a computing means for correcting the self-position,
The computing means is
Obstacle information (referred to as first distance data) based on the map information that should be sensed using the laser radar at a position recognized as the current position, and actual obstacle information sensed by the laser radar (Referred to as second distance data)
For each of the first distance data and the second distance data, an angle of a line segment connecting the data points is obtained, and an appearance frequency of the obtained angle is obtained to obtain a first angle histogram and a second angle histogram. Forming,
The cross-correlation function of the first and second angle histograms is calculated, and based on the result, a correction angle for correcting the rotational direction of the currently recognized autonomous mobile body is obtained, and the first distance data is corrected. Obtain new first distance data rotated by an angle,
Extracting two frequently occurring angles in either the first angle histogram or the second angle histogram (these are called the first angle and the second angle),
The new first distance data includes an axis in a direction orthogonal to the direction of the first angle (referred to as a first translation axis) and the second angle so that the data overlaps the second distance data. The position of the autonomous mobile body currently recognized by the translation along the axis in the direction orthogonal to the direction of the angle (referred to as a second translation axis) is corrected in the first and second translation axis directions. Self-position recognition system characterized by that. The calculation means includes the frequency of appearance of data points with respect to the first translation axis by projecting data points on the first translation axis for each of the new first distance data and the second distance data. Forming a first position histogram and a second position histogram, and projecting data points onto the second translation axis, and a first position histogram and a second position histogram comprising the frequency of appearance of the data points with respect to the second translation axis Form a position histogram of
The cross-correlation function of the first and second position histograms with respect to the first translation axis is calculated, and distance data for correcting the position of the autonomous mobile body currently recognized based on the result in the first translation axis direction is obtained. In addition, the cross-correlation function of the first and second position histograms with respect to the second translation axis is calculated, and the position of the autonomous mobile body currently recognized based on the result is corrected in the second translation axis direction. 2. The self-position recognition system according to claim 1, wherein distance data is obtained.   Both the new first distance data and the second distance data are rotated so that the direction of the first or second angle is parallel to one of the coordinate axes of the coordinate system used for self-position recognition. 3. The self-position recognition system according to claim 1, wherein a correction distance in the first and second translational axis directions is obtained using the rotated distance data.   The self-position recognition system according to any one of claims 1 to 3, wherein the first and second angles are extracted from the first angle histogram.   2. When the difference between the first angle and the second angle is close to 90 degrees, one of the two angles is set to an angle perpendicular to the direction of the other angle. The self-position recognition system according to any one of claims 1 to 4.

Images