JP5499355B2 - Mobile environment recognition apparatus and method - Google Patents

Description

  The present invention relates to a mobile environment recognition apparatus and method, and is suitable for application to recognize the presence or absence of a required mobile environment when a leg motion type mobile body such as a legged robot or a leg wheel type robot moves. It is a thing.

  A leg motion type mobile body such as a leg type robot or a leg wheel type robot having a wheel as a leg landing member, when walking with a leg in a moving environment with a step, for example, like a step board of a staircase, It is necessary to step on a place where this leg can stand on its own each time it moves (this is called a step), and it is necessary to recognize the presence of the step before starting the walking motion.

  Conventionally, methods of Patent Documents 1 to 3 have been proposed as this type of recognition method.

Japanese Patent Laid-Open No. 9-61117 JP 2002-144278 A JP 2008-224380 A

  The technique described in Patent Literature 1 irradiates a ring light having a conical extension from an oblique direction, acquires an image with a camera on a vertical axis with respect to the object, and detects image coordinates. This is a three-dimensional position detection method for measuring the distance by comparing the three-dimensional coordinate calculation means for calculating the distance to the object with a part or all of the image recorded in advance in the image memory.

  This method has the function of a stereo camera using ring illumination based on so-called triangulation.

  However, the method of Patent Document 1 has a problem that it cannot detect the inclination of the plane and cannot detect the change in inclination.

  In addition, the technique disclosed in Patent Document 2 uses a light emitting means for irradiating slit light and an image for capturing an optical cutting line in order for an external sensor attached to the waist to measure the distance to the floor surface and the floor surface shape. Two imaging means having different quasi-values, a means for calculating the distance to the measurement object based on an image taken from the light cutting direction, and a means for monitoring the surroundings are provided, and the same object is provided by the two imaging means. And a binocular stereoscopic structure in which distance is calculated using parallax and parallax.

  The technique of this patent document 2 is characterized in that the detection sensitivity is improved by being attached to the waist, and that the floor surface can be stably measured even during the operation of the robot. Environmental awareness is still inadequate in using technology as is.

  Furthermore, the technique of Patent Document 3 extracts the features of the recognition target in the vertical and horizontal directions from the data array of the distance data in the vertical scanning plane obtained by the three-dimensional laser radar. It is designed to obtain movement environment information that allows the body to walk the recognition target, but in order to reconstruct the three-dimensional shape from the features of the cut surface by laser scanning, there is a lot of information to be processed, Since the procedure is also complicated, it is desirable to reduce these.

  The present invention has been made in consideration of the above points, and proposes a mobile environment recognition apparatus that can obtain detailed mobile environment information with a much smaller amount of information processing and a simplified procedure. is there.

  In order to solve such a problem, in the present invention, the cone scanning detection light DET that scans in the direction along the conical surface with the collimation direction Z as the center is projected from the sensor origin O to the measurement object 7, and the cone scanning detection is performed. A moving environment detecting means 3 that receives the conical scanning detection light DET reflected from the circle TR between the light DET and the surface S2 of the measuring object 7 and measures the distance from the sensor origin O to the measuring object 7, and the moving environment Based on the distance measurement data measured by the detecting means 3, the feature quantity calculating means 9 for calculating the feature quantities K1, K11, K21, K31, K41 of the intersecting circle TR, and the feature quantities K1, K11, K21, K31 of the intersecting circle TR. , K41, and a shape determining means 52 for determining the shape of the measurement target surface S2.

  According to the present invention, the distance from the sensor origin to the measurement object is measured based on the reflected light from the intersection circle between the conical scanning detection light and the surface of the measurement object, and according to the feature amount of the intersection circle obtained from the measurement result. By obtaining the shape of the surface of the measurement object, the shape of the measurement object can be easily and reliably measured using the three-dimensional configuration of the intersecting circle.

It is a rough-line side view which shows the structure of a moving body. It is a block diagram which shows the detailed structure of the external field recognition apparatus 9 of FIG. It is a rough-line perspective view which shows the detailed structure of the movement environment detector of FIG. It is a basic diagram with which it uses for description of the relationship between a cone scanning distance measuring method and a cone scanning drive method ((alpha), (theta), (phi)). It is a basic diagram with which it uses for description of the drive method of a cone scanning. It is a basic diagram with which it uses for description of a cone scanning type distance measuring method (setting value _dST , _rST , designation _| designated (alpha) measured value L). It is a basic diagram with which it uses for description of the relationship between the cone scanning measurement method in the case of using a distance image sensor, and a distance image imaging surface. It is a basic diagram which shows the structure of a distance image data memory. It is a basic diagram with which it uses for description of the method of detecting the tread of a staircase by cone detection light. It is a basic diagram with which it uses for description of the method of measuring the discontinuous surface of a staircase with cone detection light. It is a basic diagram with which it uses for description of the method of measuring the cliff of a staircase by cone detection light. It is a characteristic curve figure which shows the feature-value of convex coupling | bonding. It is a characteristic curve figure which shows the feature-value of a plane. It is a characteristic curve figure which shows the feature-value of concave coupling. It is a characteristic curve figure which shows the feature-value of a discontinuous surface. It is a characteristic curve figure which shows the feature-value of a cliff. It is a graph which shows the relationship between the feature-value of an intersection circle, and the characteristic of a measurement surface. It is a flowchart which shows the feature-value extraction process sequence by the crossed-circle feature-value extraction part 41 of FIG. 19 is a flowchart showing a detailed configuration of an intersecting circle feature quantity extraction processing procedure RT2 of FIG. FIG. 20 is a chart used for explaining a feature quantity classification program procedure in FIG. 19; FIG. FIG. 20 is a flowchart showing a detailed configuration of a processing procedure RT11 for calculating a feature amount of convex combination in FIG. 19; FIG. FIG. 22 is a schematic diagram for explaining features of a convex coupling calculated in a procedure for calculating a characteristic amount of convex coupling in FIG. 21; FIG. 22 is a characteristic curve diagram illustrating the feature amount of the convex combination calculated by the procedure for calculating the feature amount of the convex combination in FIG. 21. FIG. 20 is a flowchart illustrating a detailed configuration of a processing procedure for calculating a feature amount of concave coupling in FIG. 19. FIG. It is a basic diagram with which it uses for description of the characteristic of a concave coupling | bonding. FIG. 25 is a characteristic curve diagram showing the feature amount of the concave connection calculated by the processing procedure RT12 for calculating the feature amount of the concave connection in FIG. 24; FIG. 20 is a flowchart showing a detailed configuration of a cliff feature amount calculation processing procedure RT13 in FIG. 19; FIG. It is an approximate line figure used for explanation of the feature of a cliff. It is a characteristic curve figure which shows the feature-value of the cliff obtained by the calculation process procedure of the cliff feature-value of FIG. FIG. 20 is a flowchart showing a detailed configuration of a planar feature quantity calculation processing procedure RT14 in FIG. 19. FIG. It is an approximate line figure used for explanation of the feature of a plane. FIG. 31 is a characteristic curve diagram showing a planar feature value obtained by a planar feature value calculation processing procedure RT14 in FIG. 30; It is an approximate line figure showing how to compute the feature of a plane. FIG. 20 is a flowchart illustrating a detailed configuration of a discontinuous surface feature amount calculation processing procedure RT15 of FIG. 19. FIG. It is a basic diagram with which it uses for description of the characteristic of a discontinuous surface. FIG. 35 is a characteristic curve diagram showing a feature amount of a discontinuous surface obtained by a discontinuous surface feature amount calculation processing procedure RT15 of FIG. It is a basic diagram with which it uses for description of the method of the vertical scanning of an upstairs. It is a basic diagram with which it uses for description of the method of the vertical scanning by the cone detection light in the cone detection light in the case of FIG. It is a basic diagram with which it uses for description of the method of detection of the three-dimensional feature of a plane. It is a flowchart which shows the calculation process procedure of the staircase feature-value by vertical scanning. It is a basic diagram with which it uses for description of the method of the vertical scanning of a descent | fall stair. It is a flowchart which shows the calculation process procedure of the downward staircase feature-value by vertical scanning. It is a basic diagram with which it uses for description when a downward level | step difference exceeds limit height. It is a basic diagram with which it uses for description of the method of the vertical scan of a cliff. It is a flowchart which shows the calculation process procedure of the cliff feature-value by vertical scanning. It is a basic diagram with which it uses for description of the method of horizontal scanning of a tread. It is a flowchart which shows the calculation process procedure of the tread surface feature-value by horizontal scanning. It is a basic diagram with which it uses for description of the method of determining the range of a step. It is a basic diagram with which it uses for description of the method of the horizontal scanning of a kick surface. It is a flowchart which shows the calculation process procedure of the kick surface feature-value by horizontal scanning. It is a flowchart which shows the step confirmation process procedure of the time series scanning external field confirmation part 47. FIG. 4 is a flowchart showing a front leg staircase ascent process by the main body drive unit 15; It is a rough-line perspective view with which it uses for description of the upward motion of the moving body with respect to the upward staircase.

  Hereinafter, an embodiment of the present invention will be described in detail with reference to the drawings.

(1) Configuration and operation of a leg motion type mobile body In FIG. 1, reference numeral 1 denotes a mobile body as a whole, and the mobile environment detector 3 is projected forward from the center of the front upper end of a rectangular parallelepiped mobile body 2. Is provided.

  The left front leg 4L and the right front leg 4R are attached to the left and right ends of the lower part of the front surface of the mobile body 2, and the left rear leg 5L and the right rear leg 5R are attached to the left and right ends of the lower part of the rear surface of the mobile body 2. It is installed.

  The left front leg 4L, the right front leg 4R, the left rear leg 5L, and the right rear leg 5R hold the leg wheel J3 through a joint J0, a hip joint J1, and a knee joint J2 around the support shaft, respectively, so that a walking motion can be performed. .

  Thus, when the left front leg 4L and the right front leg 4R, and the left rear leg 5L and the right rear leg 5R are standing on the flat floor surface 6, the leg wheel J3 is driven to rotate, thereby moving the mobile body 2 to the wheel. When the mobile body 2 moves up and down the stairs 7 to be measured, the left front leg 4L or the right front leg 4R or the left rear leg 5L or the right rear leg is movable. As one means of stepping the leg wheel J3 on the tread surface 8 of the staircase 7 by causing any one of 5R to walk up and down without moving the leg wheel J3 so as to go up and down one step of the staircase 7. It can be used to go up and down the stairs 7 step by step.

  Such a moving operation of the moving body 1 is performed by the external environment recognition device 9 provided in the moving body main body 2 recognizing the moving environment using the detection result of the moving environment detector 3 and the left front leg 4L or the right front leg 4R. Alternatively, it is performed by driving and controlling the left rear leg 5L or the right rear leg 5R.

  As shown in FIG. 2, the external environment recognition device 9 includes a conical scanning distance measuring unit 31 or a distance image measuring unit 36 as the moving environment detector 3 and is constituted by a personal computer that performs information processing. When the system control central processing unit (CPU) 13 receives the command data input by the user from the bus 12, the CPU 13 detects the movement environment detector 3 using the program ROM / operation memory RAM 14 accordingly. By performing environment recognition calculation processing according to the result and notifying the main body drive unit 15 of the environment recognition result through the data input / output means 11, the left front leg 4L or the right front leg 4R or the left rear leg 5L or the right rear leg 5R Move it.

(2) Moving Environment Detection Function The moving environment detector 3 has a horizontal scanning mechanism 21 as shown in FIG. 3, and the horizontal scanning mechanism 21 protrudes forward from the front surface of the moving body 2. As shown in FIG. 1, the horizontal scanning mechanism 21 rotates the horizontal turntable 23 around the central axis z in the vertical direction in the horizontal plane as indicated by the arrow a.

  A vertical scanning mechanism 25 is attached to the lower surface of the horizontal turntable 23 via an attachment member 24, whereby the vertical scanning mechanism 25 attaches the vertical turntable 26 to one horizontal axis y as indicated by an arrow b. Is pivoted in the vertical plane around the center.

  A distance measuring device 27 is attached to the vertical turntable 26, whereby the distance measuring direction of the distance measuring device 27 is controlled to be changed in the vertical plane (and thus the elevation angle is changed) with reference to the other horizontal axis x. .

  In the case of this embodiment, the distance measuring device 27 emits an infrared pulse beam in a predetermined measurement direction, receives return light obtained by reflecting the infrared pulse beam on the measurement object, and transmits the infrared pulse beam. Based on the time difference between the emission time and the arrival time of the return light, the distance L from the distance measuring device 27 to the measurement object is measured.

  In the case of this embodiment, the mounting member 24, the vertical scanning mechanism 25, the vertical turntable 26 and the distance measuring device 27 attached to the horizontal turntable 23 are covered with a protective cover 28.

  Scanning of the distance measuring device 27 by the horizontal scanning mechanism 21 and the vertical scanning mechanism 25 is performed by a cone scanning detection light DET emitted from the distance measuring device 27 according to a user operation command input from the data input means 11 (FIG. 4). This is performed by the CPU 13 controlling the emission direction (referred to as collimation direction).

  Thus, the moving environment detector 3 can scan the distance measuring device 27 in all the angular directions in the horizontal plane by sequentially specifying the xy coordinates in the horizontal plane, and the vertical plane for all the angular positions in the horizontal plane. The distance between the recognition object existing in the environment to which the moving body 1 should move and the distance measuring device 27 can be scanned by the cone scanning distance measuring method shown in FIG. Can be measured.

  In FIG. 4, the position O where the distance measuring device 27 is disposed is defined as the origin (this is referred to as the sensor origin), the collimation direction of the distance measuring device 27 is represented by the Z axis, and the collimation direction axis Z The coordinates of the orthogonal plane are represented by the orthogonal coordinate axes X and Y.

In the case of FIG. 4, a circular plane having a radius r _ST drawn around the collimation direction axis Z (hereinafter referred to as the collimation direction) at the position of the reference distance d _ST from the sensor origin O of the collimation direction axis Z. By defining the horizontal and vertical scanning plane S1 as the infrared pulse detection beam from the sensor origin O along the circular orbit of the horizontal and vertical scanning plane S1, the sensor origin O is the apex and the horizontal and vertical scanning plane S1 is a cone. A conical scanning solid is assumed as the bottom.

As for the cone having the sensor origin O as the apex and the horizontal / vertical scanning plane S1 as the conical bottom, as shown in FIG. 5A, the center position C _ST of the horizontal / vertical scanning plane S1 from the sensor origin O with respect to the XZ plane is shown. a point obtained by moving from (horizontal and vertical scanning plane S1 and the intersecting point between the Z-axis) parallel to the only X-axis radius r _ST, a line connecting the sensor origin O, the detection pulsed light Lp emitted from the sensor origin O ( That is, it represents a conical surface through which an infrared pulse beam) passes.

Therefore, the radius r _ST of the conical bottom surface of the conical surface is given by the following equation from the conical half apex angle β and the reference distance d _ST from the sensor origin O to the horizontal / vertical scanning surface S1.

Can be represented by

  For each scanning position on the horizontal / vertical scanning plane S1, with respect to the orthogonal coordinate axis XY of the horizontal / vertical scanning plane S1, the conical scanning rotation angle α (= 0 to 360 °) starts from the intersection of the X axis and the X axis. When the scanning point P1 along the outer peripheral surface of the conical bottom surface is sequentially scanned by the above, the detection pulse light passing through the scanning point P1 reaches the measurement point P2 on the measurement target surface S2 located in front of the horizontal / vertical scanning surface S1. Therefore, a measurement point P2 that moves corresponding to the scanning of the scanning point P1 on the horizontal / vertical scanning surface S1 on the measurement target surface S2 can be considered.

  Here, if the appearance shape of the measurement target is arbitrary, the scanning target surface S2 does not necessarily have a surface orthogonal to the collimation direction axis Z, and may have an inclination, but is considered to be orthogonal. Then, the measurement target surface S2 draws a circle similar to the horizontal / vertical scanning surface S1.

  Thus, the x coordinate point P1x of the xy plane of the scanning point P1 on the horizontal / vertical scanning plane S1 is expressed by the following equation according to the horizontal scanning angle θ as shown in FIG.

And the y-coordinate point P1y of the yz plane is represented by the vertical scanning angle φ as shown in FIG.

Can be represented by

  As shown in FIG. 5C, the coordinate x on the x-axis of the scanning point P1 on the horizontal / vertical scanning plane S1 is expressed by the following equation, assuming that the cone scanning rotation angle α changes with time as ωt.

And the coordinate y on the y-axis is

It can be expressed as

  Based on the above operation principle, the CPU 13 (FIG. 2) activates the conical scanning distance measuring unit 31 to scan the horizontal and vertical scanning plane S1 by the conical scanning control unit 32 of the conical scanning distance measuring unit 31. A pulse beam emitted from the distance measuring device 27 at the sensor origin O by changing and controlling the point coordinates x and y based on the equations (4) and (5) to change the cone scanning rotation angle α with the angular velocity ω. Driving angle data representing the horizontal scanning angle θ and the vertical scanning angle φ necessary for scanning the lens along the circular orbit of the bottom surface of the cone.

  The driving angle data of the horizontal scanning angle θ and the vertical scanning angle φ is given to the horizontal / vertical scanning unit 33, and the horizontal / vertical scanning unit 33 sends the moving environment detector 3 to the horizontal scanning mechanism 21 and the vertical scanning mechanism 25. Send drive output.

  As a result, the pulse beam is projected onto the measurement target surface S2 so as to scan from α = 0 [°] to α = 360 [°].

  Here, if the measurement target surface S2 has a plane orthogonal to the axis of the collimation direction Z as shown in FIG. 6, the detection pulse light Lp emitted from the distance measuring device 27 is the scanning point P1. It passes through and is reflected by the measurement point P2.

  As a result, the measurement point P2 draws a circular intersection trajectory TR (hereinafter referred to as an intersection circle) similar to the horizontal / vertical scanning surface S1 on the measurement target surface S2, and the measurement target surface S2 corresponds to this. When tilted without being orthogonal to the axis of the collimation direction Z, an elliptical intersection circle TR is drawn.

  Thus, when the detection pulse light Lp reflected from the intersecting circle TR of the measurement target surface S2 returns to the distance measuring device 27 of the moving environment detector 3 (FIG. 2), the detection data is conically scanned via the bus 12. The distance from the sensor origin O to the measurement point P2 can be detected by being taken into the distance detection unit 34 of the formula distance measurement unit 31, and the measurement distance L is the cone scanning rotation angle α (= 0 to 360 [°]). Is stored in the cone scanning distance data memory 35A as data (α, L).

  On the other hand, in the case of the embodiment using the distance image sensor, the detection distance data of the measurement distance L measured by the distance image measuring unit 36 from the intersection circle TR of the measurement target surface S2 is the intersection of the distance image imaging surface DT of FIG. The pixel position data corresponding to the circle TR is stored in the distance image data memory 36B.

  As shown in FIG. 8, the distance image data memory 36B for storing the distance image imaging surface DT is designated by the line address i and the column address j corresponding to the orthogonal coordinate xy coordinate system as in the horizontal / vertical scanning surface S1. When the measurement distance data about the measurement distance L at the scanning point coordinates (x, y) on the scanning point P1 on the horizontal / vertical scanning plane S1 is obtained, the corresponding line address i is obtained. And the detection data memory location (i, j) having the column address j.

  The distance image data memory 36B in FIG. 8 sets the scanning line coordinates (x, y) on the horizontal / vertical scanning plane S1 to the memory area where the upper left corner memory address is the line address i = 0 and the column address j = 0. Stored as detection data L (i, j) converted to address = i and column address = j.

As a result, in the distance image data memory 36B, the measurement at the measurement point P2 of one intersecting circle TR (FIG. 7) obtained for each scan (α = 0 to 360 [°]) on the horizontal and vertical scanning plane S1. when the distance L of the data is obtained as the center line address i and the column address j corresponding to the central position C _ST horizontal and vertical scanning plane S1 so as to correspond to a circular corresponding to the radius r _ST conical bottom It is possible to obtain one piece of image data in which measurement distance data corresponding to the cone scanning rotation angle α (= 0 to 360 [°]) is stored at the memory address position.

(3) Measurement of various measurement targets If the measurement target is the staircase 7 described above with reference to FIG. 1, the measurement target surface is projected when the conical scanning detection light DET is projected on the floor surface 6 or the tread surface 8 which are flat surfaces. When S2 is inclined with respect to the collimation direction Z, when a kick from the floor surface 6 or the tread surface 8 is projected to the kick surface, or a nose from the kick surface to the tread surface one step above is projected Or when projecting a portion from the tread to the cliff, the data L (i, j) obtained by one photographing is detected in the distance image data memory 36B by individual cone scanning. Measurement data representing the three-dimensional appearance of the portion onto which the light is projected is stored in the distance data memory 35.

  Hereinafter, a case where the measurement data (α, L) is stored in the distance data memory 35 using the conical scanning distance measuring unit will be described.

  When the leg motion type moving body 1 as described above with reference to FIG. 1 goes up and down the stairs 7, it is necessary to distinguish various measurement objects in order for each leg to step on the tread surface 8 safely.

(3-1) Planar tread surface FIG. 9 shows the movement of the conical scanning detection light DET when detecting the planar tread surface 8 when going up the stairs 7. When approaching the tread surface 8, when the moving environment detector 3 is at the position 3A, the conical scanning detection light DET irradiates the convex nose 8A (this is referred to as “convex coupling”).

  At this time, the conical scanning detection light DET irradiates the nose 8A and the upper end portion of the kick surface 19 and the front end portion of the tread surface 8 that are adjacent to the nose 8A.

  When the moving environment detector 3 comes to a position 3B slightly advanced from this state, the conical scanning detection light DET irradiates the plane 8B from the nose 8A to the kick 8C (this is referred to as “plane”). .

  When the moving environment detector 3 comes to the further advanced position 3C, the conical scanning detection light DET is in a state of irradiating the concave kick 8C (this is called “concave coupling”).

  At this time, the conical scanning detection light DET irradiates the kick 8 </ b> C, the rear end portion of the tread surface 8 adjacent to the front and rear thereof, and the lower end portion of the kick surface 19.

  In the case of FIG. 9, the height from the floor surface 6 to the moving environment detector 3 is 500 [mm], the height from the tread surface 8 to the moving environment detector 3 is 400 [mm], and the depression angle in the collimation direction Z is It is assumed that the angle is set to 45 [°].

(3-2) Discontinuous surface FIG. 10 shows a case where the moving body 1 descends the staircase 7 from the floor surface 6 to the tread surface 8 lower by one step. In this case, the moving object 1 from the mobile environment detector 3 at the position 3D is shown. The conical scanning detection light DET is in a state of irradiating a discontinuous plane that moves from the floor surface 6 to the stepped surface 8 of the lower stage (this is referred to as a “discontinuous surface”).

  In the case of FIG. 10, the height from the floor surface 6 to the moving environment detector 3 is 400 [mm], the height from the tread surface 8 to the moving environment detector 3 is 500 [mm], and the depression angle in the collimation direction Z is It is assumed that the angle is set to 45 [°].

(3-3) Cliff FIG. 11 shows a part of the cone scanning detection light DET from the moving environment detector 3 at the position 3E when the moving body 1 comes to the cliff 10 whose shape is unknown from the tread surface 8. In this state, the reflected light does not return (this is called “cliff”).

  In the case of FIG. 11, the height from the tread surface 8 to the moving environment detector 3 is 400 [mm], the height from the tread surface 8 to the moving environment detector 3 is 500 [mm], and the depression angle in the collimation direction Z is It is assumed that the angle is set to 45 [°].

(4) Features of distance image data As a result of the movement environment detector 3 measuring the measurement object of FIGS. 9 to 11, the distance data (α, L) stored in the distance data memory 35 is the cone scanning detection light DET. While the circular scanning rotation angle α makes a round from 0 ° to 360 °, the measurement distance L changes as shown in FIGS. 12 to 16 (this is called a feature amount).

(4-1) Convex Coupling Feature Value As described above with reference to FIG. 9, when the conical scanning detection light DET is convexly coupled to the nose 8A as a measurement target, the value of the measurement distance L accumulated in the distance data memory 35 is As shown in FIG. 12, while the cone scanning rotation angle α makes a round from 0 to 360 [°], two local maximum values MX1 and MX2 that change continuously are generated and changed so as to turn back. A characteristic curve representing the feature quantity K1 that yields two minimum values MN1 and MN2 is obtained.

  In FIG. 12, the measurement distance L is in the rear direction where α is between 0 and 180 [°], and the first half portion of the intersection circle TR is once closer to the front side, which is once farther away from the tread surface 8 which is a horizontal step higher. Since the scanning is performed so as to return, the change continuously passes through the maximum value MX1, and subsequently, when α is 180 to 360 °, the second half portion of the intersection circle TR passes through the nose 8A and is a vertical surface. 19 is scanned so as to return to the upper nose 8A which is closer to the lower side and then returns to the upper nose 8A which is closer to the upper side. Change through value MX2.

  The characteristic curve of the feature value K1 is discontinuous at the minimum values MN1 and MN2 where the first derivative of the characteristic curve of the measurement distance L when the cone scanning rotation angle α is changed from 0 to 360 [°], that is, the edge strength is minimal. This represents a jump edge), and by determining that the changes before and after the minimum values MN1 and MN2 are turned back, it is determined that the measurement target is the nose 8A and its minimum value. It can be detected that MN1 and MN2 are the measurement distance L from the sensor origin O of the moving environment detector 3 to the nose 8A.

  The feature value K1 in FIG. 12 is based on the measurement value when the measurement target surface S2 is a plane orthogonal to the collimation direction Z (and therefore has no inclination) as described above with reference to FIG. The minimum values MN1 and MN2 occur at α = 0 and 180 °, and the maximum values MX1 and MX2 occur at α = 90 and 270 °.

(4-2) Plane Feature Amount When the cone scanning detection light DET irradiates the plane 8B in FIG. 9, the distance image data in the distance data memory 35 has a cone scanning rotation angle α of 0 as shown in FIG. A characteristic curve of the feature quantity K11 is obtained in which one continuous maximum value MX11 and one continuous minimum value MN11 are generated during one round to ˜360 [°] and there is no discontinuous portion.

  In FIG. 13, when α is 0 to 180 °, the measurement distance L returns to the rear which is close to the front surface 8B of the tread surface 8B in which the first half portion of the intersecting circle TR is a horizontal surface once turned to the front. In this way, the change continuously passes through the maximum value MX11. Subsequently, when α is 180 to 360 °, the latter half portion of the intersection circle TR once becomes closer to the plane 8B and then becomes farther away. Since scanning is performed so as to return to the front, the change continuously passes through the minimum value MN11.

  On the other hand, when the measurement distance L scans a plane made of a vertical surface such as the kicking surface 9, the first half of the intersecting circle TR temporarily approaches the vertical surface while α is 0 to 180 °. Since the scanning is performed so as to return to the farther downward direction after being directed upward, the change continuously passes through the minimum value MN11. Subsequently, when α is 180 to 360 °, the second half portion of the intersecting circle TR is a vertical plane. Since the scanning is performed so as to turn downward and then return upward, the change continuously passes through the maximum value MX11.

  As described above, in any of the horizontal plane and the vertical plane, the measurement distance L changes so as to have one continuous maximum value and one continuous minimum value and no discontinuous portion.

  Thus, if the distance data stored in the distance data memory 35 is determined to have no jump edge by calculating the edge strength by first-order differentiation with respect to the edge, the measurement object is a plane. And the distance from the moving environment detector 3 to the measured plane and its posture can be calculated.

  The feature value K11 of FIG. 13 is based on the measurement value when the measurement target surface S2 is a plane orthogonal to the collimation direction Z (and therefore has no inclination) as described above with reference to FIG. The maximum value MX11 and the minimum value MN11 occur at α = 90 and 270 [°].

(4-3) Concave coupling feature amount When the conical scanning detection light DET is in a state of irradiating the kick 8C in FIG. 9, the distance data in the distance data memory 35 has a conical scanning rotation angle α as shown in FIG. The characteristic curve of the characteristic quantity K21 that two local minimum values MN21 and MN22 appear as continuous changes and two local maximum values MX21 and MX22 that change so as to return are generated during one round from 0 to 360 [°]. Is obtained.

  In FIG. 14, the measurement distance L is continuously continuous when α is in the range of 0 to 180 °, because the first half of the intersection circle TR returns to the lower side after once turning upward near the kick surface, which is a vertical surface. After passing through the minimum value MN21, and after α is 180-360 [°], the second half of the intersection circle TR passes through the kick 8C and temporarily turns the tread surface 8 which is a horizontal plane toward the rear. Since the scanning is performed so as to return to the farther front, the change continuously passes through the minimum value MN22 between the maximum values MX21 and MX22 that change so as to be turned back toward the closer position at the position of the kick 8C.

  In this case, since the edge strengths of the maximum values MX21 and MX22 are discontinuous, it is determined from the first derivative of the edge that there are two turns, and that there are two continuous minimum values MN21 and MN22. By determining, the measurement object is the kick 8C, and the distance from the moving environment detector 3 to the kick 8C can be calculated.

  The feature value K21 in FIG. 14 is based on the measurement value when the measurement target surface S2 is a plane orthogonal to the collimation direction Z (and thus has no inclination) as described above with reference to FIG. The minimum values MN21 and MN22 occur at α = 90 and 270 [°], and the maximum values MX21 and MX22 occur at α = 0 and 180 [°].

(4-4) Discontinuous Surface Feature Amount When the moving environment detector 3 detects the discontinuous surface of FIG. 10, the distance data memory 35 makes a round of the cone scanning rotation angle α from 0 to 360 [°]. As shown in FIG. 15, a characteristic curve of the feature value K31 is generated so that one continuous maximum value MX31 and one local minimum value MN31 are generated, and there are two discontinuous portions.

  In FIG. 15, the measurement distance L is near after the tread surface 8, which is a horizontal surface which is lower by one step (and thus far) in the first half of the intersection circle TR, when the α is in the range of 0 to 180 °, once turned forward. Since the scanning is performed so as to return backward, the change continuously passes through the maximum value MX31. Subsequently, when α is 180 to 360 °, it passes through the nose 8A and is one step higher than the tread surface 8 (therefore, the treading step). Since the floor surface 6 which is a horizontal surface (closer to the surface 8) is scanned backward so as to return to the nose 8A, the distance is continuously reduced to a minimum value at both ends of the discontinuous points NC31 and NC32. Change through MN31.

  In this case, when moving from the scanning of the tread surface 8 to the scanning of the floor surface 6 and when returning from the scanning of the floor surface 6 to the scanning of the tread surface 8, the measurement distance L changes abruptly by a value corresponding to the step. Thus, the discontinuous points NC31X and NC32X at both ends of the change continuously passing through the local minimum value are located at the same α position as the discontinuous points NC31 and NC32 at both ends of the change passing through the local maximum value MX31.

  Thus, if it is determined that there is one maximum value and one minimum value by calculating the edge strength by the first derivative of the edge and that there is a jump edge, the measurement object currently measured is a discontinuous surface. It can be judged that.

  The feature value K31 in FIG. 15 is based on the measurement value when the measurement target surface S2 is a plane orthogonal to the collimation direction Z (and thus has no inclination) as described above with reference to FIG. The maximum value MX31 and the minimum value MN31 occur at α = 90 and 270 °, and the two discontinuities occur at α = 0 and 180 °.

(4-5) Cliff Feature Amount When the moving environment detector 3 measures the cliff described above with reference to FIG. 11, the distance data memory 35 has a cone scanning rotation angle α of 0 to 360 [° as shown in FIG. ], A characteristic curve of the characteristic quantity K41 is obtained so that two local maximum values MX41 and MX42 are generated and one local minimum value MN41 is generated.

  In FIG. 16, the measurement distance L does not have a detection value because α is between 0 and 180 °, and the first half of the intersection TR irradiates the cliff 10 where no return light can be obtained. Between 180 and 360 [°], the second half of the intersection circle TR scans the tread surface 8 which is a horizontal plane so as to return to the edge farther forward after turning to the rear once passing through the edge. The maximum value MX41 and MX42 that are discontinuous to each other are continuously changed through the minimum value MN41.

  At this time, it is determined that there is a discontinuous part when the cone scanning rotation angle α makes a round in the rotation direction, and that there is a jump by calculating the edge strength by the first derivative of the edge, and the discontinuous part When it is determined that there are two places, it can be determined that the measurement object measured by the mobile environment detector 3 is a cliff.

  In this case, regarding the cone scan rotation angle α, one of the cliff judgment conditions is that there is a data missing area at a point where data is missing during a round of α = 0 to 360 °. You can also.

  The feature value K41 in FIG. 16 is based on the measurement value when the measurement target surface S2 is a plane orthogonal to the collimation direction Z (and thus has no inclination), as described above with reference to FIG. The minimum value MN41 occurs at α = 270 °, and the discontinuous points occur at α = 180 and 360 °.

(4-6) Identification of Measurement Object Based on Feature Amount The CPU 13 of the external environment recognition device 9 in FIG. 2 extracts the above-described feature amount using the measured distance data in the distance image data memory 35 by the intersecting circle feature amount calculation unit 41. By performing the processing, as shown in FIG. 17, the feature amount of the intersecting circle TR obtained for each round of the cone scanning rotation angle α is calculated, and the intersection circle number is assigned and accumulated in the intersecting circle feature amount memory 42. .

  The intersecting circle feature quantity calculation unit 41 is a position where the measurement distance data has two local maximum values, two local minimum values, no jump edge, a small change in size, and the measurement data is folded back at the position of the two local minimum points. When this occurs, the feature amount data indicating that the feature amount of the intersecting circle TR is a convex combination is stored in the intersecting feature amount memory 42 with the intersecting circle number.

  In addition, the intersecting circle feature quantity calculation unit 41 performs the intersecting circle TR when the measurement distance data has one maximum value, one minimum value, no jump edge, the change in size is small, and the measurement data does not return. The feature amount data indicating that the feature amount is a plane combination is stored in the intersecting feature amount memory 42 with an intersection number.

  Further, the intersecting circle feature amount calculation unit 41 intersects the circle when two local maximum values, two local minimum values, no jump edge, a change in size is small, and the return of measurement data occurs at two local maximum points. Feature amount data indicating that the feature amount of TR is a concave combination is stored in the intersection feature amount memory 42 with an intersection number.

  Further, the intersecting circle feature quantity calculation unit 41 calculates the intersection value when there is one maximum value, one minimum value, two jump edges, and the measurement data changes to the next surface and the measurement data does not return. Feature amount data indicating that the feature amount of the circle TR is a discontinuous surface is stored in the intersecting circle feature amount memory 42 with an intersection circle number.

  Further, the intersecting circle feature amount calculation unit 41 has two maximum values, one minimum value, two jump edges, a change in the size of the measurement data is the distance to the origin, and the measurement data does not return. The feature amount data indicating that the feature amount of the intersecting circle TR is a cliff combination is stored in the intersecting feature amount memory 42 with the intersection number.

  Thus, the intersecting circle feature quantity calculation unit 41 accumulates feature quantity data representing the state of the scaffold that the moving body 1 should step on based on the feature quantity to be measured in the feature quantity memory 42 for each intersection circle TR. go.

(5) Measurement processing of measurement target (5-1) Feature amount extraction When the user inputs an operation command by the data input means 11, the CPU 13 enters the processing of the feature amount extraction processing procedure RT1 shown in FIG. In SP1, the collimation direction Z is set as the measurement direction for the measurement object.

  At this time, the CPU 13 inputs the collimation direction Z from the vertical scanning feature amount extraction unit 45, the horizontal scanning feature extraction unit 46, or the time-series scanning external field confirmation unit 47 to the conical scanning distance measurement unit 31, thereby The scanning unit 33 adjusts the distance measuring device 27 of the moving environment detector 3 to the set collimation direction Z.

  In the case of this embodiment, the external environment recognition device 9 recognizes the external environment centered on the current position by the vertical scanning feature amount extraction unit 45 and the horizontal scanning feature extraction unit 46 while the moving body 1 is stopped. For this purpose, a plurality of vertical and horizontal collimation directions Z are sequentially set, and a vertical scanning feature amount is extracted from the time-series scanning external field confirmation unit 47 in order to move the moving body 1 over time. The collimation direction Z is sequentially set for the direction to be measured corresponding to the movement of the moving body 1 by the unit 45 or the horizontal scanning feature extraction unit 46.

  Subsequently, in step SP2, the CPU 13 causes the horizontal and vertical scanning unit 33 to drive the horizontal scanning mechanism 21 and the vertical scanning mechanism 25 (FIG. 3) in accordance with the control signal of the conical scanning control unit 32. Based on the above, the conical scanning type conical scanning detection light DET is emitted from the distance measuring device 27 of the moving environment detecting unit 3.

  This conical scanning detection light DET scans in the radial direction of the detection light pulse along the horizontal / vertical scanning plane S1 (FIG. 4) in the circumferential direction at an angular velocity ω (FIG. 5C), thereby detecting the three-dimensional scanning type. The measurement object is irradiated as light.

  The detection pulse light reflected from the measurement object is received by the distance measuring device 27, the distance detection unit 34 measures the distance to the measurement object, and the measurement distance data is stored in the operation address pixel position ( i, j) (FIG. 8).

  Thereby, in the distance data memory 35, detection data representing the measurement distance L between the measurement target surface S2 (FIG. 4) in the collimation direction set in the above-described step SP1 and the sensor origin O is stored in the distance data memory 35. The scanning trajectory for one round is stored in the distance data memory 35.

  Based on the accumulated distance data, the CPU 13 causes the intersecting circle feature amount calculation unit 41 to execute the intersecting circle feature amount extraction processing procedure shown in FIG. 19 in the next subroutine RT2.

  When entering the intersecting circle feature value extraction processing procedure RT2, the intersecting circle feature value calculating unit 41 first sets the target data (α−L) as a processing target by reading it from the distance data memory 35 in step SP11.

  Here, since the distance data L is determined as processing target data for each cone scanning rotation angle α, the processing target data is represented by (α−L).

  Subsequently, in the next step SP12, the intersecting circle feature value calculating unit 41 calculates the edge intensity ΔL of the measurement distance L in the direction in which the conical scanning rotation angle α increases in the processing target data (α−L). Obtain a "second derivative edge operator".

  This “first-order differential edge operator” represents a change ΔL (and hence a change rate) of the accumulated distance detection data L when the cone scanning rotation angle α is slightly changed.

  Subsequently, the intersecting circle feature quantity calculation unit 41 determines the number and size of L jump edges (that is, discontinuous points of the measurement distance L) in step SP13.

  The L jump edge (discontinuous point) is obtained by checking the number and size of the discontinuous points when the distance image data of the discontinuous surface feature amount K31 as shown in FIG. It is used to detect a measurement object that represents K31.

  Subsequently, the intersecting circle feature quantity calculation unit 41 extracts a sign change point of the edge strength ΔL in step SP14.

  When this process is performed by increasing the cone scanning rotation angle α in the feature values K1 to K14 of FIGS. 12 to 16, the edge intensity ΔL, and hence the sign of the first-order differential edge operator, is “+”. If the state changes to “−”, it can be seen that the distance detection data L has a maximum value at the changed position of the cone scanning rotation angle α.

  On the contrary, when the sign of the first-order differential edge operator changes from “−” to “+”, it means that the distance detection data L at the change point is a minimum value.

  Subsequently, in step SP15, the intersecting circle feature value calculating unit 41 determines the number of local maximums and minimums regarding the measurement target alternating circle.

  Subsequently, in the next step SP16, the intersecting circle feature quantity calculation unit 41 determines the classification of the feature quantity by confirming the combination of the number of local maximums and the number of local minimums (this is called a combination of extreme values). .

  As shown in FIG. 20, the procedure of the feature quantity classification program in this step SP16 determines the order of the feature quantities to be determined as “convex coupling”, “concave coupling”, “cliff”, “plane” and “discontinuous”. "Surface" order.

  In accordance with this order, when the extreme value is “maximum 2 and minimal 2” as a result of the combination determination of extreme values in step SP16, the intersecting circle feature value calculating unit 41 proceeds to step SP17 and changes from 0 to 360 °. The “secondary differential edge operator” is obtained by calculating the rate of change (namely, (ΔΔL)) of the primary differential edge operator of the processing target data (α−ΔL) respectively corresponding to the cone scanning rotation angle α.

  By calculating this “secondary differential edge operator” ΔΔL, the intersecting circle feature value calculating unit 41 determines the maximum and minimum of the feature value by the primary differential edge operator of the processing target data (α−ΔL) in the next step SP18. Confirmation and determination of continuity and turn-around of the image are made.

  Based on the determination result, the intersecting circle feature value calculating unit 41 determines the combination of continuous and folded in the next step SP19, and when the maximum continuous and minimal folded determination result is obtained, the process proceeds to the subroutine RT11. “Calculation of feature value of convex combination” is performed by executing “procedure calculation process procedure of convex combination feature” RT11 shown in FIG.

  On the other hand, when the determination result of maximum folding and minimum continuation is obtained in step SP19, the intersecting circle feature value calculation unit 41 proceeds to subroutine RT12 and performs "calculation of feature value of concave coupling". This is performed by executing the “concave coupling feature amount calculation processing procedure” RT12 shown in FIG.

  Further, when the combination determination result of the extreme value in the above-described step SP16 is “maximum 2 and minimal 1”, the intersecting circle feature value calculation unit 41 proceeds to subroutine RT13 and performs “calculation of the cliff feature value”. This is performed by executing the “cliff feature value calculation processing procedure” RT13 shown in FIG.

  Further, when the combination determination result of the extreme values in the above-described step SP16 is “maximum 1, minimum 1”, the intersecting circle feature quantity calculation unit 41 determines whether or not there is an “L jump edge” in step SP20. When a determination result indicating that there is no L jump edge is obtained, the process proceeds to subroutine RT14 to execute “calculation of planar feature value” RT14 and “planar feature value calculation processing procedure” RT14 shown in FIG. Do.

  On the other hand, when the determination result that there is an L jump edge is obtained in step SP20 described above, the intersecting circle feature value calculation unit 41 proceeds to subroutine RT15 and “calculates the feature value of the discontinuous surface”. Is performed by executing the “discontinuous surface feature value calculation processing procedure” RT15 in FIG.

  In this way, the intersecting circle feature amount calculation unit 41 determines whether the processing target data (α-L) based on the distance data stored in the distance data memory 35 is a convex coupling, a concave coupling, a cliff, a plane, or a discontinuous plane. After calculating the feature amount and calculating the feature amount, the process proceeds to step SP21 to check whether or not all the processing of the processing target data (α-L) is completed, and a negative result is obtained. When the process returns to the above-described step SP11, the process of the remaining process target data (α-L) is repeated.

  On the other hand, when a positive result is obtained in step SP21, the intersecting circle feature value calculating unit 41 proceeds to step SP22 and ends the intersecting circle feature value extraction processing procedure RT2.

  Thus, in step SP1 (FIG. 18), the intersecting circle feature value calculating unit 41 calculates the feature amount of the measurement target in the currently set collimation direction and stores it in the intersecting circle feature amount memory 42. Moving to SP4, it is determined whether or not the feature amount extraction processing for all scheduled collimation directions is completed, and when a negative result is obtained, the process returns to the above-described step SP1 and the features for the collimation direction remaining. Repeat the volume extraction process.

  On the other hand, when a positive result is obtained in step SP4, the intersecting feature quantity calculation unit 41 proceeds to step SP5 and ends the feature quantity extraction processing procedure RT1.

(5-2) Calculation of Feature Quantity of Each Measurement Object The calculation of each feature quantity described above with reference to FIG. 19 is performed by the intersecting circle feature quantity calculation unit 41 using the calculations shown in FIG. 21, FIG. 24, FIG. 27, FIG. This is done by executing a processing procedure.

(5-2-1) Convex Combining Feature Value Calculation Processing Procedure Upon entering the convex combining feature value calculation processing procedure RT11 (FIG. 21), the intersecting circle feature amount extraction unit 41 performs step SP31 in FIG. In the processing of steps SP16 to SP19 of the feature amount extraction processing procedure RT2, the processing target data (α-L) acquired from the distance data memory 35, the processing target data (FIG. 12) having two continuous maximums and two folding minimums. Set.

  As the processing target data (α-L) in this case, data detected under the measurement conditions of FIG. 22 is used as a specific embodiment.

  As shown in FIG. 22B, the measurement condition is that the moving environment detector 3 of the moving body 1 standing on the floor surface 6 is a kick surface following the floor surface 6 in the environment detection direction indicated by the arrow a. The conical scanning detection light DET is irradiated to the upper end portion of 19, the nose 8 </ b> A and the rear end portion of the tread surface 8, and as a result, as shown in FIG. The distance data of the irradiated portion of the tread surface 8 (FIG. 22A) and the irradiated portion of the kick surface 19 (FIG. 22C) across the nose 8A is included.

  In the case of FIG. 22, the collimation direction Z obliquely intersects with the tread surface 8 and the kick surface 19 from the sensor origin O at the rear upper side, so that the intersection circle TR is an ellipse having a major axis in the direction of the environment detection direction a. Draw.

  Thus, as shown in FIG. 23, the detection distance L is a change in which the conical scanning rotation angle α passes through the continuous maximum value MX1 along the intersecting circle TR on the tread surface 8 in the range of α = 0 to 180 [°]. Then, in the range of α = 180 to 360 [°], by continuously scanning the tread surface 8 and then scanning the kick surface 9 (FIG. 22C) through the nose 8A at the return minimum value MN1. Then, the distance detection operation is performed along an elliptical trajectory that passes through the continuous maximum MX2 and then returns to the tread surface 8 through the folding minimum MN2 of the rear nose 8A.

  As a result, the conical scanning detection light DET uses the scanning trajectory of the intersecting circle TR constituting the bottom of the cone as the measurement target surface that forms a solid over the kick surface 19 and the tread surface 8, and the three-dimensional shape of the measurement target. As described above, the processing target data (α−L) represented by the value of the detection distance L and its change can be obtained.

  On the basis of the set processing target data (α−L), the intersecting circle feature quantity extraction unit 41 determines the boundary between the tread surface 8 and the kick surface 19 created by the nose 8A in the next step SP32. A line W1 (referred to as a nose line) W1 is determined by two folding minimum points MN1 and MN2.

  This nose line W1 represents the distance, posture, and height of the nose 8A from the sensor origin O that the leg wheel J3 of the moving body 1 must pass over, and thus the external environment recognition device 9 represents the three-dimensional solid environment. It means that it was detected.

  Subsequently, in the next step SP33, the intersecting circle feature quantity detection unit 41 determines the presence of the horizontal plane (that is, the tread surface 8) based on the folding minimum two points MN1 and MN2 and the continuous maximum one point MX1.

  In further subsequent step SP34, the intersecting feature amount detecting unit 41 determines the vertical plane (that is, the kicked surface 19) based on the folding minimum two points MN1 and MN2 and the continuous maximum one point MX2 therebetween.

  The horizontal plane and the vertical plane mean that there is a three-dimensional measurement target portion at the back and front intersection positions following the nosing line W1, and that the size of the external recognition device 9 can be detected.

  Thus, the intersecting circle feature quantity detection unit 41 ends the process of the convex combination feature quantity calculation processing procedure RT11 in step SP35, and returns to the above-described intersecting circle feature quantity extraction processing procedure RT2 (FIG. 19).

  In this way, the external environment recognition device 9 includes the presence of the kick surface 19 that is a vertical plane from the floor surface 6 on which the moving body 1 stands among the ascending steps existing in the set collimation direction Z, and the kick surface 19. The nose 8A at the upper end of the nose and the tread surface 8, which is a horizontal plane following the nose 8A, can be detected as features of convex coupling.

(5-2-2) Concave Connection Feature Value Calculation Processing Procedure The intersecting circle feature value calculation unit 41 calculates the concave connection feature value in the subroutine RT12 based on the processing in steps SP16 to SP19 in FIG. This is performed by executing the combined feature amount calculation processing procedure RT12.

  When entering the concave joint feature value calculation processing procedure RT12, the intersecting circle feature value calculating unit 41 obtains from the distance data memory 35 in step SP41 in steps SP16 to SP19 of the intersecting feature value extraction processing procedure RT2 in FIG. Among the processing target data (α-L), the processing data (FIG. 14) for two folding maximum points and two continuous minimum points is set. The processing target data (α-L) in this case uses data detected under the measurement conditions of FIG. 25 as a specific embodiment.

  As shown in FIG. 25 (B), the measurement condition is that the moving body 1 standing on the floor surface 6 has a stepped kick 8C and a kick surface 19 following the floor surface 6 in the environment detection direction indicated by the arrow a. As shown in FIG. 25A, as a result, the intersection TR of the cone scanning detection light DET has a floor surface 6 and a kick surface 19 (see FIG. C)).

  Also in the case of FIG. 25, since the collimation direction Z is obliquely crossed from the rear upper sensor origin O with respect to the floor surface 6 and the kick surface 19, the intersection circle TR is an ellipse having a major axis in the direction of the environment detection direction a. Draw.

  Thus, as shown in FIG. 26, the detection distance L increases until the cone scanning rotation angle α starts rotating from α = 0 and reaches the kick 8C, and the return maximum is at the position of the kick 8C. After the value MX21 is reached, when the kicking surface 19 which is a vertical surface is rotating, it passes through the continuous minimum value MN21 and returns to the floor surface 6 from the kicking 8C to reach the return maximum value MX22, and then the floor surface 6 is rotated. During this time, a change is made to return to α = 360 [°] through the continuous minimum value MN22.

  As a result, the conical scanning detection light DET uses the trajectory of the intersecting circle TR that forms the bottom of the cone to measure the three-dimensional shape of the measurement target surface that forms a solid over the floor surface 6 or the kick surface 19. The processing target data (α-L) to be expressed is obtained.

  Based on the set processing target data (α−L), the intersecting circle feature quantity calculation unit 41 determines the boundary between the floor surface 6 and the kick surface 19 created by the kick 8C in the next step SP42. A line (referred to as a kick-in line) W2 is determined by the folded maximum values MX21 and MX22.

  Subsequently, in the next step SP43, the intersecting circle feature value calculating unit 41 determines a horizontal plane (in this case, the floor surface 6) based on the folding maximum two points MX21 and MX22 and the continuous minimum one point MN22 therebetween.

  Subsequently, in step SP44, the intersecting circle feature quantity calculation unit 41 determines the kick surface 19 that is a vertical plane based on the two folding maximum points MX21 and MX22 and the one continuous minimum point MN21 therebetween.

  Here, the kick line W2 represents the distance to the kick 8C that the leg wheel J3 of the moving body 1 can travel forward and the posture of the kick surface 19, and the determined horizontal and vertical planes are the floor to the kick 8C. This means that it has been detected that the surface continues and the kick 8C continues to the kick surface 19.

  Thus, the intersecting circle feature quantity detection unit 41 ends the processing of the concave coupling feature quantity calculation processing procedure RT12 in step SP45, and returns to the intersecting circle feature quantity extraction procedure procedure RT2 (FIG. 19).

  In this way, the external environment recognition device 9 includes the presence of the kick surface 19 that is a vertical plane from the floor surface 6 on which the moving body 1 stands among the steps existing in the collimation direction Z, and the floor surface 6 and the kick surface. The kick 8C between the surface 19 and the surface 19 can be detected as a three-dimensional measurement target portion at the intersection TR position.

(5-2-3) Cliff Feature Quantity Calculation Processing Procedure When the intersecting circle feature value calculating unit 41 enters the cliff feature value calculation subroutine RT13 following step SP16 of the intersecting circle feature value extraction processing procedure RT2 of FIG. First, in step SP51 of the cliff feature amount calculation processing procedure RT13 shown in FIG. 27, processing target data (α-L) (FIG. 16) having two discontinuous maximum points and one continuous minimum point is set from the distance data memory 35. To do.

  The processing target data (α-L) in this case uses data detected under the measurement conditions of FIG. 28 as a specific embodiment.

  As shown in FIG. 28 (B), this measurement condition is that there is no detection surface following the floor surface 6 in the environment detection direction indicated by the arrow a when the moving body 1 standing on the floor surface 6 (or dance scene) is shown. As shown in FIG. 28 (A), the cliff 10 is irradiated with the cone scanning detection light DET. There is no measurement target surface that forms an intersecting circle.

  In the case of FIG. 28, the edge part of the floor surface 6 is irradiated with the rear end part of an ellipse having a major axis in the direction of the environment detection direction a as an intersection circle TR.

  Thus, as shown in FIG. 29, the measurement distance L scans the edge portion adjacent to the cliff 10 of the floor surface 6 as an intersection circle TR in the range where the cone scanning rotation angle α is α = 180 to 360 [°]. As a result, processing target data (α that generates discontinuous maximum values MX41 and MX42 corresponding to the boundary line W3 of the floor surface 6 and the cliff 10 (this is called a cliff line) and a continuous minimum value MN41 therebetween. -L) is obtained.

  Based on the set processing target data (α−L), the intersecting circle feature quantity calculation unit 41 determines a cliff line W3 based on two discontinuous maximum points in the next step SP52.

  This cliff line W3 represents the distance and posture from the sensor origin O to the edge of the cliff 10 that the leg wheel J3 of the moving body 1 should not step on, and thus the external environment recognition device 9 indicates the environment where the cliff 10 is located. It means that it was detected.

  Subsequently, in the next step SP53, the intersecting circle feature value calculation unit 41 determines that the floor surface 6 (or dance) is a horizontal plane based on the presence of two discontinuous maximum points MX41 and MX42 and one continuous minimum point MN41 therebetween. Scene).

  This horizontal plane means that the external recognition device 9 has detected that the limit of the tread surface on which the mobile body 1 can stand before reaching the cliff 10 is in front of the cliff line W3.

  Thus, the intersecting circle feature quantity detection unit 41 ends the processing of the cliff feature quantity calculation processing procedure RT13 in step SP54, and returns to the intersecting circle feature value extraction processing procedure RT2 (FIG. 19).

  In this way, the external environment recognition device 9 detects that the measurement target is a three-dimensional environment having the cliff feature amount K41 based on the presence of the horizontal plane portion and the cliff line W3 existing in the set collimation direction Z. can do.

(5-2-4) Calculation of Plane Feature Quantity The intersecting circle feature quantity calculation unit 41 enters the plane feature quantity calculation processing subroutine RT14 following steps SP16 and SP20 of the intersecting circle feature quantity extraction processing procedure RT2 of FIG. In step SP61 of the planar feature amount calculation processing procedure RT14 shown in FIG. 30, the processing target data (α-L) (FIG. 13) having one discontinuous maximum and one continuous minimum is set from the distance data memory 35. To do.

  As processing target data (α-L) in this case, data detected under measurement conditions shown in FIG. 31 is used as a specific embodiment.

  As shown in FIG. 31 (A), the measurement conditions are as follows. The floor 6 is inclined from the sensor origin O toward the front lower side and from the left back to the right front as indicated by the arrow c. By irradiating the conical scanning detection light DET, the processing target data (α−L) having the plane feature quantity K11 shown in FIG. 32 is obtained from the elliptical intersection circle TR formed on the floor surface 6.

  The feature amount of this plane is that when the conical scanning rotation angle α is rotated to α = 45 °, the intersecting circle TR goes up the inclined surface, so that the measurement distance L becomes smaller and eventually the continuous minimum point MN11. When passing through and further rotating, the intersecting circle TR moves away in the major axis direction while going down the slope, so that the measurement distance L starts to rise.

  Thereafter, between α = 90 and 225 [°], the measurement distance L increases as the intersecting circle TR goes down the inclined surface, and reaches the continuous maximum value MX11 at the lowest point of the inclined surface.

  After passing this continuous maximum value MX11, the measurement distance L becomes smaller as the intersecting circle TR climbs the inclined surface.

  Based on the processing target data (α−L) having such a feature amount K11, the intersecting circle feature amount calculation unit 41 sets the maximum value Lmax and the rotation angle position α that becomes the maximum value of the measurement distance L in the next step SP62. (Lmax), the minimum value Lmin of the measurement distance L, and the rotation angle position α (Lmin) that is the minimum value are identified.

  Subsequently, the intersecting circle feature amount detection unit 41 sets the inclination direction α (Lmax), the inclination angle γ, and the distance dz from the sensor origin O to the plane in FIG. 33 in the next step SP63. Calculation is based on the positional relationship of the maximum point MX11 shown from the minimum point MN11.

  In FIG. 33, z represents the distance from the sensor origin O to the collimation direction Z, and the main axis m represents the distance in the direction orthogonal to z.

  In FIG. 33, β is a half apex angle of the cone scanning detection light DET formed by the cone scanning rotation angle α, and the distance dmin from the sensor origin O to the minimum point MN11 is expressed by the following equation:

And the distance dmax from the sensor origin O to the maximum point MX11 is given by the following equation:

The difference h can be calculated by the following equation:

become.

  Here, a line connecting the maximum point MX11 and the minimum point MN11 represents a component on the plane 6, and the difference h represents the height of the inclination of the plane.

  On the other hand, the leg lengths rmin and rmax lowered from the local minimum point MN11 and the local maximum point MX11 to the z-axis, respectively,

Thus, the sum k of both can be expressed by the half apex angle β.

become.

  As a result, between the difference h in equation (8),

There is a relationship.

  In the equation (12), γ represents an inclination angle of the inclined surface connecting the local maximum point MX11 and the local minimum point MN11 (that is, the plane 6 to be measured) from the conical bottom surface of the conical scanning detection light DET. The angle γ can be obtained from equation (12).

  The distance dz from the sensor origin O to the measurement target plane is determined by the following equation by the inclination angle γ thus obtained:

Can be obtained.

  In this manner, the intersecting circle feature quantity calculation unit 41 calculates the plane inclination direction α (Lmax), the inclination angle γ, and the distance dz from the sensor origin O to the plane in step SP63, and then proceeds to step SP64. Is determined as to whether the plane is a horizontal plane (γ is positive) or a vertical plane (γ is negative).

  Thus, the intersecting circle feature quantity calculation unit 41 ends the planar feature quantity calculation processing procedure RT14 in step SP65, and returns to the intersecting circle feature quantity extraction processing procedure RT2 (FIG. 19).

  In this way, the external environment recognition apparatus 9 determines whether the measurement target is a tilt direction, a tilt angle, a distance from the sensor origin O to the plane, and the plane is a horizontal direction, which exists in the set collimation direction Z. Whether it is a vertical plane can be detected as a three-dimensional environment having a plane feature K11.

(5-2-5) Calculation of Discontinuous Surface Feature Amount The intersection circle feature amount calculation unit 41 calculates a discontinuous surface feature amount after steps SP16 and SP20 of the intersection circle feature amount extraction processing procedure RT2 of FIG. When the procedure RT15 is entered, in the processing SP15 of the discontinuous surface feature value calculation process RT15 shown in FIG. 34, processing data (α for one continuous maximum, one continuous minimum, and two discontinuities from the distance data memory 35. -L) (FIG. 15) is set.

  As the processing target data (α-L) in this case, data detected under the measurement conditions of FIG. 35 is used as a specific embodiment.

  As shown in FIG. 35 (B), the measurement condition is that the moving body 1 standing on the floor surface 6 (or a dance scene) has one stairs following the floor surface 6 in the environment detection direction indicated by the arrow a. As shown in FIG. 35 (A), the floor tread 8 (or dancing scene) is irradiated with the cone scanning detection light DET. As a result, the floor 8A is sandwiched between the intersections TR of the cone scanning detection light DET. The surface 6 and the tread surface 8 are included.

  In the case of FIG. 35, since the conical scanning detection light DET is irradiated from the sensor origin O toward the front lower side, detection return from the floor surface 6 and the tread surface 8 at the rear end position and the front end position in the environment detection direction a. As a result of the light being obtained, an intersecting circle portion that is separated into the rear portion and the front portion of TR is generated, whereas the shadow 8X of the nose 8A at the tip of the tread surface 8 is between the intersecting circle portions. Due to the occurrence on the tread surface 8 at the lower stage, no detection return light can be obtained in the shadow range TRX.

  As a result, as shown in FIG. 36, the feature quantity K31 of the discontinuous surface is detected light from the intersection circle TR of the tread surface 8 in the rotation range where the cone scanning rotation angle α is α = 0 to 180 °. On the other hand, a change having a continuous maximum point MX31 is obtained based on the above, but if the intersection circle TR passes α = 180 ° and enters the shadow range TRX, the rotation that has entered the shadow range TRX A discontinuous point NC31 on the tread surface 8 side and a discontinuous point MX41 on the floor surface 6 side appear at the position of the angle α.

  At this time, the discontinuous point NC31 on the tread surface 8 side represents the measurement distance based on the return light from the tread surface 8 that is one step lower than the nose 8A, so the value of the measurement distance L is large. Since the discontinuous point MX41 on the floor surface 6 side represents the measurement distance based on the return light from the floor surface 6 closer to the nose 8A, the value of the measurement distance L becomes small, and as a result, between the discontinuous points NC31 and MX41. Causes a large jump in the measurement distance L.

  Thereafter, when the rotation of the cone scanning rotation angle α continues, the intersection TR on the floor surface 6 passes through the discontinuous minimum point MN31 and reaches the boundary line L4 of the nose line at α = 270 [°].

  Here, the intersecting circle again enters the shadow range TRX, thereby generating a discontinuous point MX42 on the floor surface 6 and a discontinuous point NC32 on the tread surface 8 in the feature value K31, and then stepping on a point of α = 360 [°]. Intersection circle TR of surface 8 is connected.

  At this time, even when returning from the discontinuous point MX42 to the discontinuous point NC32, a large jump occurs between the discontinuous points MX42 and NC32.

  As a result, the conical scanning detection light DET is a measurement target surface that forms a solid over the intersection circle TR formed on the floor surface 6 and the tread surface 8 and the shadow range TRX between the tread surface 8 and the nose 8A. Is obtained using the scanning trajectory of the intersecting circle TR forming the conical bottom surface, and processing target data (α-L) representing the three-dimensional shape of the measurement target is obtained.

  On the basis of the set processing target data (α−L), the intersecting circle feature amount calculation unit 41 uses the discontinuous points NC31 and NC32 on the floor surface 6 side to generate a boundary line based on the nose line W4 in the next step SP72. To decide.

  Subsequently, in the next step SP73, the intersecting circle feature value calculation unit 41 calculates the floor surface 6 (or dance) for the floor surface 6 by the feature amount curve including the continuous minimum point MN31 and the discontinuous points MX41 and MX42 at both ends thereof. Determine the horizontal plane that composes the scene.

  Subsequently, in the next step SP74, the intersecting circle feature value calculation unit 41 calculates the tread surface 8 (or dance) by the feature amount curve including the discontinuous maximum point MX31 and the discontinuous points NC31 and NC32 at both ends of the tread surface 8. Determine the horizontal plane that composes the scene.

  Thus, the intersecting circle feature quantity calculation unit 41 ends the discontinuous surface feature quantity calculation processing procedure RT15 in step SP75 and returns to the intersecting circle feature quantity extraction processing procedure RT2 of FIG.

  In this way, the external environment recognition device 9 has a three-dimensional measurement environment in which one step of the stair existing in the collimation direction Z is lowered from the floor surface 6 on which the moving body 1 stands to the tread surface 8 on the nose 8A. It can be detected as a feature of a discontinuous surface.

(6) Recognition of the outside world As described above, the outside world recognition device 9 of the moving body 1 shows the feature of each part of the three-dimensional measurement target as a change in the measurement distance L using the conical scanning detection light DET. By using the calculation function of the quantity (that is, the convex coupling feature quantity, the concave coupling feature quantity, the cliff feature quantity, the plane feature quantity, and the discontinuous plane feature quantity), for example, a three-dimensional structure such as an upstairs KD1 shown in FIG. By performing vertical scanning and horizontal scanning on the measurement object, the staircase feature amount necessary for the moving body 1 to go up and down is calculated.

  In the case of this embodiment, the vertical scanning means that the moving environment detector 3 detects the three-dimensional structure of the ascending staircase KD1 by scanning the conical scanning detection light DET in a direction orthogonal to the ascending staircase KD1. In addition, the horizontal scanning means that the structure in the width direction of the ascending staircase KD1 is detected by scanning the plane of the measuring object detected by the vertical scanning with the conical scanning detection light DET in the horizontal direction.

(6-1) Calculation of Ascending Staircase Feature Quantity by Vertical Scan As shown in FIG. 37, the external environment recognition device 9 of the moving body 1 starts from the floor surface D0 to the first step tread surface 1 (H1), second When going up the ascending stairs KD1 having the step tread surface 2 (H2)..., The conical scanning detection light DET is vertically scanned along the vertical scanning line SN1 orthogonal to the ascending stairs KD1, as shown in FIG. As a result, the intersection circle TR is irradiated onto the floor surface D0, the tread surfaces H1, H2,... Along the vertical scanning line SN1, thereby confirming the presence of the floor surface D0, the tread surfaces H1, H2,. By checking the floor D0, the treads H1, H2,..., The intersection circle TR is horizontally scanned in the horizontal direction, the range of the lateral width in which the moving body 1 can safely go up is confirmed. Can do.

  In the case of this embodiment, the CPU 13 of the external recognition device 9 drives the vertical scanning feature amount extraction unit 45 when a vertical scanning command is input from the data input means 11 by the user, as shown in FIG. The measurement target portions on the vertical scanning line SN1 are irradiated with the cone scanning detection light DET generated by the cone scanning distance measuring unit 31, thereby causing the distance data memory 35 to take in the data of the measurement distance L, and The feature amount data calculated by the intersecting feature amount calculating unit 41 based on the measured distance data is accumulated in the intersecting feature amount memory 42.

  At this time, when the conical scanning detection light DET irradiates the nose G1 (G2, G3...) As shown in FIG. The detection process of the three-dimensional structure part including the kick surface-nosed-tread surface which is a part is performed.

  Further, as shown in FIG. 39B, the vertical scanning feature amount extraction unit 45 irradiates the cone scanning detection light DET on the tread surface H1 (H2, H3...) And the kick surface F1 (F2, F3...). When this is done, the characteristic and type of the plane and the position and orientation of the plane are detected from the intersecting circle TR that scans the plane.

  Further, as shown in FIG. 39C, the vertical scanning feature amount extraction unit 45, when the conical scanning detection light DET scans the kick E2 (E3, E4...), The tread surface-kick from the intersecting circle TR. -The detection process of the three-dimensional component part which consists of a kick surface is performed.

  Such processing is performed by the vertical scanning feature amount extraction unit 45 executing “upward staircase feature amount calculation processing procedure by vertical scanning” RT21 shown in FIG.

  In the calculation processing procedure RT21, the vertical scanning feature quantity extraction unit 45 first sets an intersecting feature list by vertical scanning in step SP101.

  As the crossed circle feature list, the crossed circle feature quantity extraction unit 41 uses the one stored in the crossed circle feature quantity memory 42 by scanning the scanning line SN1 for the ascending step KD1 in FIG.

  Subsequently, in step SP102, the vertical scanning feature amount extraction unit 45 determines the floor surface D0 based on the planar feature amount of the floor surface D0 or the concave coupling feature amount of the kick 1 (E1), and the vertical scanning measurement target feature memory 50 To accumulate.

  Subsequently, in step SP <b> 103, the vertical scanning feature quantity extraction unit 45 determines a kick line for the kick 1 (E <b> 1) based on the concave coupling feature quantity, and accumulates it in the vertical scanning measurement target feature memory 50.

  Subsequently, in step SP104, the vertical scanning feature quantity extraction unit 45, the concave coupling feature quantity of the kick 1 (E1), the subsequent plane feature quantity of the kick face 1 (F1), and the subsequent nose 1 ( The kick surface 1 (F1) is determined based on the convex combination feature amount in G1) and stored in the vertical scanning measurement target feature memory 50.

  Subsequently, in the next step SP105, the vertical scanning feature quantity extraction unit 45 determines the nosing line of the nosing 1 (G1) based on the convex coupling amount of the nosing 1 (G1), and the vertical scanning measurement target feature memory. Accumulate in 50.

  Subsequently, in step SP106, the vertical scanning feature amount extraction unit 45 performs the convex coupling coupling amount in the nose 1 (G1), the subsequent planar feature amount in the tread surface 1 (H1), and the subsequent kick 2 (E2). The tread surface 1 (H1) is determined on the basis of the concave coupling feature amount in FIG.

  Thus, the vertical scanning feature amount extraction unit 45 determines a kick line, a kick surface, a nose line, and a tread surface that determine the structure of the cross feature list from the floor surface D0 to the first step surface 1 (H1). Then, it is stored in the vertical scanning measurement target feature memory 50.

  Subsequently, the vertical scanning feature quantity extraction unit 45 confirms whether or not the processing of all the vertical direction feature lists has been completed in step SP107, and when a negative result is obtained, returns to the above-described step SP103 and returns to the next stage. Repeat the process of determining each structure for the tread.

  This iterative process is performed by the vertical scanning feature amount extraction unit 45 in the vertical scanning range, that is, the tread surface of the ascending stairs KD1 that the moving body 1 intends to rise, that is, the second tread surface 2 (H2), three steps. Repeat until the vertical feature list for tread 3 (H3).

  Eventually, when all the processes in the vertical direction feature list for the vertical scanning line SN1 are completed, the vertical scanning feature amount extraction unit 45 proceeds to step SP108 when a positive result is obtained in step SP107.

  In step SP108, the vertical scanning feature amount extraction unit 45 calculates the distance from the position immediately below the sensor origin O to the first kick line, that is, the position of the kick 1 (E1), for the floor surface D0 determined in step SP108. Is stored in the vertical scanning measurement target feature memory 50.

  Subsequently, in the next step SP109, the vertical scanning feature amount extraction unit 45 obtains the attitude of the first kick line, that is, the line of the kick 1 (E1), and ascends KD1 with respect to the sensor origin O based on the attitude of the first kick line. Is stored in the vertical scanning measurement target feature memory 50.

  The data about the inclination of the upstairs KD1 is used to correctly position the mobile body 1 in front of the upstairs KD1 when the mobile body 1 actually climbs the upstairs KD1.

  Subsequently, the vertical scanning feature quantity extraction unit 45 proceeds to step SP110, obtains the number of treads, that is, the number of steps, based on the number of kick lines and the number of nosing lines, and then in step SP111, The interval is obtained as the length of the tread surface, and the altitude difference between the tread surfaces is obtained based on the kick height in step SP112, and the obtained data is stored in the vertical scanning measurement target feature memory 50.

  Thus, the vertical scanning feature quantity extraction unit 45 ends the processing of the ascending step feature quantity calculation processing procedure RT21 by vertical scanning in step SP113.

  According to the above configuration, the vertical scanning feature amount extraction unit 45 moves the leg wheel J3 of each leg on the floor D0 and approaches the ascending stairs KD1 for the ascending stairs KD1 that the moving body 1 intends to ascend. The recognition information necessary for stepping on the first, second,... Treads H1, H2.

(6-2) Calculation of descending staircase feature amount by vertical scanning The moving body 1 goes down the descending staircase KD2 shown in FIG. 41 (B) one step at a time from the floor surface L0 to the landing L1 through the tread surfaces N1 and N2. When going down, the moving environment detector 3 emits the conical scanning detection light DET emitted from the sensor origin O from the floor surface L0 immediately below, along the scanning line SN11, as shown in FIG. Perform vertical scanning forward.

  At this time, the CPU 13 causes the vertical scanning feature quantity extraction unit 45 to execute “downward staircase feature quantity calculation processing procedure by vertical scanning” RT22 of FIG. 42, and the vertical scanning feature quantity extraction unit 45 first performs conical scanning in step SP120. From the state in which the detection light DET is irradiated from the floor L0 to the first step nose 1 (M1) and the first step tread 1 (N1), the second step nose 2 (M2) on the tread 1 (N1). ) Until the second step tread 2 (N2) and the third step landing L1 are repeated in the same manner, the feature amount for the descending staircase KD2 is calculated as an intersection feature amount calculation unit 41. And is stored in the intersecting circle feature amount memory 42.

  Thus, the vertical scanning feature quantity extraction unit 45 sets the accumulated intersection feature quantity list from the intersection feature quantity memory 42, and then, in step SP121, the first tread surface 1 from the planar feature quantity for the floor L0. Based on the upper feature amount of the discontinuous surface following (N1), the floor surface L0 on which the moving body 1 stands is determined and stored in the vertical scanning measurement target feature memory 50.

  Subsequently, the vertical scanning feature amount extraction unit 45 proceeds to step SP122 and based on the boundary line of the discontinuous surface in the nose 1 (M1) following the first step tread surface 1 (N1) from the floor surface L0. The nosing line W11 of the nosing 1 (M1) is determined and stored in the vertical scanning measurement target feature memory 50.

  Subsequently, the vertical scanning feature amount extraction unit 45 proceeds to the next step SP123, and based on the planar feature amount of the first step surface 1 (N1) following the feature amount of the first discontinuous surface, The step tread surface 1 (N1) is determined and stored in the vertical scanning measurement target feature memory 50.

  Subsequently, in the next step SP124, the vertical scanning feature quantity extraction unit 45 is based on the feature of the discontinuous surface following the first step tread surface 1 (N1). It is determined whether or not it is the tread surface 2 (N2), and is stored in the vertical scanning measurement target feature memory 50.

  The next step SP125 is a step of determining whether or not all the determination processes for the vertical direction feature list accumulated in the intersecting feature quantity memory 42 have been completed. If a negative result is obtained, this is still not true. This means that the processing for all the planes, that is, the treads N2 and N3 and the landing L1 has not been completed. At this time, the vertical scanning feature extraction unit 45 returns to the above-described step SP122 and performs the intersection by the next vertical scanning. The processes in steps SP122 to SP125 are repeated for the circle feature list.

  If a positive result is eventually obtained in step SP125, this means that the determination or determination of the tread surface 1 (N1), the tread surface 2 (N2), and the landing L1 has been completed. At this time, the vertical scanning feature value extraction is performed. The unit 45 moves to step SP126 and moves the distance from the position immediately below the floor L0 to the nosing line W11 of the first nosing 1 (M1) based on the data accumulated in the vertical scanning measurement target feature memory 50. The distance required for the body 1 to go to just before the descending stairs KD2 is obtained and stored in the vertical scanning measurement target feature memory 50.

  Subsequently, in the next step SP127, the vertical scanning feature quantity extraction unit 45 obtains the inclination of the descending step KD2 (inclination from the direction orthogonal to the sensor origin O) based on the attitude of the first nose line W11. After that, in step SP128, the number of steps on the front, that is, the number of steps on the staircase is obtained based on the number of lower steps of the discontinuous surface and the number of nose lines in the forward direction, and stored in the vertical scanning measurement target feature memory 50. To do.

  Here, the information on the inclination of the nose line W11 is obtained by correcting the moving direction of the moving body 1 so as to face the descending staircase KD2 when the moving body 1 actually descends the descending staircase KD2. It is used to be able to go.

  Subsequently, in step SP129, the vertical scanning feature amount extraction unit 45 determines the vertical lengths of the first and second step treads N1 and N2 based on the horizontal intervals of the nosing lines W11, W12. In step SP130, the height difference between the adjacent treads L0 and N1, N1 and N2, and N2 and L1 is obtained as the kick heights h1, h2 and h3 and stored in the vertical scanning measurement target feature memory 50. .

  Thus, the vertical scanning feature quantity extraction unit 45 ends the descending staircase feature quantity calculation processing procedure RT22 by the vertical scanning in step SP131.

  According to the above configuration, the external environment recognition device 9 is the position of the tread surface required for the leg wheel J3 of each leg to walk on the stairs KD2 from which the moving body 1 is going to descend from the floor L0 to the landing L1. The vertical length and the kick-up height can be reliably measured, and the measurement results can be stored in the vertical scanning measurement target feature memory 50.

(6-3) When the step exceeds the limit height In the case of FIG. 41, when the descending stairs KD2 are lowered by vertical scanning, the kick heights h1, h2, and h3 of each step are the same as the leg wheels J3 of the moving body 1. As shown in FIG. 43, the kicking height hX of the kicking surface QX from the floor surface L0 to the stepping surface NX1 that is one step below the limit height is exceeded. If so, the vertical scanning feature amount extraction unit 45 determines that the moving body 1 cannot descend without determining the nosing line in step SP122 of FIG. The processing is switched to the cliff line determination processing procedure RT23 to be performed.

(6-4) Calculation of cliff feature quantity by vertical scanning When the vertical scanning feature quantity extraction unit 45 vertically scans the vertical detection light DET from the sensor origin O along the vertical scanning line SN21 as shown in FIG. When the cliff surface S1 is in front of the floor surface R0 on which the moving body 1 is standing, “procedure calculation process procedure of cliff feature value by vertical scanning” RT23 of FIG. 45 is executed.

  When the calculation processing procedure RT23 is entered, the vertical scanning feature amount extraction unit 45 performs vertical scanning based on the intersecting feature amount data accumulated in the vertical scanning measurement target feature memory 50 by vertically scanning the cliff in step SP140. In step SP141, the plane feature amount obtained by scanning the floor surface R0 and the intersection feature TR0 obtained by scanning the floor surface R0 and the upper edge of the cliff surface S1 are included. The floor surface R0 is determined based on the cliff feature value by the intersection circle TR1.

  Subsequently, in step SP142, the vertical scanning feature quantity extraction unit 45 determines the cliff line W21 based on the boundary line between the floor surface R0 and the cliff surface S1.

  Subsequently, in step SP143, the vertical scanning feature amount extraction unit 45 determines whether or not the processing for all of the vertical direction feature lists based on the scanning line SN21 is completed, and when a negative result is obtained, the above-described processing is performed. Returning to step SP142, the determination process of the cliff line W21 is repeated.

  When a positive result is eventually obtained in step SP143, the vertical scanning feature amount extraction unit 45 moves to step SP144 and moves the moving body 1 based on the distance from the position where the moving body 1 is currently standing to the cliff line W21. The distance to the cliff surface S1 on the possible floor surface R0 is obtained.

  Subsequently, the vertical scanning feature quantity extraction unit 45 proceeds to the next step SP145, and obtains the inclination of the cliff surface S1 (ie, the inclination with respect to the vertical direction) based on the attitude of the cliff line W21.

  Thus, the vertical scanning feature quantity extraction unit 45 ends the cliff feature quantity calculation processing procedure in step SP146 by the vertical scanning.

  According to the above configuration, the external environment recognition device 9 can reliably recognize that there is a cliff that cannot be advanced in front of the moving body 1 by vertical scanning.

  In the case of FIG. 44, the case where the cliff surface S1 is measured has been described. However, even when the descending step described above with reference to FIG. 43 exceeds the limit height, the movement is the same as in the case of the cliff surface S1. In this case, the vertical scanning feature quantity extraction unit 45 does not allow the body 1 to go down the level difference, and the vertical level difference is high during the execution of the descending staircase feature quantity calculation processing procedure RT22 by vertical scanning shown in FIG. When it is determined that the value exceeds that, the calculation processing procedure RT22 is switched to the processing of the cliff feature amount calculation processing procedure RT23 by vertical scanning in FIG. 45, so that the descending may occur in steps SP144 and SP145. By detecting the distance to the descending step and the inclination thereof, it is possible to reliably measure the dangerous state existing in the front also in this case.

(6-5) Calculation of tread surface feature quantity by horizontal scanning As shown in FIG. 46, the CPU 13 receives a command for executing vertical scanning by the vertical scanning feature quantity extraction unit 45 by the user using the data input means 11. As a result of the vertical scanning performed on the vertical scanning line SN31 with respect to the staircase KD3, when the vertical scanning feature quantity extraction unit 45 extracts the tread surface X5, the horizontal scanning feature quantity extraction unit 46 performs the “stepping by horizontal scanning” of FIG. By executing the “surface feature amount calculation processing procedure” RT24, it is detected that the vertical wall surface X1 and the cliff X2 are adjacent to both sides or one side of the tread surface X5.

  In the case of this embodiment, the external environment recognition device 9 moves the tread surface X5 at a position higher than the kick surface X4 from the sensor origin O of the floor surface X3 on which the moving body 1 is currently standing by the height of the kick surface X4 in the left-right direction. The horizontal scanning is performed by the conical scanning detection light DET in the horizontal direction along the scanning line SN32.

  When the processing step RT24 for calculating the tread surface feature amount by the horizontal scanning is entered, the horizontal scanning feature amount extraction unit 46 first makes a reference step from the vertical scanning result accumulated in the vertical scanning measurement target feature memory 50 in step SP150. After setting the plane X5, the process proceeds to step SP151, and the conical scanning distance measuring device 31 performs scanning on the horizontal scanning line SN32, whereby the distance data memory 35, the intersecting feature amount calculating unit 41, and the intersecting feature amount. A cross feature list by horizontal scanning obtained via the memory 42 is set.

  Subsequently, the horizontal scanning feature quantity extraction unit 46 proceeds to Step SP152, and the kick line W31 is determined from the boundary line indicated by the concave coupling feature quantity of the intersection circle TRX with respect to the kick X6 between the tread surface X5 and the wall surface X1 on the left side. Are stored in the horizontal scanning measurement target feature memory 51.

  Subsequently, in step SP153, the horizontal scanning feature quantity extraction unit 46 detects that the plane feature quantity is a vertical plane from the data of the intersecting circle TRX on the wall surface X1 following the kick X6, and the position of the vertical plane is moved. Based on the fact that the body 1 has a feature sufficiently larger than the height at which the body 1 can climb, the wall line W32 is determined and stored in the horizontal scanning measurement target feature memory 51.

  Subsequently, in step SP154, the horizontal scanning feature quantity extraction unit 46 determines the nose X72 and its nosing line W33 based on the feature quantity of the boundary line of the convex joint discontinuity surface between the tread face X5 and the cliff X2 on the right side. At the same time, the process proceeds to the next step SP155, where the cliff line W34 is determined based on the cliff feature amount, and is stored in the horizontal scanning measurement target feature memory 51.

  The processing of steps SP152 to SP155 is repeated until the processing of all the intersecting circles TRX of the horizontal scanning line SN32 is completed by obtaining a negative result in step SP156.

  If an affirmative result is eventually obtained in step SP156, the horizontal scanning feature quantity extraction unit 46, in step SP157, steps on the plane interval in the horizontal scanning direction of the plane by the leg wheel J3 of the moving body 1 in step SP157. In step SP158, the boundary line on both sides in the left and right direction, that is, the horizontal interval between the wall line W32 and the cliff line W34 is determined as the step width X11, and the horizontal scanning measurement is performed. Accumulate in the target feature memory 51.

  Subsequently, in step SP159, the horizontal scanning feature quantity extraction unit 46, based on the presence of the wall surface X1 based on the wall line W32 determined in the above-described step SP153, cannot enter the movable body 1 through this area. And stored in the horizontal scanning measurement target feature memory 51.

  Thus, the horizontal scanning feature quantity extraction unit 46 ends the processing of the tread surface feature quantity calculation processing procedure RT24 by the horizontal scanning in step SP160.

  According to the above configuration, the external environment recognition device 9 can safely and reliably measure the step range and step width where the leg wheel J3 of the moving body 1 can stand on the tread surface X5 constituting the stair KD3. it can.

(6-6) Calculation of kick surface feature quantity by horizontal scanning As shown in FIG. 49, the external environment recognition device 9 uses the same staircase KD3 as described above with reference to FIG. By horizontally scanning the kick surface X4, which is a vertical surface continuing from the sensor origin O via the kick X11 with respect to the floor surface X3, the wall surface X1 and the cliff X2 are adjacent to both sides or one side of the kick surface X4. This is detected by causing the horizontal scanning feature quantity extraction unit 46 to execute “a calculation process procedure of the kick surface feature quantity by horizontal scanning” RT25 shown in FIG.

  When entering the calculation processing procedure RT25 of the kicking surface feature value by horizontal scanning, the horizontal scanning feature value extracting unit 46 first becomes the reference kicking surface from the vertical scanning result stored in the vertical scanning measurement target feature memory 50 in step SP170. After setting X4, the process proceeds to step SP171, and the conical scanning distance measuring unit 31 performs scanning on the horizontal scanning line SN33, thereby causing the distance data memory 35, the intersecting feature amount calculating unit 41, and the intersecting feature amount memory. An intersecting circle feature list by horizontal scanning obtained through the menu 42 is set.

  Subsequently, the horizontal scanning feature amount extraction unit 46 proceeds to step SP172, and the boundary line indicated by the concave coupling feature amount of the intersection circle TRY of the vertical wall surface X1 between the kick surface X4 and the wall surface X1 on the left side. From this, the wall line W22 is determined and stored in the horizontal scanning measurement target feature memory 51.

  Subsequently, in step SP173, the horizontal scanning feature quantity extraction unit 46 determines the vertical cliff line W34 based on the boundary line where the intersecting circle TRY scanning the edge of the cliff X2 extending in the vertical direction indicates the cliff feature quantity. Then, it is stored in the horizontal scanning measurement target feature memory 51.

  Thus, the horizontal scanning feature amount extraction unit 46 determines the wall line W22 and the cliff line W34 for the horizontal scanning line SN33, and continues to the above-described steps SP172 and SP173 until the processing for all the horizontal direction feature lists is completed in step SP174. Repeat the process.

  When it is determined in step SP174 that the processing of all the horizontal feature lists has been completed, the horizontal scanning feature amount extraction unit 46 determines in step SP175 the range X24 of the kick surface X4 from the horizontal interval of the plane feature values for the kick surface X4. And is stored in the horizontal scanning measurement target feature memory 51.

  Subsequently, in step SP176, the horizontal scanning feature quantity extraction unit 46 obtains the step width X25 of the kick surface X4 from the horizontal interval of the boundary line obtained in steps SP172 and SP173 described above, and obtains the horizontal scanning measurement target feature memory 51. To accumulate.

  Subsequently, in step SP177, the horizontal scanning feature amount extraction unit 46 determines that there is a wall surface X1 based on the wall line W22 measured in step SP172 described above, and sets this as an inaccessible region where the moving body 1 cannot enter. It is obtained and stored in the horizontal scanning measurement target feature memory 51.

  Thus, the horizontal scanning feature quantity extraction unit 46 ends the processing of the kick surface feature quantity calculation processing procedure RT25 by the horizontal scanning in step SP178.

  According to the above configuration, the external environment recognition device 9 is based on the kick surface X4 constituting the stairs KD3, the range of the kick surface X4 that the mobile body 1 may enter, and the wall surface X1 and the cliff X2 that should not enter. Can be measured safely and reliably.

(6-7) External world confirmation processing procedure by time series scanning The CPU 13 of the external environment recognition device 9 causes the time series scanning external world confirmation unit 47 to execute the “stepping confirmation processing procedure at the time of stair climbing” RT31 shown in FIG. Thus, the main body drive unit 15 of the moving body 1 is controlled to drive up the ascending staircase KDY shown in FIG. 53 by executing the “front leg stair climbing operation processing procedure” RT32 as shown in FIG. .

  The operation of the moving body in FIG. 53 is an operation in which the moving body 1 moves up the ascending stairs KDY having the first step surface (Y1), the second step surface (Y2), and the third step landing (Y3). As an example, the operation of landing the left front leg 4L and the leg wheel 3J of the right front leg 4R on the first step tread surface 1 (Y1) by the time series scanning external field confirmation unit 47 is illustrated.

  In the case of FIG. 53, as shown in FIG. 53 (A), the mobile body 1X is in front of the ascending staircase KDY and is stationary (both front legs are at the same height). The moving operation is started from the state in which the first step tread surface 1 (Y1) which is the surface is confirmed by the conical scanning detection light DET.

  First, as shown in FIG. 53 (B), when the moving body 1 collimates and confirms the planned position of the left front leg 4L with the conical scanning detection light DET, the left front leg 4L is not changed in position and orientation of the main body. It works like raising.

  Subsequently, as shown in FIG. 53 (C), the moving body 1 slightly raises the position and posture of the main body so that the leg wheel J3 of the left front leg 4L sufficiently exceeds the nose of the first step tread surface 1 (Y1). Raise the left front leg 4L while changing.

  At this time, since the position and orientation of the main body changes upward, the collimation of the left front leg 4L to the planned stepping location is shifted, and the cone scanning detection light DET is irradiated to the boundary line in front of the first stepped surface Y1. The position shifts.

  At this time, as shown in FIG. 53 (D), the moving environment detector 3 of the moving body 1 collimates the collimation direction of the conical scanning detection light DET to the planned landing place of the left front leg 4L for confirmation.

  After the confirmation, as shown in FIG. 53 (E), the moving body 1 causes the left front leg 4L to land on the planned landing location, but at this time, the position and orientation of the main body changes.

  In this state, the moving environment detector 3 of the moving body 1 collimates the conical scanning detection light DET at the planned position for the right front leg landing and confirms this.

  Subsequently, as shown in FIG. 53 (F), the moving environment detector 3 collimates the conical scanning detection light DET at the planned stepping position of the right front leg 4R and confirms this, and then the moving body 1 is the main body. The right front leg 4R is raised without changing the position and orientation.

  Subsequently, as shown in FIG. 53 (G), the moving body 1 changes the position and orientation of the main body slightly upward and further raises the right front leg 4R, whereby the leg wheel J3 of the right front leg 4R becomes the first step tread Y1. Operate to make sure to cross the nose.

  At this time, the collimation of the conical scanning detection light DET collimated at the planned landing position of the right front leg is shifted, and the boundary line ahead of the first step surface 1 (Y1) is irradiated.

  Subsequently, as shown in FIG. 53H, the moving environment detector 3 collimates the collimation direction of the conical scanning detection light DET and confirms the planned landing place of the right front leg 4R.

  After the confirmation, as shown in FIG. 53 (I), the moving body 1 causes the right front leg 4R to land on the planned landing location, and at this time, the position and orientation of the main body changes to irradiate the cone scanning detection light DET. become.

  When entering the step confirmation processing procedure RT31 when the stairs are rising, the time-series scanning external field confirmation unit 47 first reads the step information and the step information when the user inputs the data using the data input means 11 in step SP201.

  The state of FIG. 53 (I) is a state where the moving body 1 has landed the left and right front legs on the first step tread surface 1 (Y1), and the conical scanning detection light DET is the second step which is the front tread surface. By collimating the tread surface 2 (Y2), the tread surface 2 (Y2) is confirmed.

  Therefore, when the moving body 1 further moves up the ascending staircase KDY, the operations of FIGS. 53B to 53I are repeated for the second step surface 2 (Y2).

  Thus, the moving body 1 sequentially performs the operations of FIGS. 53A to 53I to land the left and right front legs 4L and 4R at the planned landing position of the first step surface 1 (Y1). Thus, it is possible to perform a movement operation such that both front legs are placed on the planned landing place.

  53 (A) to 53 (I), the time-series scanning external field confirmation unit 47 executes the “stepping confirmation processing procedure when the stairs rise” RT31 in FIG. 51 and responds thereto. As described above, the main body drive unit 15 drives the moving body 1 by executing the “front leg stair climbing operation procedure” RT32 of FIG.

  The information read by the time-series scanning external field confirmation unit 47 in step SP201 includes the structure of the ascending staircase KDY that the moving body 1 is going to rise and the left and right front legs 4L and 4R (and the left and right rear legs 5L) of the moving body 1. And 5R) have received the designation of the step to walk, and thus the time-series scanning external field confirmation unit 47 of the external recognition device 9 performs the left and right front legs 4L and 4R (and The moving body 1 is moved while confirming that the left and right rear legs 5L and 5R) are operated as specified.

  When the time-series scanning external field confirmation unit 47 acquires the staircase information in step SP201 in this way, the main body driving unit 15 enters the “front leg stair climbing process” RT32 in FIG. 52, and in step SP211 the ascending staircase KDY. Is in a standby state in front of (FIG. 53A).

  In this state, the time-series scanning external field confirmation unit 47 moves to step SP202, and according to the input operation from the user by the data input means 11, the cone half-vertical angle of the cone scanning detection light DET for measuring the cone scanning distance. Is set, and the collimation direction Z of the conical scanning detection light DET is set to the expected landing position of the left front leg 4L (FIG. 53B).

  Subsequently, the time-series scanning external field confirmation unit 47 measures the conical scanning distance based on the return light from the intersecting circle TR of the conical scanning detection light DET in step SP203, and extracts the intersecting feature amount in step SP204. Thereafter, in step SP205, the step recognition is executed.

  In step SP205, the platform is recognized by irradiating the cone scanning detection light DET as the platform information about the planned platform (FIG. 53B) where the moving body 1 is to land the left front leg 4L. Recognize the position, posture and range of the tread surface 1 (Y1).

  Subsequently, the time-series scanning external field confirmation unit 47 moves to the next step SP206, determines whether or not the boundary line is detected in the step information recognized in step SP205, and when a negative result is obtained, This means that the conical scanning detection light DET correctly collimates the tread surface, so that the process proceeds to step SP207 to notify the main body drive unit 15 of the moving body 1 of the step confirmation information.

  In the case of this embodiment, the information acquisition processing in the above-described steps SP202 to SP206 is performed based on the processing operations of the vertical scanning feature quantity extraction unit 45 and the horizontal scanning feature quantity extraction unit 46, and the horizontal scanning measurement target feature memory 50 and the horizontal scanning feature quantity memory 50. The process is executed based on the information stored in the scanning measurement target feature memory 51, and the processing result of the time-series scanning external world confirmation unit 47 is thus stored in the external world confirmation data memory 52.

  In parallel with the confirmation information acquisition process of the time-series scanning external field confirmation unit 47, the main body drive unit 15 of the moving body 1 performs step SP212 of the left front leg raising operation subroutine RT32A of the front leg stair raising operation processing procedure RT32. A step confirmation request for the left front leg 4L is transmitted to the time-series scanning external field confirmation unit 47, and the state waits for a notification of the step confirmation information (step SP207) from the time-series scanning external field confirmation unit 47 in step SP213. Become.

  Thus, the main body drive unit 15 is waiting for confirmation of the footstep in the state where the conical scanning detection light DET collimates the footstep of the left front leg 4L in FIG. When the series scanning external environment confirmation unit 47 reports the step confirmation information in step SP207, the main body drive unit 15 moves to the next step SP214 when a positive result is obtained in step SP213.

  In this step SP214, the main body drive unit 15 moves up the left front leg 4L of the moving body 1, and then moves to step SP215 to wait for confirmation of the step.

  First, the left front leg is lifted without changing the position and orientation of the main body as described above with reference to FIG. 53B, and then the position and posture of the main body are slightly changed as described above with reference to FIG. It includes an operation of further raising the left front leg 4L by changing upward.

  Therefore, as shown in FIG. 53C, the position and orientation of the main body is changed, so that the collimation direction of the cone scanning detection light DET is shifted forward and shifted to the position where the nose of the second step 2 (Y2) is irradiated. Then, the time-series scanning external world confirmation unit 47 obtains a positive result in step SP206 by detecting the boundary line of the nose and returns to step SP202 described above.

  At this time, the time-series scanning external field confirmation unit 47 performs a resetting operation for returning the conical scanning detection light DET to the stepping position of the left front leg 4L and the specified position, and then repeats the loop processing of steps SP203 to SP206.

  At this time, the time-series scanning external field confirmation unit 47 corrects the collimation direction for the tread surface 1 (Y1) that does not detect the boundary line with the conical scanning detection light DET (FIG. 53D), so a negative result is obtained in step SP206. In step SP207, the platform confirmation information is reported to the main body drive unit 15, and the platform is confirmed in step SP208. If a negative result is obtained, the process returns to step SP203, and steps SP203 to SP208 are performed. Repeat the process.

  At this time, the main body driving unit 15 responds to the notification from the time-series scanning external field confirmation unit 47, determines that the stepping area has been confirmed in step SP215, moves to step SP216, and moves the main body of the moving body 1 downward. The left front leg is landed on the tread surface 1 (Y1) by changing the position and orientation to (FIG. 53E).

  Thus, the main body drive unit 15 terminates the step confirmation process of the left front leg 4L in the next step SP217 by landing the left front leg 4L at the designated step position of the tread surface 1 (Y1). The processing of the left front leg raising operation subroutine RT32A is terminated, and then the processing of the right front leg raising operation subroutine RT32B is entered.

  When entering the right front leg raising operation subroutine RT32B, the main body drive unit 15 first sends a step confirmation request for the right front leg 4R to the time-series scanning outside world confirmation unit 47 in step SP218.

  At this time, the time-series scanning external field confirmation unit 47 returns to step SP202 based on the fact that the confirmation of all the landings is not completed in step SP210, and performs the cone scanning distance measurement for confirmation of the landing of the right front leg 4R. The cone half apex angle and the collimation direction are set, and the cone scanning distance measurement operation is performed in step SP203 on the tread surface 1 (Y1) that is the step of the right front leg 4R. The extraction process is performed, and the step recognition process is performed in step SP205 to recognize the position of the right front leg 4R, the posture of the tread surface 1 (Y1), and the tread surface range as the step information.

  Subsequently, the time-series scanning external field confirmation unit 47 determines whether or not a boundary line has been detected in the next step SP206. The conical scanning detection light DET set at this time is on the plane of the tread surface 1 (Y1). Since a negative result is obtained because the cross circle is irradiated, a notification of the step confirmation information is sent to the main body drive unit 15 in step SP207.

  At this time, the main body drive unit 15 moves to step SP220 and raises the right front leg 4R by obtaining the step information in step SP219.

  At this time, the main body drive unit 15 raises the right front leg 4R without changing the position and orientation of the main body as described above with reference to FIG. 53 (F), and then moves the position and posture of the main body upward as described above with reference to FIG. 53 (G). The right front leg 4R is further raised in the state changed to.

  At this time, the collimation direction of the conical scanning detection light DET is shifted forward from the planned landing location of the right front leg 4R, and the nose of the second step surface 2 (Y2) is irradiated.

  At this time, since the boundary line about the nose line of the second step tread surface 2 (Y2) is extracted as the step information, the time-series scan outside world confirmation unit 47 returns to the above-described step SP202 and performs the cone scan. The collimation direction of the detection light DET is reset.

  The time-series scanning external field confirmation unit 47 performs conical scanning distance measurement in step SP203 for the reset collimation direction, and after extracting the intersection feature amount in step SP204, performs the step recognition process in step SP205. Do.

  At this time, since the conical scanning detection light DET irradiates the flat surface of the tread surface 2 (Y2), the time-series scanning external field confirmation unit 47 obtains a negative result at step SP206, thereby reporting step confirmation information at step SP207. Is sent to the main body drive unit 15.

  Further, in step SP221, the main body drive unit 15 was able to collimate the expected landing position of the right front leg 4R in a state where the collimation direction of the conical scanning detection light DET was corrected as described above with reference to FIG. If this can be confirmed, the process moves to step SP222, and the right front leg is landed on the tread surface 1 (Y1) while changing the position and orientation of the main body as described above with reference to FIG.

  Thus, the main body drive unit 15 ends the process of the front leg stair lift operation processing procedure RT32 in step SP224 when the process of the right front leg lift operation subroutine RT32B is completed.

  At the same time, the time-series scanning external field confirmation unit 47 ends the confirmation of the step for the right front leg raising operation subroutine RT32B of the main body drive unit 15 in step SP208 and all the left front leg 4L and the right front leg 4R in step SP209. When the step confirmation process is finished, the step confirmation process procedure RT31 when the stairs are raised is finished in step SP210.

  In the configurations of FIGS. 51 to 53, the time-series scanning external field confirmation unit 47 irradiates based on this intersection feature amount based on the conical scanning detection light DET projected from the moving environment detector 3 of the moving body 1. By sequentially detecting the steps of the ascending stairs as time passes, the main body drive unit 15 is driven and controlled while confirming the operation until the front legs of the moving body 1 reach the landing specified by the user. Thus, even if the position and posture of the moving body 1 change over time, the moving body 1 can be safely and reliably placed by sequentially landing the front legs on the planned landing in response to the change. Can be made to go up the staircase KDY.

(7) Other Embodiments (7-1) In the above embodiment, the leg wheel J3 of the moving body 1 is described as moving the moving body 1 while confirming the tread surface which is a plane. The shape of the tread surface is not limited to this, and may be a cylindrical surface or a spherical surface. In this case, if a feature amount that can be safely matched is set, the same effect as in the above case can be obtained.

(7-2) In the embodiment of FIGS. 51 to 53, the staircase ascending operation of the front leg of the moving body 1 has been described. Similarly, the ascending operation of the rear leg also moves with the time-series scanning external field confirmation unit 47. The corresponding operation with the drive unit 15 can be performed safely and reliably.

  Further, even when going down the stairs, the moving body 1 can be lowered safely and reliably in the same manner by making the feature extraction method correspond to the down stairs.

(7-3) In the above embodiment, the processing target data (α−L) is obtained and recognized using the distance data (α, L) stored in the conical scanning distance data memory 35A of the distance data memory 35. Although the case where the target feature amount calculation processing is described has been described, the feature amount calculation processing is similarly performed when the image data (i, j, L) of the distance image data memory 36B is used instead. Can do.

  The present invention can be used when a moving body is moved while being detected by conical scanning detection light.

  DESCRIPTION OF SYMBOLS 1 ... Mobile body, 2 ... Mobile body main body, 3 ... Mobile environment detector, 4L, 4R ... Left and right front leg, 5L, 5R ... Left and right rear leg, 6 ... Floor surface, 7 ... ... staircase, 8 ... tread surface, 8A ... nose, 8B ... flat surface, 8C ... kick-in, 9 ... external recognition device, 10 ... cliff, 11 ... data input means, 12 ... bus, 13 ... ... central processing unit for system control, 14 ... program ROM / operation RAM, 15 ... main body drive unit, 19 ... kick-in surface, 21 ... horizontal scanning mechanism, 25 ... vertical scanning mechanism, 27 ... distance measuring instrument , 31... Cone scanning distance measuring unit, 31 A... Three-dimensional distance data memory (θ, φ, L), 32... Cone scanning control unit, 33. 35: Distance data memory, 35A: Conical scanning distance data memory (α, L), 36: Distance Image measuring unit, 36B ... Distance image data memory (i, j, L), KD1 ... Upstairs, E1, E2 ... Kick, F1, F2 ... Kick surface, G1, G2 ... Nose, TR, TR0, TR1 to TR3: cross circle, KD2: descending staircase, L0: floor surface, L1: landing, M1, M2 ... step nose, N1, N2 ... tread surface, 41 ... cross circle feature value calculation unit, 42... Intersection feature amount memory, 45... Vertical scanning feature amount extraction unit, 46... Horizontal scanning feature amount extraction unit, 47 .. time series scanning external field confirmation unit, 50. …… Horizontal scan measurement target feature memory, 52 …… External world confirmation data memory.

Claims (6)

The cone scanning detection light that scans in the direction along the conical surface with the collimation direction as the center is projected from the sensor origin to the measurement target, and reflected from the intersection of the cone scanning detection light and the surface of the measurement target. A moving environment detection means for receiving the conical scanning detection light and measuring the distance from the sensor origin to the measurement object;
Feature amount calculating means for calculating the feature value of the intersecting circle based on the distance measurement data measured by the moving environment detecting means;
A moving environment recognition apparatus comprising: shape determining means for determining the shape of the measurement target surface from the feature value of the intersecting circle. A distance image data memory for storing the distance measurement data measured by the moving environment detection unit in correspondence with the cone scanning rotation angle of the cone scanning detection light; and the feature amount calculation unit includes the cone scanning rotation angle. The mobile environment recognition device according to claim 1, wherein the feature amount is determined based on a method of changing the distance measurement data with respect to a rotation direction. The moving environment detection means scans the collimation direction of the cone scanning detection light, and the shape determination means obtains the shape of the measurement target surface according to a change caused in the distance measurement data by the scanning. The mobile environment recognition device according to claim 1, wherein The moving environment detecting means scans the collimated scanning detection light in a direction perpendicular to the measurement object, and the feature quantity calculating means detects the planar feature quantity when the feature quantity calculating means detects the planar feature quantity. The shape determining means obtains a boundary line of the measurement target portion by causing the collimated scanning detection light to scan the collimated scanning detection light in a horizontal direction for the detected measurement target portion. The mobile environment recognition device described. The cone scanning detection light that scans in the direction along the conical surface with the collimation direction as the center is projected from the sensor origin to the measurement target, and reflected from the intersection of the cone scanning detection light and the surface of the measurement target. A moving environment detection means for receiving the conical scanning detection light and measuring the distance from the sensor origin to the measurement object;
Feature amount calculating means for calculating the feature value of the intersecting circle based on the distance measurement data measured by the moving environment detecting means;
A moving environment recognition method comprising: shape determining means for determining the shape of the measurement target surface from the feature value of the intersecting circle. The cone scanning detection light that scans in the direction along the conical surface with the collimation direction as the center is projected from the sensor origin to the measurement target, and reflected from the intersection of the cone scanning detection light and the surface of the measurement target. A moving environment detection means for receiving the conical scanning detection light and measuring the distance from the sensor origin to the measurement object;
Feature amount calculating means for calculating the feature value of the intersecting circle based on the distance measurement data measured by the moving environment detecting means;
A moving body comprising: shape determining means for determining the shape of the surface to be measured based on the feature value of the intersecting circle, and controlling the movement operation based on the determination result of the shape determining means.

Images