Patent Description:
One form of a mobile body disclosed in <CIT> has been known. The controlling of a mobile body disclosed in <FIG> of <CIT> includes a step for obtaining a road surface data which includes at least a height data obtained by a road surface data obtaining portion, a step for rearranging the obtained road surface data in order of height, a step for extracting change point of the rearranged road surface data and a step for recognizing an obstacle from the change point as a border. Thus, obstacles can be accurately recognized.

Further, another form of the mobile body has been known which is shown in <CIT>. The mobile body disclosed in <FIG> of <CIT> includes ,a distance sensor <NUM> which measures the distance between the floor surface in front of a movable carriage <NUM> and the main body of the mobile body and a moving amount measuring portion <NUM> provided on the movable carriage <NUM>, wherein when a deviation between the measuring distance between the front floor surface <NUM> in front of the movable carriage <NUM> and movable carriage <NUM> and the predetermined set value continuously exceeds a reference amount, the moving distance after the deviation has exceeded the reference amount is measured by the moving amount measuring portion <NUM> and when such moving distance has exceeded a reference moving amount, the movable carriage <NUM> is stopped by a judgment that the mobile body is in an un-travelable state. Therefore, when an inclination or step is detected at the front side of the travelling course, the height or the width of the step or a degree of inclination can be recognized to prevent an unnecessary stop of the mobile body. The mobile body continues to travel on a low step or a narrow ditch or groove, judging that such situation is an obstacle that can be cleared.

<CIT> discloses a cleaning robot comprising a distance image sensor and a plane detection device.

According to the mobile body disclosed in <CIT>, an accurate recognition of the step or inclination of a road surface on which the mobile body is travelling has been demanded. Further, according to the disclosure of <CIT>, the detection of step or inclination of the road surface is made on the assumption that the mobile body is travelling. Accordingly, when the road surface on which the mobile body is travelling (point where the mobile body is currently travelling) is a flat surface, the travelling road surface (road surface in a direction in which the mobile body is travelling) can be detectible. However, when the road surface on which the mobile body is travelling is not flat (for example, uphill surface or downhill surface or uneven surface), the step or inclination of the road surface cannot accurately be detected.

Accordingly, this invention was made in consideration with the above-mentioned situation and the objective of the invention is to provide a mobile body which can accurately recognize the step or inclination of the road surface by suppressing the influence from the profile of the road surface on which the mobile body travels.

In order to solve the above problems, a mobile body according to claim <NUM> is provided.

A mobile body according to an aspect is characterized in that the mobile body, which travels driven by a driving device, includes an obtaining portion which obtains information of surrounding of the mobile body as a three-dimensionally indicated point group data, a dividing portion which divides the point group data obtained by the obtaining portion into a plurality of sections which is a plurality of predetermined areas defined on a predetermined plane, a feature amount calculating portion which calculates a feature amount which is associated with a feature of one of the plurality of sections from a divided point group data which is the point group data belonging to the one of the plurality of sections per every section divided by the dividing portion, a search starting point setting portion which sets a search starting point which serves as a starting point for searching whether there is a travelable section in which the mobile body is travelable among the plurality of sections based on the feature amount calculated by the feature amount calculating portion, and a judging portion which judges a resembling section which includes a feature amount resembling the feature amount associated with the search starting point from among a surrounding section, which is one of the plurality of sections surrounding the search starting point set by the search starting point setting portion.

According to the aspect described above, a resembling section can be judged based on the feature amount calculated per every section, referencing the search starting point among the sections surrounding the search starting point which has been set previously. In other words, the travelable section of the movable body can be judged based on the search starting point as a reference point. As a result, a mobile body which more accurately recognizes a step or an inclination of a road surface on which the mobile body travels, by suppressing an influence from a shape of the road surface on which the mobile body travels can be proposed.

First embodiment of the mobile body will be explained hereinafter with reference to the attached drawings. As an example of the mobile body according to the embodiment, an electric wheel chair <NUM> shown in <FIG> will be explained. It is noted that for the purpose of illustration, the explanation will be made assuming that the top side and bottom side of <FIG> indicate the upper side and lower side of the electric wheel chair <NUM>. Similarly, the bottom left side and the top right side of <FIG> indicate the front side and rear side of the electric wheel chair <NUM> and the top left side and the bottom right side of <FIG> indicate the right side and the left side of the electric wheel chair <NUM>, respectively. The arrows in <FIG> indicate respective directions.

The electric wheel chair <NUM> includes a wheel chair main body <NUM>, a driving device <NUM>, an operating device <NUM>, a detecting device <NUM> and a control device <NUM>. The electric wheel chair <NUM> corresponds to the mobile body which travels, driven by the driving device <NUM> which is driven based on the input to the operating device <NUM> by an occupant of the mobile body. The driving device <NUM>, the operating device <NUM>, the detecting device <NUM> and the control device <NUM> are installed on the wheel chair main body <NUM>. It is noted here that the mobile body is not limited to the one which is operated by an operator on board, another type, such as, an autonomous mobile body on which no operator is seated for operation can be applicable to the teachings.

The wheel chair main body <NUM> includes a frame <NUM>, a seat <NUM> on which the occupant is seated and wheels <NUM>. The seat <NUM> and the wheels <NUM> are assembled to the frame <NUM>. The wheels <NUM> are structured to be rotatable about respective rotation axes. The wheels <NUM> are arranged at the right side and the left side of the wheel chair main body <NUM> and includes a left drive wheel 13a and a right drive wheel 13b which are driven by the driving device <NUM> and a left auxiliary wheel 13c and a right auxiliary wheel 13d for auxiliary supporting the traveling of the electric wheel chair <NUM>.

The driving device <NUM> drives the electric wheel chair <NUM> by rotatably driving the respective drive wheels 13a and 13b. The driving device <NUM> is, for example, structured by a combination of an electric motor (not shown) and reduction gear mechanism (not shown). The driving device <NUM> is provided at respective drive wheels 13a and 13b (total number of the driving device <NUM> is two).

The operating device <NUM> is operated by the occupant of the electric wheel chair <NUM> to give instructions regarding to the straight traveling speed "v" and the turning speed "w" of the electric wheel chair <NUM>. The straight traveling speed "v" is a speed of the electric wheel chair <NUM> traveling in a forward direction (front direction) with respect to the electric wheel chair <NUM>. The turning speed "w" is an angular speed of the electric wheel chair <NUM> turning around the center of gravity thereof, as a turning center. In this embodiment, a joystick is used as the operating device <NUM>. The position that the operating device <NUM> is erected upright in a vertical direction is determined to be the non-operated position (hereinafter, referred to as "neutral position"). The operating device <NUM> is operated by the occupant of the wheel chair <NUM> by inclining the device <NUM> from the neutral position to an angular position. The operated state of the operating device <NUM> is indicated by the coordinates of the tip end of the operating device <NUM>, when the operating device <NUM> is projected onto the X-Y plane which is in parallel with the horizontal plane as shown in <FIG>. The X-axis is the same direction with the front/rear direction of the electric wheel chair <NUM> and positive direction of the X-axis is the same with the front direction of the electric wheel chair <NUM>. The Y-axis is the same direction with the right/left direction of the electric wheel chair <NUM> and the positive direction of the Y-axis is the same with the right direction of the electric wheel chair <NUM>. The value of the X-coordinates indicates a desired straight traveling speed "xjs" of the electric wheel chair <NUM> corresponding to the straight traveling speed desired by the occupant of the wheel chair <NUM>. The value of the Y-coordinates indicates a desired turning speed "yjs" of the electric wheel chair <NUM> corresponding to the turning speed desired by the occupant of the wheel chair <NUM>. The values of the desired straight traveling speed "xjs" and the desired turning speed yjs" are outputted to the control device <NUM>, every first predetermined time as the operation information inputted to the operating device <NUM>. The first predetermined time is set to be, for example, <NUM> / <NUM> second.

The detecting device <NUM> is a device which detects a detecting object located around the electric wheel chair <NUM>. The detecting object includes a road surface on which the electric wheel chair <NUM> is to travel from now, a human, and obstacles, etc.
The detecting device <NUM> is formed by a 3D (three dimensional) measurement section sensor (laser range scanner (3D scanner)). The detecting device <NUM> receives the reflection wave from an detected object at the detecting portion <NUM> by emitting laser in a horizontal direction and up/down direction (three dimensionally) from the detecting portion <NUM>. Thus, the detecting device <NUM> obtains the straight distance from the detecting portion <NUM> to the detected object, horizontal angle (with respect to a reference (for example, to the straight forward direction of the electric wheel chair <NUM>)) and a vertical angle (with respect to a reference (for example, relative to the horizontal direction of the electric wheel chair <NUM>)). Eventually, the detecting device <NUM> can obtain the three-dimensional coordinates associated with the detected object as a point group data D. The surrounding information (detected object information) is shown as a point group data D which is the three dimensional coordinates. The point group data D represents the position and shape of the surface of the detected object. The detecting device <NUM> irradiates the laser radially to the surrounding (in this embodiment, front side) of the electric wheel chair <NUM> in a radial direction. The irradiation possible angular range of the laser corresponds to the angular range that the detecting device <NUM> can detect the detecting object. The detecting device <NUM> obtains for example, the surrounding information every first predetermined time. The surrounding information obtained by the detecting device <NUM> is outputted to the control device <NUM>.

The control device <NUM> drives the electric wheel chair <NUM> to travel by controlling the driving amount of the driving device <NUM> based on the operation information. The driving device <NUM>, the operating device <NUM> and the detecting device <NUM> are connected to the control device <NUM> as shown in <FIG>. The control device <NUM> includes an operation information obtaining portion <NUM>, a driving control portion <NUM>, a surrounding information obtaining portion <NUM>, a dividing portion <NUM>, a feature amount calculating portion <NUM>, a search starting point setting portion <NUM> and a judging portion <NUM>.

The operation information obtaining portion <NUM> obtains the operation information from the operating device <NUM>. The driving control portion <NUM> controls the driving device <NUM> to control the travelling of the electric wheel chair <NUM> based on the operation information obtained by the operation information obtaining portion <NUM>. In more detail, the control device <NUM> starts travelling control from the time when the operating device <NUM> is operated and the control device <NUM> obtains the operation information from the operating device <NUM>. The control device <NUM> converts the operation information (desired straight traveling speed "xjs" and desired turning speed "yjs") from the operating device <NUM> into the straight traveling speed "v" and the turning speed "w". The control device <NUM> calculates the straight travelling speed "v" from the obtained desired straight travelling speed "xjs", based on the first map M1 shown in <FIG>. The first map M1 indicates the relationship between the desired straight traveling speed "xjs" and the straight traveling speed "v". The control device <NUM> calculates the turning speed "w" from the obtained desired turning speed "yjs", based on the second map M2 shown in <FIG>. The second map M2 indicates the relationship between the desired turning speed "yjs" and the turning speed "w".

The first map M1 includes proportional portions mv1 in which the desired straight traveling speed "xjs" and the straight traveling speed "v" are proportional and dead zone portions mv2 in which the straight traveling speed "v" is constant regardless of the magnitude of the desired straight traveling speed "xjs" as shown in <FIG>. When the straight traveling speed "v" is a positive value, the electric wheel chair <NUM> moves forward. On the other hand, when the straight traveling speed "v" is a negative value, the electric wheel chair <NUM> moves rearward. Further, the second map M2 includes proportional portions mw1 in which the desired turning speed "yjs" and the turning speed "w" are proportional and dead zone portions mw2 in which the turning speed "w" is constant regardless of the magnitude of the desired turning speed "yjs" as shown in <FIG>. When the turning speed "w" is a positive value, the electric wheel chair <NUM> turns in the right direction. On the other hand, when the turning speed "w" is a negative value, the electric wheel chair <NUM> turns in the left direction.

The control device <NUM> controls the driving amount (number of rotation) of the driving device <NUM> based on the converted values of the straight traveling speed "v" and the turning speed "w". In detail, the converted values of the straight traveling speed "v" and the turning speed "w" are further converted into the rotation speed of the left drive wheel 13a and the rotation speed of the right drive wheel 13b. The magnitude of the straight traveling speed "v" is proportional to the magnitude of the rotation speed of the left drive wheel 13a and the magnitude of the rotation speed of the right drive wheel 13b. Further, the magnitude of the turning speed "w" is proportional to the magnitude of the difference in rotation speed between the left drive wheel 13a and the right drive wheel 13b. The relationship between the straight traveling speed "v" and the turning speed "w" and the rotation speed of the left drive wheel 13a and the right drive wheel 13b is obtained in advance by calculation through the experimental work or the like. It is noted that since the driving device <NUM> is PWM (Pulse Width Modulation)-controlled, the control instruction value of the driving device <NUM> is calculated based on the duty ratio (cycle).

While the control device <NUM> is performing the travelling control, if the occupant of the electric wheel chair <NUM> moves the position of the operating device <NUM> to the neutral position, the straight travelling speed "v" and the turning speed "w" become zero and the electric wheel chair <NUM> stops. Then the control device <NUM> finishes the travelling control.

The surrounding information obtaining portion <NUM> is an obtaining portion which obtains the information around the electric wheel chair <NUM> as the three-dimensionally indicated point group data from the detecting device <NUM>. The dividing portion <NUM> divides the point group data D obtained by the surrounding information obtaining portion <NUM> into a plurality of sections G which is a plurality of predetermined areas defined on a predetermined plane. According to this embodiment, the plurality of predetermined areas is defined on a circle coordinates C (plane polar coordinates) as shown in <FIG>. The circle coordinates C is provided on the predetermined plane and is a polar coordinates having the position of the detecting portion <NUM> of the detecting device <NUM> as the origin C0 and the upper side in <FIG> being the front direction of the electric wheel chair <NUM>. The circle coordinates C includes the plurality of sections G which is provided with a predetermined interval spaced from one another in a radial direction and a predetermined degree separated from one another in a circumferential direction (for example, <NUM> in a radial direction and <NUM> degrees in a circumferential direction). The predetermined plane is set to be in parallel with the horizontal plane when the electric wheelchair <NUM> is travelling on a road with the horizontal plane surface and is defined to be the reference plane including the detecting portion <NUM>.

It is noted here that the origin C0 of the circle coordinates C may be set to the center of gravity of the electric wheel chair <NUM> instead of setting at the position of the detecting portion <NUM> of the detecting device <NUM>. Further, the rows and columns of the divided sections Gnm are defined such that a column (row) closest to the detecting portion <NUM> in a radial direction is assumed to be the first row and a column at the left side in a circumferential direction is assumed to be the first column. The symbol "n" indicates the row number, whereas the symbol "m" indicates the column number. The section G11 means the section defined by the first row and the first column. The section G17 means the section defined by the first row and the seventh column. The section G21 means the section defined by the second row and the first column and the section G27 means the section defined by the second row and the seventh column.

The dividing portion <NUM> divides the point group data D obtained by the surrounding information obtaining portion <NUM> into the plurality of sections G as described above. In more detail, the dividing portion <NUM> divides the point group data D into the plurality of sections G by projecting the point group data D on the circle coordinates C. The reference position of the point group data D is the position of the detecting portion <NUM> and accordingly, by agreeing the reference position to the origin C0 of the circle coordinates C, the point group data D can be projected on the circle coordinates C by the dividing portion <NUM>. It is noted that the dividing portion <NUM> can divide the point group data D by using the plane orthogonal coordinates (i.e., using grids), instead of using the circle coordinates C.

The feature amount calculating portion <NUM> calculates the feature amount associated with the feature of the section Gnm from the divided point group data Dnm which is the point group data D that belongs to the section Gnm. This calculation is made per every divided section Gnm divided by the dividing portion <NUM>. In more detail, the feature amount calculating portion <NUM> includes a merging portion 55a, a presumed plane setting portion 55b, an identifying portion 55c, a normal vector calculating portion 55d, an inclination calculating portion 55e and a height calculating portion 55f.

The merging portion 55a merges the section Gnm in which the number of divided point group data Dnm is relatively small and a neighboring section Gnm which is located adjacent thereto thereby forming a new section which is a merged section newly formed by merging. For example, the section G37 in which the number of divided point group data is small merges the neighboring section G36 which is located adjacent to the section G37 to form a merged new section. This merged new section becomes the section subject to further processing or treatment. By this merging, the number of vacant sections in which the number of point group data is relatively small can be reduced.

The presumed plane setting portion 55b sets the presumed plane PI from the divided point group data Dnm associated with the section Gnm per every section Gnm (See <FIG>). The presumed plane PI is a flat plane and can be set by conducting RANSAC method or Moving Least Squares method on the divided point group data "Dab".

The identifying portion 55c identifies the effective point group data D and the outlier values from the divided point group data Dnm based on the presumed plane PI set by the presumed plane setting portion 55b. In more detail, the identifying portion 55c identifies a dot located at a position where the distance shortest from the presumed plane PI is smaller than a distance threshold value, as an effective data (See black dots in <FIG>) and identifies a white circle located at a position where the distance shortest from the presumed plane PI is larger than the distance threshold value as an outlier value (See white circle in <FIG>). It is noted that the divided point group data Dnm in <FIG> indicates a certain divided point group data Dab.

The normal vector calculating portion 55d calculates the normal vector of the section Gnm from the effective point group data D identified by the identifying portion 55c. In more detail, the normal vector calculating portion 55d calculates the point of center of gravity (or the median) to the identified point group data D (black dots indicated in <FIG>) and then the normal vector calculating portion 55d calculates the covariance matrix with the effective point group data D relative to the calculated point of center of gravity (shown with the white circle) to thereby conduct a principal component analysis. The normal vector calculating portion 55d sets the minimum principal component as the normal vector of the presumed plane PI. It is noted that it is preferable to calculate the covariance matrix after weighting the covariance of the center of gravity and the point group in response to the distance therebetween. Then, the influence by the noise can be minimized and the normal vector can be more accurately calculated.

The inclination calculating portion 55e calculates the inclination (angle relative to the horizon plane) which is one of the feature amounts of the section Gnm from the normal vector calculated by the normal vector calculating portion 55d. Since the direction of the normal vector and the plane surface of the section Gnm are in an orthogonal relation, the inclination of the section Gnm can be calculated from the direction of the normal vector. In more detail, the inclination calculating portion 55e calculates the inclination of the section Gnm by the inner product of the vertical upward direction vector and normal vector. The inclination becomes positive value when the inclination inclines to the near side of the travelling direction (upward inclination) relative to the horizon plane and becomes a negative value when the inclination inclines far side of the travelling direction (downward inclination) relative to the horizon plane.

The height calculating portion 55f calculates the height which is one of the feature amounts of the section Gnm based on the effective point group data D which is identified by the identifying portion 55c. In detail, the height calculating portion 55f calculates for example, the mean value or the median of the identified effective point group data D as the height of the section Gnm. The height value becomes a positive value when the height is higher than the reference plane and becomes a negative value when the height is lower than the reference plane. It is noted that the reference plane is for example, set to be a horizontal plane and is the plane on which the wheels of the electric wheel chair <NUM> are ground-contacted under a parallel (horizontal) state.

The search starting point setting portion <NUM> sets the search starting point which is the starting point for searching a travelable section, where the electric wheel chair <NUM> can travel, from the plurality of sections based on the feature amount calculated by the feature amount calculating portion <NUM>. In detail, the search starting point setting portion <NUM> sets a section as a search starting point where the section is positioned within a predetermined distance from the present position of the electric wheel chair <NUM>, the height that belongs to the feature amount calculated by the feature amount calculating portion <NUM> is within a height judging range and further the inclination which belongs to the feature amount calculated by the feature amount calculating portion <NUM> is within an inclination detectible range from the plurality of sections G. It is noted that the height judging range is set to be equal to or more than the first height threshold value and at the same time is set to be equal to or less than the second height threshold value which is larger than the first height threshold value. The first height threshold value is, for example, set to correspond to the depth of the road (recessed step) where the electric wheel chair <NUM> can travel over and is indicated as a negative value. The second height threshold value is, for example, set to correspond to the height of road (projecting step) where the electric wheel chair <NUM> can travel over and is indicated as a positive value. It is further noted that the inclination detectible range is set to be equal to or more than the first inclination threshold value and at the same time is set to be equal to or less than the second inclination threshold value which is larger than the first inclination threshold value. Further, the inclination judging range is set to be equal to or more than the first inclination threshold value and at the same time is set to be equal to or less than the second inclination threshold value which is larger than the first inclination threshold value. The first inclination threshold value is, for example, set to correspond to the angle of the downhill road where the electric wheel chair <NUM> can travel over and is indicated as a negative value. The second inclination threshold value is, for example, set to correspond to the angle of the uphill road where the electric wheel chair <NUM> can travel over and is indicated as a positive value. For example, it is preferable that the search starting point setting portion <NUM> sets a section as a search starting point where the section is positioned within a predetermined distance from the present position of the electric wheel chair <NUM>, the height that belongs to the feature amount calculated by the feature amount calculating portion <NUM> is equal to or less than the second height threshold value (and equal to or more than zero) and further the inclination which belongs to the feature amount calculated by the feature amount calculating portion <NUM> is equal to or less than the second inclination threshold value (and equal to or more than zero).

The height threshold value explained above is set in response to the inclination calculated by the feature amount calculating portion <NUM>. In detail, the height threshold value Hth can be calculated as a value corresponding to the inclination An1 (road surface angle) of the section G from the map shown in <FIG> or the following mathematic formula <NUM> (M1). <MAT> wherein "α" is a coefficient which is used for converting the angle of the road surface into the height (height threshold value Hth) and is a constant specific to the electric wheel chair <NUM>. The "Hoffset" is an offset term and is set to the upper limit value of the step on which the electric wheel <NUM> can travel over (this value determines the travelable range when the angle of the road surface is in a vertically upward direction).

According to this structure, the height threshold value which detects the search starting point can be variable depending on the inclination (angle) of the section (road surface). As a result, as long as the inclination is within the travelable range, even the step has the height higher than the travelable height, the section can be set as the travelable search starting point. Therefore, the travelable section can be correctly detected (judged).

The judging portion <NUM> judges whether or not a section is a resembling section which has a feature amount closer to the feature amount associated with the search starting point among from the surrounding sections which correspond to the plurality of sections surrounding the search starting point (for example, sections neighboring to the search starting point) set by the search starting point setting portion <NUM>. In other words, the judging portion <NUM> judges whether or not a section is travelable for the electric wheel chair <NUM> judging from all sections Gnm.

In other words, the judging portion <NUM> judges whether or not the section surrounding the search starting point is a resembling section, or judges whether the section is travelable or not, by judging whether or not the difference in feature amount between the search starting point and the surrounding section is continuous. When the difference in the feature amount between the search starting point and the surrounding section is judged to be continuous, the judging portion <NUM> judges that the search starting point and the surrounding section are the resembling sections with each other and that the surrounding section is travelable. On the other hand, when the difference in the feature amount between the search starting point and the surrounding section is judged not to be continuous, the judging portion <NUM> judges that the search starting point and the surrounding section are not the resembling sections with each other and that the surrounding section is not travelable. As shown in <FIG>, the section with the X mark is not travelable by the electric wheel chair <NUM> and the section with O mark is travelable by the electric wheel chair <NUM>. The surrounding sections are located neighboring with each other in vertical and horizontal directions and do not include the sections neighboring in oblique directions. However, it is possible to include such sections neighboring in oblique directions.

A detail judging method whether a section is resembling or not will be explained as follows. Regarding to the height which is one of the feature amounts, when the height difference between the search starting point and a surrounding section is smaller than the first threshold value, the height difference is judged to be continuous between the search starting point and the surrounding section and therefore the surrounding section resembles with the search starting point. On the other hand, when the height difference between the search starting point and a surrounding section is larger than the first threshold value, the height difference is judged to be not continuous between the search starting point and the surrounding section and therefore the surrounding section does not resemble with the search starting point.

Further, regarding to the inclination which is another one of the feature amounts, when the inclination difference between the search starting point and a surrounding section is smaller than the second threshold value, the inclination difference is judged to be continuous between the search starting point and the surrounding section and therefore the surrounding section resembles with the search starting point. On the other hand, when the inclination difference between the search starting point and a surrounding section is larger than the second threshold value, the inclination difference is judged not to be continuous between the search starting point and the surrounding section and therefore the surrounding section does not resemble with the search starting point. As explained, when both of the height and the inclination which are associated with the feature amounts are continuous, the search starting point and the surrounding section and therefore the surrounding section resembles with the search starting point. Other than that, the search starting point and the surrounding section and therefore the surrounding section do not resemble with each other.

Further, the judging portion <NUM> sets (renews) a new search starting point which resembles most to the search starting point from the resembling sections and judges or determines a new resembling section based on the renewed search starting point. The judging portion <NUM> repeatedly executes the renewal of the judgment of the resembling section and the search starting point. If no resembling section is found (no renewal of the search starting point can be executed), the judging portion <NUM> sets another search starting point similar to the setting by the search starting point setting portion <NUM>. Further, the judging portion <NUM> (control device <NUM>) memorizes the section which has been judged to be the resembling section by the judging portion <NUM> in the memory portion. Further, the judging portion <NUM> sends the judgment result to the driving control portion <NUM>. The driving control portion <NUM> controls the travelling of the electric wheel chair <NUM> based on the judgement result from the judging portion <NUM>, or based on the judgement whether the section where the electric wheel chair <NUM> is travelling is a travelable section or non-travelable section.

The control of operation of the mobile body, particularly the control of the mobile body on judgment whether the mobile body is in a travelable section or not will be explained with reference to the flowchart shown in <FIG>. The control device <NUM> executes the program based on the flowchart.

The control device <NUM> obtains the surrounding information of the electric wheel chair <NUM> from the detecting device <NUM> as the point group data D, as similar to the surrounding information obtaining portion <NUM> as described above at the step S102. At the step S104, the control device <NUM> divides all of the point group data D obtained by the surrounding information obtaining portion <NUM> into the plurality of sections G which is a plurality of predetermined areas defined on a predetermined plane, as similar to the dividing portion <NUM> as described above.

At the step S106, similar to the calculation by the feature amount calculating portion <NUM> as described above, the control device <NUM> calculates the feature amount associated with the feature of the section Gnm from the divided point group data Dnm which is the point group data D that belongs to the section Gnm. This calculation is made per every divided section Gnm divided by the dividing portion <NUM>. In more detail, the control device <NUM> executes the program of flowchart shown in <FIG> as a sub-routine.

The control device <NUM> judges whether all sections G are processed or not at the step S202. When all of the sections G are processed, the control device <NUM> terminates the execution of the sub-routine and when the processing of all sections G is not finished, the program goes to the step S204. The control device <NUM> judges whether the section G is an effective section or not by judging whether the number of belonging point group is greater than a judging threshold value or not at the step S204.

At the step S206, similar to the merging of the merging portion 55a as described above, the control device <NUM> merges the section Gnm in which the number of divided point group data Dnm is relatively small and a neighboring section Gnm which is located adjacent thereto thereby forming a new section which is a merged section by merging. At the step S208, the control device <NUM>, similar to the judgement at the step S204, judges whether the merged section is an effective section or not.

When the section (merged section) is judged to be an effective section, the control device <NUM> calculates the feature amount of the section (in detail, the height and the inclination) at the step S210 and thereafter. On the other hand, when section (merged section) is judged to be not an effective section, the control device <NUM> labels the section as a vacant section and memorizes that the section is a vacant section at the step S218.

At the step S210, similar to the presumed plane setting portion 55b, the control device <NUM> sets the presumed plane PI from the divided point group data Dnm associated with the section Gnm per every section Gnm. At the step S212, similar to the identifying portion 55c as described above, the control device <NUM> identifies the effective point group data D and the outlier values from the divided point group data Dnm, based on the presumed plane PI set at the step S210. At the step S214, similar to the normal vector calculating portion 55d as described above, the control device <NUM> calculates the normal vector of the section Gnm from the effective point group data D identified at the step S212. At the step S216, similar to the inclination calculating portion 55e as described above, the control device <NUM> calculates the inclination which is one of the feature amounts of the section Gnm from the normal vector calculated at the step S214 and at the same time similar to the height calculating portion 55f, calculates the height which is one of the feature amounts of the section Gnm from the effective point group data D identified at the step S212.

At the step S108, the control device <NUM> judges whether or not the evaluation (judgement, search) on whether or not the section is travelable to all of the sections G is finished. When the evaluation has been finished to all of the sections G, the program goes to the step S110 and thereafter. When the evaluation is not finished to all of the sections G, the program goes to the step S112 and thereafter. At the step S110, the control device <NUM> applies label to the section where no label is applied, i.e., the section where no search (evaluation) has not been conducted that the section is a non-searched section and memorizes the section as non-searched section. The non-searched section is a section neither the travelable section or the non-travelable section (stepped section, inclination section).

At the step S112, similar to the search starting point setting portion <NUM> as described above, the control device <NUM> sets the search starting point which is a starting point for searching a section whether the section is travelable by the electric wheel chair <NUM> from the plurality of sections, based on the feature amount calculated at the step S106. In detail, the control device <NUM> executes the program of the flowchart shown in <FIG>, as a sub-routine.

At the step S302, the control device <NUM> judges whether the section is a vacant section or not. When the section is a vacant section, the control device <NUM> finishes the sub-routine and when the section is not a vacant section, advancers the program to the step S304. At the step S304, the control device <NUM> judges whether the section is located within a predetermined distance from the present position of the electric wheel chair <NUM> or not. If the distance is located further than the predetermined distance, the control device <NUM> finishes the sub-routine and if the distance is within the predetermined distance, advances the program to the step S306.

At the step S306, the control device <NUM> judges whether the section is searched or not searched. When the section has been searched, the control device finishes the sub-routine and when the section has not been searched advances the program to the step S308. At the step S308, the control device <NUM> calculates the height threshold value (or the height judging range) based on the inclination.

At the step S310, the control device <NUM> judges whether the section can be a search starting point or not. In detail, when the height of the section is within the height judging range, (for example, smaller than the second height threshold value) and yet the inclination of the section is within the inclination judging range (for example, smaller than the second inclination threshold value), the control device <NUM> judges that the section can be a search starting point. In a case other than the case described above, the control device <NUM> judges that the section cannot be a search starting point. The control device <NUM> finishes the program when the section could not be a search starting point and advances the program to the step S312 when the section is judged to be a search starting point and sets the section as the search starting point.

At the step S114, the control device <NUM> judges whether a search starting point exists or not. When the control device <NUM> judges that there exists no search starting point (search starting point is not set), the program returns to the step S108 and when the control device <NUM> judges that there exists a search starting point (search starting point is set), advances the program to the step s116 and thereafter.

At the step S116 and thereafter, similar to the judging portion <NUM> as described above, the control device <NUM> judges a resembling section which has a feature amount close to the feature amount associated with the feature of the search starting point among the surrounding sections which surround the search starting point set at the step S112. In other words, the control device <NUM> judges whether a section is travelable by the electric wheel chair <NUM> or not to all of the sections Gnm.

At the step S116, the control device <NUM> judges whether the judgement (search) for the surrounding sections has been finished or not. When the judgement (search) for the surrounding sections has not been finished, the control device <NUM> advances the program to the step S118 and thereafter but on the other hand, when the judgement (search) for the surrounding sections has been finished, the control device 50advances the program to the step S128 and thereafter.

At the steps S118 through S126, the control device <NUM> judges whether a section where a search has not been made resembles with the search starting point or not. When the difference in height between the search starting point and the section that has not been searched is smaller than a first threshold value (at the step S118: NO), and yet the difference in inclination between the search starting point and the section that has not been searched is smaller than a second threshold value (at the step S120: NO), the control device <NUM> judges that the difference in feature amount is continuous and that the search starting point and the section that has not been searched resemble with each other to thereby judge that the section is travelable by the electric wheel chair <NUM> (at the step S126). When the difference in height between the search starting point and the section is larger than the first threshold value (at the step S118: YES), the control device <NUM> judges that the difference in feature amount (height) is not continuous and the search starting point and the section do not resemble with each other, thereby to judge that the section is a non-travelable section with a large step (large step difference area) (at the step S122). When the difference in height between the search starting point and the section is smaller than the first threshold value (at the step S118: NO) and yet the difference in inclination between the search starting point and the section is larger than the second threshold value (at the step S120: YES), the control device <NUM> judges that the difference in the feature amount (inclination) is not continuous and that the search starting point and the section do not resemble with each other, to thereby judge that the section is a non-travelable section with a large inclination (steep slope area) (at the step S124).

At the step S128, the control device <NUM> judges whether or not a travelable section (resembling section) exists. When any travelable section does not exist, the control device <NUM> returns the program to the step S108 and when the travelable section exists, the control device <NUM> advances the program to the step S130. At the step S130, the control device <NUM> selects the resembling section which resembles most with the search starting point from the plurality of resembling sections and at the step S132, sets (renews) the selected section as a new search starting point. Then, the control device <NUM> returns the program to the step S116 to judge a new resembling section based on the renewed search starting point.

As apparent from the explanation above, the electric wheel chair <NUM> (mobile body) is a mobile body which travels driven by the driving device <NUM>. The electric wheel chair <NUM> includes a surrounding information obtaining portion <NUM> (obtaining portion) which obtains information of surrounding of the electric wheel chair <NUM> as a three-dimensionally indicated point group data, a dividing portion <NUM> which divides the point group data obtained by the surrounding information obtaining portion <NUM> into a plurality of sections G which is a plurality of predetermined areas defined on a predetermined plane, a feature amount calculating portion <NUM> which calculates a feature amount which is associated with a feature of one of the plurality of sections G from a divided point group data which is the point group data belonging to the one of the plurality of sections G per every section divided by the dividing portion <NUM>, a search starting point setting portion <NUM> which sets a search starting point which serves as a starting point for searching whether there is a travelable section in which the electric wheel chair <NUM> is travelable among the plurality of sections G based on the feature amount calculated by the feature amount calculating portion <NUM> and a judging portion <NUM> which judges a resembling section which includes a feature amount resembling the feature amount associated with the search starting point from among a surrounding section G, which is one of the plurality of the sections surrounding the search starting point set by the search starting point setting portion <NUM>.

According to the embodiment as described above, a resembling section can be judged based on the feature amount calculated per every section G, referencing the search starting point among the sections G surrounding the search starting point which has been set previously. In other words, the travelable section of the electric wheel chair <NUM> can be judged based on the search starting point as a reference point. As a result, an electric wheel chair <NUM> which more accurately recognizes a step or an inclination of a road surface on which the electric wheel chair <NUM> travels, by suppressing an influence from a shape of the road surface on which the electric wheel chair <NUM> travels can be proposed.

Further, since the section which is judged to be travelable (search starting point) as the base of the comparison and the surrounding section surrounding the search starting point are judged to be resembled with each other or not, even the posture of the electric wheel chair <NUM> or the detecting device <NUM> is changed, the correlative relationship between the sections are not changed. Accordingly, an influence from the disturbance due to the posture change of the electric wheel chair <NUM> or the detecting device <NUM> when travelling on an inclined road surface or an odd-even road surface can be suppressed. As a result, the electric wheel chair <NUM> can recognize more accurately the steps or the inclination of the road surface where the electric wheel chair <NUM> travels.

Further, the feature amount calculating portion <NUM> includes a presumed plane setting portion 55b which sets the presumed plane PI from the divided point group data D associated with the section G per every section G, an identifying portion 55c which identifies the effective point group data and the outlier value from the divided point group data based on the presumed plane PI set by the presumed plane setting portion 55b, a normal vector calculating portion 55d which calculates the normal vector of the section G from the point group data identified by the identifying portion 55c, an inclination calculating portion 55e which calculates the inclination which is one of the feature amounts of the section G from the normal vector calculated by the normal vector calculating portion 55d and a height calculating portion 55f which calculates the height which is one of the feature amounts of the section G from the effective point group data identified by the identifying portion 55c. According to this structure, the feature amount (height and inclination) associated with the section G can be accurately calculated per every section. As a result, the step or the inclination (steep slope) of the travelling road surface of the electric wheel chair <NUM> can be accurately recognized. Further, even under travelling on an odd-even road surface or a road with a big noise the normal vector of the section can be accurately calculated. Therefore, the electric wheel chair <NUM> can accurately recognize the travelable section and accordingly, can enlarge the section of travelling.

The feature amount calculating portion <NUM> further includes a merging portion 55a which merges the section G in which the number of divided point group data is relatively small and a neighboring section G which is located adjacent thereto thereby forming a new section G which is a merged section by merging. The height and the inclination of the merged section merged by the merging portion 55a are calculated. According to the structure above, by merging the section G which number of divided point group data is relatively small and the neighboring section G, the number of measuring points can be increased to accurately calculate the feature amount associated with the section G. As a result, the electric wheel chair <NUM> can more accurately recognize the step or the inclination of the road surface on which the electric wheel chair <NUM> travels.

Further, the search starting point setting portion <NUM> sets a section G as a search starting point where the section G is positioned within a predetermined distance from the present position of the electric wheel chair <NUM>, the section has the height that belongs to the feature amount calculated by the feature amount calculating portion <NUM> and that is within the height judging range and further the section G has the inclination that belongs to the feature amount calculated by the feature amount calculating portion <NUM> and that is within the inclination judging range. Thus, the search starting point can be accurately set and as a result, the electric wheel chair <NUM> can more accurately recognize the step or the inclination of the road surface on which the electric wheel chair <NUM> travels.

Further, the height judging range (each height threshold value) is set in response to the inclination calculated by the feature amount calculating portion <NUM>. According to this structure, the search starting point can be more accurately set and as a result, the electric wheel chair <NUM> can more accurately recognize the step or the inclination of the road surface on which the electric wheel chair <NUM> travels. Accordingly, the electric wheel chair <NUM> can more accurately recognize the gradual slope that can be traveled, and can enlarge the section of travelling thereby to improve convenience.

According to the embodiments above, upon setting the search starting point, feature amounts (such as, height information and inclination information) are calculated per every section from the divided point group data obtained from the point group data and based on the feature amounts, the search starting point is set. In this invention, the following structure may be applicable which is:.

The search starting point setting portion calculates (corrects) the feature amounts (for example, height information and inclination information) based on the information from another sensor (such as, an acceleration sensor) and the search starting point may be set based on the feature amounts calculated thereby. In this case, the accuracy of the calculated feature amount is improved to more properly and accurately set the search starting point.

Claim 1:
A mobile body (<NUM>) comprising:
a driving device (<NUM>) configured to drive the mobile body (<NUM>) for travelling;
an obtaining portion (<NUM>) configured to obtain three-dimensional coordinates associated with a detected object located around the mobile body from a detecting device (<NUM>) as three-dimensionally indicated point group data;
a dividing portion (<NUM>) configured to project the point group data on a predetermined plane including a plurality of sections (G), which is a plurality of predetermined areas, and an origin (C0) that is a reference position of the point group data, and to divide the point group data into a divided point group data (Dnm) by associating the point group data whose projections lie in each section (Gnm) with the each section (Gnm);
a feature amount calculating portion (<NUM>) configured to calculate a feature amount of the each section (Gnm) from the divided point group data (Dnm);
a search starting point setting portion (<NUM>) configured to set one of the sections (Gnm) which is travelable by the mobile body (<NUM>) as a search starting point based on the feature amount calculated by the feature amount calculating portion (<NUM>);
a judging portion (<NUM>) configured to judge whether one of the plurality of sections (Gnm) surrounding the search starting point set by the search starting point setting portion (<NUM>) has a feature amount resembling the feature amount of the search starting point and is travelable by the mobile body (<NUM>); and
a driving control portion (<NUM>) configured to control the driving device (<NUM>) for travelling the mobile body (<NUM>) based on the judgement result from the judging portion (<NUM>).