JP2009008648A - Three-dimensional distance measuring device and caster-type robot - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To provide a three-dimensional distance measuring device and a caster type robot equipped with the device capable of scanning a scan area without being hindered by a drive mechanism of a distance measuring sensor. <P>SOLUTION: The three-dimensional distance measuring device 200 includes a two-dimensional distance measuring device 112, a motor 116, an encoder 118, pulleys 120a, 120b, and a belt 121. A first rotating shaft of the motor 116 is engaged with the pulley 120a to become a rotating shaft of the pulley 120a, and the pulley 120a is rotationally driven by the rotational driving force of the motor 116. The pulleys 120a, 120b are disposed so that the positions of axes A, B are spaced by a predetermined distance in a horizontal direction, and the rotational driving force of the pulley 120a is transmitted to the pulley 120b through the belt 121. The pulley 120b and a second rotating shaft for rotationally driving the two-dimensional distance measuring device 112 are engaged to rotate the second rotating shaft interlocked with the rotation of the pulley 120b. <P>COPYRIGHT: (C)2009,JPO&INPIT

Description

  The present invention relates to a three-dimensional distance measuring apparatus including a distance measuring sensor and a rotation driving mechanism, and in particular, within a predetermined rotation angle range, the distance measurement is not hindered by the driving mechanism including the rotation driving mechanism. The present invention relates to a three-dimensional distance measuring device capable of scanning a scanning range with a sensor and a leg-wheel type robot having the device.

Conventionally, there is a three-dimensional distance measuring device that measures three-dimensional distance information by rotationally driving a two-dimensional distance measuring device such as a two-dimensional range sensor, a laser range finder, a range sensor, or a flat sensor. For example, there is a technique described in Non-Patent Document 1 as a three-dimensional distance measuring device using a flat sensor.
In Non-Patent Document 1, in order to scan the entire environment, the planar sensor can be rotated by 360 [°] or more.
Tatsuro Ueda, Hirohiko Kawada, Tetsuo Serizawa, Akihisa Oya, Shinichi Yuta “3D range sensor using infinite rotation mechanism”, Robodia Mechatronics Lecture 06, 2P1-C16

However, in the prior art of Non-Patent Document 1, in order to rotate the planar scanner by 360 [°] or more, all the mechanisms for driving the planar scanner are gathered in one place below the planar scanner. For this reason, the mechanism is long in the vertical direction, which may be disadvantageous in terms of space factor.
Further, in the conventional technique of Non-Patent Document 1, since a slip ring that can communicate and supply power without performing wiring is used in order to achieve a mechanism capable of rotating 360 [°] or more, there is a problem of contact life. appear. Even if the communication unit is made wireless, power supply wiring is required. Moreover, even if the power supply is supplemented with a battery, problems such as an increase in weight and battery duration occur.

In addition, sufficient measurement information can be obtained even if there is no rotation mechanism of 360 [°] or more if the space is prepared so that humans can be active in environment recognition, shape recognition, obstacle avoidance, and the like.
Therefore, the present invention has been made paying attention to such an unsolved problem of the conventional technique, and scans the scanning range without being disturbed by the driving mechanism of the distance measuring sensor within a predetermined rotation angle range. It is an object of the present invention to provide a three-dimensional distance measuring device that can be used and a leg-wheel type robot including the device.

  [Invention 1] In order to achieve the above object, a three-dimensional distance measuring apparatus according to Invention 1 includes a distance measuring sensor for measuring a distance to a measurement point on an object existing within a measurement distance range, and the distance measuring sensor. The first rotation mechanism that rotates around one of the two axes orthogonal to the measurement direction, and the distance measuring sensor, together with the first rotation mechanism, orthogonal to the measurement direction A second rotating mechanism that rotates around an axis in a direction different from that of the first rotating mechanism, and the first rotating mechanism and the second rotating mechanism are rotated in a predetermined rotation. A three-dimensional distance measuring device for measuring a three-dimensional distance of the object by measuring a distance by the distance measuring sensor while rotationally driving each angle, wherein the first rotating mechanism and the second rotating mechanism At least one of the rotational drives The self-drive mechanism including the first rotation mechanism and the second rotation mechanism is provided outside the scanning range in which the distance measuring sensor scans by the rotation driving within the rotation angle range. It is characterized by that.

With such a configuration, the distance measuring sensor is driven to rotate around one of two axes orthogonal to the measuring direction, and the distance measuring sensor and the first rotating mechanism are It is possible to measure the distance to the measurement point on the object while rotationally driving around the other axis out of the two axes orthogonal to the direction.
Further, it is possible to perform scanning by rotation driving of at least one of the first rotation mechanism and the second rotation mechanism within a predetermined rotation angle range without being hindered by the distance sensor driving mechanism.
Here, the predetermined rotation angle is an angle of a scanning range in which sufficient distance data can be obtained for environment recognition, shape recognition, obstacle avoidance, and the like.

[Invention 2] Further, the three-dimensional distance measurement device of Invention 2 is the three-dimensional distance measurement device of Invention 1, wherein the second rotation mechanism includes a rotation drive device that generates a rotation drive force,
A first rotation shaft of the rotation drive device and a second rotation shaft that rotates both the distance measuring sensor and the first rotation mechanism are provided to be shifted in the horizontal direction, and the first rotation shaft of the rotation drive device is provided. The rotary driving force of the rotary shaft is transmitted to the second rotary shaft through a predetermined power transmission mechanism.
With such a configuration, it is possible to arrange the two-dimensional distance measuring device including the distance measuring sensor and the first rotating mechanism and the second rotating mechanism side by side in the horizontal direction. is there.

[Invention 3] Further, the three-dimensional distance measuring device according to Invention 3 is the three-dimensional distance measuring device according to Invention 2, wherein the power transmission mechanism rotates the rotation via a plurality of pulleys and a belt that transmits power to each pulley. The rotational driving force of the first rotating shaft of the driving device is transmitted to the second rotating shaft.
With such a configuration, it is possible to transmit the rotational driving force of the first rotating shaft of the rotational driving device to the pulley, and it is possible to transmit the power between the pulleys via the belt. It is possible to transmit the transmitted rotational force of the pulley to the second rotating shaft.

[Invention 4] Furthermore, the three-dimensional distance measuring device of Invention 4 is the three-dimensional distance measuring device of Invention 2, wherein the power transmission mechanism is connected to the first rotating shaft of the rotary drive device via a plurality of gears. The rotary driving force is transmitted to the second rotary shaft.
With such a configuration, it is possible to transmit the rotational driving force of the first rotational shaft of the rotational drive device to the second rotational shaft via a plurality of gears.

  [Invention 5] Further, the three-dimensional distance measuring device according to Invention 5 is the three-dimensional distance measuring device according to any one of Inventions 1 to 4, wherein the first rotation mechanism is controlled, and the distance measuring sensor is A first scanning unit that rotates a predetermined rotation angle in the rotation direction of the first rotation mechanism, and performs a first scanning process that is a process of measuring distance information according to each rotation angle by the distance measuring sensor; The gradient calculation means for calculating the gradient for each measurement point based on the distance information for each measurement point and the distance information for the surrounding measurement points, measured by the first scanning means, and calculated by the gradient calculation means Based on the appearance frequency calculation means for calculating the appearance frequency of each gradient based on the gradient of each measurement point, the appearance frequency of each gradient calculated by the appearance frequency calculation means, and the coordinate information of the measurement point corresponding to each gradient Of the distance measuring sensor It characterized in that it comprises a recognizing shape recognition means the shape of the object existing in a constant plane.

With such a configuration, the first scanning mechanism is controlled by the first scanning unit to rotate the distance measuring sensor by a predetermined rotation angle in the rotation direction of the first rotation mechanism. The first scanning process, which is a process for measuring distance information corresponding to each rotation angle by the distance measuring sensor, can be performed.
In addition, the gradient calculation unit can calculate the gradient for each measurement point based on the distance information of each measurement point and the distance information of the surrounding measurement points measured by the first scanning unit. The frequency calculation means can calculate the appearance frequency of each gradient based on the gradient of each measurement point calculated by the gradient calculation means, and the shape recognition means can calculate the appearance frequency of each gradient calculated by the appearance frequency calculation means. It is possible to recognize the detailed shape of the object existing in the measurement plane of the distance measuring sensor based on the coordinate information of the measurement point corresponding to each gradient.

Here, the gradient calculation means, for example, approximates a regression line for a plurality of measured values in a Cartesian coordinate system by a known least square method, calculates a gradient from the regression line, or uses one axis in the Cartesian coordinate system. A gradient is calculated using a difference value between two measurement values sandwiched between a plurality of measurement points in the direction and a difference value between two measurement values sandwiched between the plurality of measurement points in the other axial direction. To do.
Further, the shape recognition means recognizes the unevenness of the object existing in the measurement plane based on the appearance frequency of each gradient and the coordinate information of the measurement point corresponding to each gradient. For example, when measurement points having a gradient of 0 or substantially 0 are continuous, it can be recognized that there is a plane there.

  [Invention 6] Furthermore, the three-dimensional distance measurement device of Invention 6 is the three-dimensional distance measurement device of Invention 5, in which the second rotation mechanism is provided each time the first scanning process for one measurement plane is completed. A second scanning unit configured to control and perform a second scanning process that is a process of rotating the distance measuring sensor together with the first rotation mechanism by a predetermined rotation angle in a rotation direction of the second rotation mechanism; In the second scanning process, the first scanning unit performs the first scanning process every time the distance measuring sensor rotates by the predetermined rotation angle, and the gradient calculating unit includes the measurement plane. Every time, the gradient for each measurement point is calculated based on the distance information of each measurement point measured by the first scanning process and the distance information of the surrounding measurement points, and the appearance frequency calculating means For each measurement plane, the gradient calculation means The appearance frequency of each gradient is calculated based on the gradient of each measurement point, and the shape recognition unit corresponds to the appearance frequency of each gradient calculated by the appearance frequency calculation unit and each gradient for each measurement plane. The shape of the object existing in the measurement plane is recognized based on the coordinate information of the measurement point to be measured.

If it is such a structure, whenever the 1st scanning process with respect to one measurement plane is complete | finished by a 2nd scanning means, a 2nd rotation mechanism is controlled and a ranging sensor is combined with a 1st rotation mechanism, It is possible to perform a second scanning process that is a process of rotating the second rotation mechanism by a predetermined rotation angle in the rotation direction.
Further, the gradient calculation means calculates the gradient for each measurement point based on the distance information of each measurement point measured by the first scanning process and the distance information of the surrounding measurement points for each measurement plane, The appearance frequency calculation means calculates the appearance frequency of each gradient based on the gradient of each measurement point calculated by the gradient calculation means for each measurement plane, and the shape recognition means calculates by the appearance frequency calculation means for each measurement plane. Based on the appearance frequency of each gradient and the coordinate information of the measurement point corresponding to each gradient, the shape of the object existing in the measurement plane can be recognized.
Here, the shape recognition means, for example, the gradient information of each measurement point calculated from both the distance information when the distance sensor is rotated by the pitch and the distance information when the distance sensor is rotated by the yaw, Since the shape of the object can be recognized from the coordinate information, it is possible to recognize the detailed surface shape of the object.

[Invention 7] On the other hand, in order to achieve the above object, the leg-wheel type robot of Invention 5 is a base, a leg connected to the base with a degree of freedom, and rotatable to the leg. And a leg that moves by the driving of the leg and the rotation of the wheel. The leg is provided with a wheel, an actuator for applying power for driving the leg and the wheel, and a control means for controlling the actuator. A wheel type robot comprising the three-dimensional distance measuring device according to any one of the first to sixth aspects, wherein the control means controls the actuator based on a measured distance of the three-dimensional distance measuring device. It is characterized by.
With such a configuration, it is possible to perform wheel traveling control and walking control based on the measured distance measured by the three-dimensional distance measuring device according to any one of inventions 1 to 4.

[Invention 8] In order to achieve the above object, the leg-wheel type robot of Invention 8 has a base and a degree of freedom around the yaw axis and a degree of freedom around the pitch axis or roll axis with respect to the base. Connected legs, wheels rotatably provided on the legs, a first actuator for applying power to drive the legs within a range of freedom around the yaw axis, and the pitch A second actuator for applying power for driving the leg within a range of degrees of freedom around the shaft or roll axis, a control means for controlling the first actuator and the second actuator, and the first to sixth aspects of the invention A leg-wheel type robot that includes the three-dimensional distance measuring device according to any one of claims 1 to 8, and moves by driving the leg and rotating the wheel, wherein the control means sets the orientation of the base in a fixed direction. While maintaining, the second actuator is controlled so that the traveling direction of the self-legged wheel type robot coincides with the traveling direction of each wheel.
With this configuration, when the self-legged wheel robot is moving, the traveling direction of the self-legged robot matches the traveling direction of each wheel while maintaining the base of the leg-wheel robot at a fixed direction. Thus, the second actuator is controlled.

  [Invention 9] In order to achieve the above object, a leg-wheel robot according to Invention 9 has a base and a degree of freedom around the yaw axis and a degree of freedom around the pitch axis or roll axis with respect to the base. A plurality of legs connected to each other, wheels provided rotatably on each leg, and a first actuator for applying power for driving each leg within a range of degrees of freedom around the yaw axis. A second actuator for applying power for driving each leg within a range of degrees of freedom around the pitch axis or roll axis, and a control means for controlling the first actuator and the second actuator; A leg-wheel type robot comprising the three-dimensional distance measuring device according to any one of 1 to 6 and moving by driving the leg and rotating the wheel. in front The direction of movement of the rotation center at the contact point between the arc trajectory drawn by the rotation center at the time of steering of each wheel and the rotation center at the time of steering when the base body rotates around the yaw axis at a predetermined rotation center position. The second actuator is controlled so that the traveling directions of the wheels coincide with each other.

  With such a configuration, during turning, the control means rotates the base around the yaw axis (vertical axis) and the arc trajectory drawn by the rotation center at the time of steering of each wheel and the rotation center at the time of steering. The second actuator is controlled so that the direction of movement of the center of rotation at the contact point with the direction of travel of each wheel coincides.

  [Invention 10] Further, the leg wheel type robot of the invention 10 is the leg wheel type robot of the invention 9, wherein the control means maintains the direction of the base body in a constant direction, The second actuator is controlled so that the traveling directions of the wheels coincide with each other.

  With this configuration, when the self-legged wheel robot is moving, the traveling direction of the self-legged robot matches the traveling direction of each wheel while maintaining the base of the leg-wheel robot at a fixed direction. Thus, the second actuator is controlled.

As described above, according to the three-dimensional distance measuring device of the first aspect, scanning can be performed within the predetermined rotation angle range without being disturbed by the driving mechanism of the distance measuring sensor. The effect that it can be performed is acquired.
Moreover, according to the three-dimensional distance measuring apparatus of the invention 2-4, since it can be set as the structure which arranged the said ranging sensor, the said 1st rotation mechanism, and the said 2nd rotation mechanism in the horizontal direction, The effect that the occupation ratio in the vertical direction of the three-dimensional distance measuring device can be suppressed is obtained.

In addition, according to the three-dimensional distance measuring devices of the inventions 5 to 6, the shape of the surface of the object can be recognized (or estimated). It is possible to recognize objects of various shapes such as staircases that are not continuous surfaces, staircases with only staircase steps such as emergency external staircases.
On the other hand, according to the leg-wheel type robots of the inventions 8 and 10, the leg-wheel type robot can be moved in a free direction regardless of the direction thereof, so that quick movement in each direction can be realized, For example, it is possible to perform activities even in areas where it is difficult to change the direction of the leg wheel type robot, such as a narrow and intricate area in which each component of the leg wheel type robot is hindered from turning. can get.

  Further, according to the leg-wheel type robot of the ninth aspect, at the time of turning, the rotation center at the time of steering of each wheel is drawn when the base of the leg-wheel type robot is rotated about the yaw axis at a predetermined rotation center position. Since the direction of movement of the center of rotation at the contact point between the circular arc track and the center of rotation at the time of steering can coincide with the traveling direction of each wheel, the leg wheel type robot is not moved back and forth at a predetermined turning center position. Can be made to turn (turning equivalent to super-spinning by a crawler or the like). Further, by setting the center position of the base body as the rotation center position when rotating, the leg wheel type robot can be turned with the minimum turning radius.

[First Embodiment]
Hereinafter, a first embodiment of the present invention will be described with reference to the drawings. FIGS. 1-14 is a figure which shows 1st Embodiment of the three-dimensional distance measuring apparatus and leg-wheel-type robot based on this invention.
First, a schematic configuration of a leg wheel type robot according to the present invention will be described with reference to FIG.
FIG. 1 is a side view of a leg wheel type robot 100 according to the present invention.

  As shown in FIG. 1, the leg-wheel type robot 100 includes a base body and four leg portions connected to the base body. Each leg is connected via three rotary joints. The upper rotary joint rotates with the direction orthogonal to the bottom surface of the leg wheel type robot 100 as the axial direction. That is, it rotates around the yaw axis. Further, the middle and lower rotary joints rotate around the pitch axis when the upper rotary joint is in the state shown in FIG. 1, and the upper rotary joint is rotated 90 degrees from the state shown in FIG. , Rotate around roll axis. That is, each leg has 3 degrees of freedom.

  Further, as shown in FIG. 1, a three-dimensional distance measuring device 200 is disposed on the upper part of the base of the leg wheel type robot 100. As shown in FIG. 1, the axis of the measuring direction of the distance measuring sensor 112a at the origin position of the three-dimensional distance measuring device 200 and the distance measuring sensor 112a described later (positions where the rotation angles θ and φ are 0 [°]) is x. An axis that is orthogonal to an axis in the horizontal direction (y-axis to be described later) with respect to the measurement direction is the z-axis.

Next, based on FIG.2 and FIG.3, the mechanical structure of the three-dimensional distance measuring apparatus 200 is demonstrated.
Here, FIG. 2A is a diagram showing a mechanical configuration of the three-dimensional distance measuring apparatus 200, and FIG. 2B is a diagram of FIG. FIG. 3 is a diagram illustrating an example of a scanning range of the distance measuring sensor 112a.
As shown in FIG. 2A, the three-dimensional distance measuring apparatus 200 includes a two-dimensional distance measuring apparatus 112, a motor 116, an encoder 118, pulleys 120a and 120b, and a belt 121. Yes.

  The rotating shaft of the motor 116 (hereinafter referred to as the first rotating shaft) is engaged with the pulley 120a so as to be the rotating shaft of the pulley 120a, and the rotational driving force of the motor 116 rotationally drives the pulley 120a. To do. The pulleys 120a and 120b are arranged such that the positions of the shaft centers A and B are opened at a predetermined distance in the horizontal direction. In this manner, the axis A of the motor 116 and the axis B of the rotating shaft (hereinafter referred to as the second rotating shaft) of the two-dimensional distance measuring device 112 are shifted in the horizontal direction via the pulleys 120a and 120b. Thus, the motor 116 and the encoder 118 and the two-dimensional distance measuring device 112 can be arranged in the horizontal direction. Further, as shown in FIG. 2A, the pulleys 120 a and 120 b, the belt 121, and other support members are disposed on the upper part of the two-dimensional distance measuring device 112.

The pulley 120a and the pulley 120b disposed at a predetermined distance in the horizontal direction are provided with depressions on the outer peripheral surface, and both depressions are used to transmit the rotation of the pulley 120a to the pulley 120b. The belt 121 is stretched over. That is, when the pulley 120a rotates, the belt 121 rotates, and this rotational driving force is transmitted to the pulley 120b.
Further, the pulley 120b and the second rotating shaft are engaged so that the second rotating shaft rotates in conjunction with the rotation of the pulley 120b. Therefore, the rotational driving force transmitted to the pulley 120b is transmitted to the second rotating shaft.

That is, when the motor 116 rotationally drives the rotational shaft, the rotational driving force is transmitted to the second rotational shaft via the pulley 120a, the belt 121, and the pulley 120b, and is shown in FIG. As described above, the two-dimensional distance measuring device 112 is rotationally driven around the vertical axis (z-axis).
On the other hand, the distance measuring sensor 112a in the two-dimensional distance measuring device 112 according to the present embodiment measures the distance while being driven to rotate about the horizontal axis (y-axis). The scanning range of the distance measuring sensor 112a is intended to allow the leg-wheel robot 100 to move up and down stairs and avoid obstacles. Therefore, as shown in FIG. This is the range where scanning is focused.

  As shown in FIG. 1, the three-dimensional distance measuring apparatus 200 includes a driving mechanism (motor 116, encoder 118, pulleys 120a and 120b, belt 121, and other supporting members) that rotationally drives the two-dimensional distance measuring apparatus 112. 3 is provided outside the scanning range shown in FIG. 3, the scanning around the y-axis of the distance measuring sensor 112 a is not hindered by each mechanism unit constituting the three-dimensional distance measuring device 200 in the scanning range shown in FIG. 3. .

  Further, since it is only necessary to be able to recognize an obstacle on the travel route of the leg-wheel type robot 100, the scanning range by rotational driving around the z axis does not need to cover up to 180 [°] forward, and the two-dimensional distance measuring device 112. A range in which the motor 116 and encoder 118 provided in the horizontal direction are outside the scanning range is also sufficient. Therefore, in the present embodiment, the rotational driving range of the two-dimensional distance measuring device 112 is a range that does not include the motor 116 and the encoder 118.

Next, the configuration of the three-dimensional distance measuring device 200 will be described with reference to FIG.
FIG. 4A is a block diagram showing a configuration of the three-dimensional distance measuring apparatus 200, and FIG. 4B is a block diagram showing a detailed configuration of the two-dimensional distance measuring apparatus 112 according to the present embodiment.
As shown in FIG. 4A, the three-dimensional distance measuring device 200 controls a motor 116 described later based on a sensing processor 110, a two-dimensional distance measuring device 112, and a command signal and an output signal of an encoder 118 described later. It includes a driver 114, a motor 116 that rotationally drives the two-dimensional distance measuring device 112, and an encoder 118 that detects the rotational angle position of the motor 116.

  The sensing processor 110 executes a dedicated program, gives a command signal to a driver 112b described later, rotationally drives a distance measuring sensor 112a described later, and measures a distance on an object existing in the measurement plane. In addition to executing the scanning process, every time the first scanning process for one measurement plane is completed, a command signal is given to the driver 114 to execute the second scanning process for rotationally driving the two-dimensional distance measuring device 112.

  Furthermore, the sensing processor 110 performs measurement points on each measurement plane based on distance information (hereinafter referred to as distance information) measured by the two-dimensional distance measurement device 112 through the first scanning process and the second scanning process. Is performed, and the one-dimensional shape (unevenness) of the object in each measurement plane and the surface of the object in the measurement range are calculated based on the calculated appearance frequency and the coordinate information of each measurement point. A process for recognizing the shape is executed.

  As shown in FIG. 4B, the two-dimensional distance measuring device 112 includes a distance measuring sensor 112a that measures a distance to a measurement point on an object existing in the measurement range, a command signal, and an output of an encoder 112d described later. A driver 112b that controls the driving of the motor 112c based on the signal, a motor 112c, and an encoder 112d that detects the rotational angle position of the motor 112c are configured.

The driver 112b is designated with the rotation axis of the motor 112c based on the scanning angle range and the scanning angle unit (for example, a predetermined rotation angle such as 0.36 [°]) designated in the command signal from the sensing processor 110. Rotation control is performed for each scanning angle unit.
Here, the axis in the laser output direction at the origin position of the distance measuring sensor 112a (the position where the scanning angle is 0 [°]) is the x axis, and one of the two axes orthogonal to the x axis is y. The axis (in this embodiment, the horizontal and horizontal axes of the sensor) and the other axis are the z-axis (in this embodiment, the vertical axis).

The motor 112c is provided so as to rotationally drive a laser output unit (not shown) and a light receiving unit (not shown) of the distance measuring sensor 112a around a horizontal axis (y axis), and is controlled by the driver 112b. In response to the signal, it rotates its own rotation axis by a specified scanning angle unit (rotation angle θ).
Here, FIG. 5 is a diagram for explaining the principle of distance measurement of the two-dimensional distance measuring device 112.

  In the two-dimensional distance measuring device 112, the distance measuring sensor 112a is rotated by a scanning angle unit designated around a horizontal axis (y-axis) in accordance with the rotational driving of the rotating shaft of the motor 112c, and is rotated. As shown in FIG. 5, each time the actuator moves, laser light is output from the laser output unit, and reflected light from an object (obstacle in FIG. 5) with respect to the output light is received by the light receiving unit. A distance corresponding to (scanning angle) (measurement distance L in FIG. 2 (distance between the object and the light receiving unit)) is measured.

FIG. 6A is a diagram showing the relationship between the measurement distance and the rotation angle θ when the distance measuring sensor 112a is driven to rotate about the horizontal axis (y-axis), and FIG. It is a figure which shows the relationship between the measurement plane and rotation angle (phi) when the two-dimensional distance measuring device 112 is rotationally driven around the vertical axis | shaft (z axis).
That is, for example, as shown in FIG. 6A, the first scanning process is performed by rotating the distance measuring sensor 112a by a specified scanning angle unit around the horizontal axis (y axis). Distance information (L (θ ₁ ), L (θ ₂ ), L (θ in FIG. 6A) according to each rotation angle (θ ₁ , θ ₂ , θ _{3 in} FIG. 6A) with respect to the position) ₃ )) is measured.

In the first scanning process, a plane formed by connecting the rotation center of the rotating shaft of the motor 112c and both ends of the scanning trajectory line of the laser is a measurement plane (a sector-shaped plane when no object is present). .
Returning to FIG. 4A, the driver 114 scans the rotation axis of the motor 116 with the designated scanning axis based on the scanning angle range and the scanning angle unit (rotation angle φ) designated in the command signal from the sensing processor 110. Control to rotate by angle unit.

  The motor 116 is provided so as to rotationally drive the two-dimensional distance measuring device 112 around a vertical axis (z axis) via a speed reducer (not shown), pulleys 120a and 120b, and a belt 121. In response to a control signal from the driver 114, the rotation axis of the driver is rotated by a specified scanning angle unit. As a result, the rotational driving force is transmitted to the second rotational shaft via the pulleys 120a and 120b according to the rotational driving of the rotational shaft of the motor 116, and the two-dimensional distance measuring device 112 is moved around the vertical axis. Rotate by specified scanning angle unit.

  That is, in the second scanning process, as shown in FIG. 6B, the two-dimensional distance measuring device 112 is rotated by a specified scanning angle unit (φ) around the vertical axis (z axis). It becomes processing. Then, the measurement plane formed by the first scanning process is continuously formed around the vertical axis (z axis) by alternately performing the first scanning process and the second scanning process. To do. Thereby, for example, as shown in FIG. 7, it is possible to measure the three-dimensional distance information of an object existing within a measurement range (scanning angle range) such as a wall, a wall, a stand, and a shelf. Here, FIG. 7 is a diagram illustrating a measurement example of the distance of the three-dimensional distance measuring apparatus 200.

Further, as shown in FIG. 6B, the distance information of each measurement point after the second scanning process is expressed as L (θ _i , φ _j ). Here, i is a serial number assigned to each measurement point according to the scanning angle around the horizontal axis (y-axis), and j is according to the scanning angle around the vertical axis (z-axis). This is a serial number assigned to each measurement point.
Next, the flow of object recognition processing in the three-dimensional distance measuring apparatus 200 will be described with reference to FIG. Here, FIG. 8 is a flowchart showing object recognition processing in the three-dimensional distance measuring apparatus 200.

The object recognition process is a process realized by the sensing processor 110 reading a dedicated program stored in a ROM (not shown) in response to a command signal from the CPU 60 described later and executing the read program. Yes, when the process is started, as shown in FIG. 8, first, the process proceeds to step S100.
In step S100, the three-dimensional distance measuring device 200 sets the scanning angle range and the scanning angle unit of the distance measuring sensor 112a and the two-dimensional distance measuring device 112 based on the command signal from the CPU 60, and the process proceeds to step S102. Here, the command signal from the CPU 60 includes information on a scanning angle range and a scanning angle unit.

  In step S102, in the two-dimensional distance measuring device 112, the driver 112b, the motor 112c, and the encoder 112d are driven in response to the command signal from the sensing processor 110, and the distance measuring sensor 112a is moved around the y axis set in step S100. In the scanning angle range, the rotation angle unit is rotated around the y axis by the same scanning angle unit set in step S100, and the distance information corresponding to each rotation angle is measured (first scanning process). The process proceeds to step S104.

In step S104, the sensing processor 110 performs filtering processing using a median filter on the distance information measured in step S102 to remove noise components, and the process proceeds to step S106.
In step S106, the sensing processor 110 converts the distance information of the rotating coordinate system after noise removal in step S104 into the distance information of the orthogonal coordinate system, and proceeds to step S108.

In the conversion from the rotational coordinate system to the orthogonal coordinate system, the coordinates of each measurement point by the first scanning process are coordinates on the z-x plane, and the x-axis is a scanning angle unit around the z-axis by the second scanning process. The coordinate position changes one by one. Accordingly, the conversion to the orthogonal coordinate system is performed for each rotation angle around the z axis.
In step S108, the sensing processor 110 calculates the gradient of each measurement point based on the distance information converted into the orthogonal coordinate system in step S106, and proceeds to step S110.

Here, FIGS. 9A and 9B are diagrams showing examples of gradient calculation.
The gradient of each measurement point is calculated using the least square method, as shown in FIG. 9A, using the measurement distance of each measurement point and the distance information of a predetermined number of measurement points before and after the measurement point.
As shown in FIG. 9A, the least square method is used to approximate a regression line for a predetermined number of measurement points before and after the measurement point (white circle in FIG. 9A) for which the gradient is calculated, The slope of the regression line is taken as the slope of the target measurement point. The least squares method is less affected by data variations, but the calculation amount is relatively large.

As another method, as shown in FIG. 9B, for each measurement point, a gradient is calculated using a difference value of distance information between two measurement points before and after a predetermined number of other measurement points. To do.
Specifically, as shown in FIG. 9B, the first difference value (for example, the distance in the z-axis direction of the measurement point of interest is z _{If i} , z _{i + 2} −z _{i− 2} ) is calculated, and the second difference value (for example, the x-axis of the measurement point of interest) that is the difference value of the measurement distance between the two measurement points in the x-axis direction is calculated. If the distance in the direction is x _i , x _{i + 2} −x _i−2 ) is calculated, and the gradient of the measurement point of interest (first difference value / second difference value) is obtained from these calculation results. In this method, the influence due to data variation is greater than that in the least square method, but since the calculation only requires simple subtraction and division, the gradient can be calculated faster than the least square method.

In step S110, the sensing processor 110 calculates the appearance frequency of each gradient based on the gradient calculated in step S108, and proceeds to step S112.
In the present embodiment, the number of measurement points for each gradient is calculated as the appearance frequency.
In step S112, in the sensing processor 110, based on the appearance frequency of each gradient calculated in step S110, the appearance frequency determination processing selects the appearance frequency of the unprocessed gradient, and the process proceeds to step S114.

In step S114, the sensing processor 110 compares the appearance frequency of the gradient selected in step S112 with a preset threshold value and determines whether the selected appearance frequency is equal to or higher than the threshold value, and is determined to be equal to or higher than the threshold value. If yes (Yes), the process proceeds to step S116; otherwise (No), the process proceeds to step S134.
When the process proceeds to step S116, the sensing processor 110 associates the selected gradient with the coordinate information of each measurement point having the gradient and records it in a recording medium such as a RAM (not shown), and the process proceeds to step S118. .

In step S118, the sensing processor 110 determines whether or not the determination process in step S114 has been completed for all gradients. If it is determined that the determination has been completed (Yes), the process proceeds to step S120; No) moves to step S112.
When the process proceeds to step S120, the sensing processor 110 determines whether or not there is a continuous surface (actually, a line where the continuous surface and the measurement plane intersect) based on the gradient recorded in step S116 and the coordinate information of each measurement point. Then, the process proceeds to step S122.

Here, the determination of the continuous surface is, for example, based on the coordinate information of each measurement point corresponding to the gradient included in the predetermined gradient range, when the coordinates of the measurement point are continuous, intersecting with these coordinate positions. It is determined that there is a continuous surface to perform. When the coordinates of the measurement points corresponding to the gradient included in the predetermined gradient range are not continuous, it is determined that there is no continuous surface.
In step S122, the sensing processor 110 determines whether or not there is a continuous surface based on the determination result of step S120. If it is determined that there is a continuous surface (Yes), the process proceeds to step S124. For (No), the process proceeds to step S126.

When the process proceeds to step S124, the sensing processor 110 converts the coordinate system of the measurement distance of each measurement point corresponding to the continuous surface into the sensor coordinate system (three-dimensional coordinates), and the measurement distance of the sensor coordinate system after conversion. Is associated with the gradient information and recorded on a recording medium such as a RAM (not shown), and the process proceeds to step S126.
In step S126, the sensing processor 110 determines whether or not the continuous surface determination processing has been completed for all measurement planes corresponding to the scanning angle range and the scanning angle unit of the second scanning processing. If it is determined (Yes), the process proceeds to step S128. If not (No), the process proceeds to step S132.

When the process proceeds to step S128, the sensing processor 110 generates surface data based on the information of the measurement distance determined as the continuous surface recorded in the recording medium such as the RAM in step S124, and the process proceeds to step S130.
Here, the generation of the surface data is performed, for example, by comparing the gradient determined to be a continuous surface between the measurement planes and the coordinate information of the measurement point, and determining that the surface is close to the coordinate value and the gradient as a surface. The coordinate information is associated with each other or connected by using a known complement method.

For example, since the surface of the surface data composed of the continuous coordinate information of the measurement points whose gradient is close to 0 can be determined as a horizontal surface, it can be determined that the surface can be walked.
In step S130, in the three-dimensional distance measuring apparatus 200, the surface data generated in step S128 is output to the CPU 60 via the hub 76 and the communication I / F 64 described later, and the process ends.

  On the other hand, if the determination process of the continuous surface is not completed for all the measurement planes in step S126 and the process proceeds to step S132, the three-dimensional distance measurement apparatus 200 responds to the command signal from the sensing processor 110. Then, the driver 114, the motor 116, and the encoder 118 are driven, and the two-dimensional distance measuring device 112 is moved by z within the scanning angle range around the z-axis set at step S100. A process of rotating around the axis (second scanning process) is executed, and the process proceeds to step S102.

In step S114, if the appearance frequency is not equal to or greater than the threshold value and the process proceeds to step S134, the sensing processor 110 excludes the measurement distance of the measurement point corresponding to the gradient selected in step S112 as measurement noise, and step S118. Migrate to
Next, a movement control system of the leg wheel type robot 100 will be described with reference to FIG.

Here, FIG. 10 is a block diagram showing a movement control system of the leg-wheel type robot 100.
As shown in FIG. 10, joint motors 40 that rotationally drive the respective rotary joints are provided at the rotary joints of the four leg portions of the leg-wheel type robot 100, respectively. Each joint motor 40 is provided with an encoder 42 that detects the rotational angle position of the joint motor 40, and a driver 44 that controls the driving of the joint motor 40 based on the motor command signal and the output signal of the encoder 42.

A wheel motor 50 for rotating the drive wheel is provided on each leg drive wheel. Each wheel motor 50 is provided with an encoder 52 that detects the rotational angle position of the wheel motor 50, and a driver 54 that controls the driving of the wheel motor 50 based on the motor command signal and the output signal of the encoder 52.
The leg-wheel robot 100 further includes a CPU 60, a three-axis attitude sensor 70 that detects the attitude of the leg-wheel robot 100, a vision processor 72 that processes an image signal of the camera 32, a three-dimensional distance measuring device 200, A wireless communication unit 74 that performs wireless communication with an external PC, a vision processor 72, a wireless communication unit 74, a sensing processor 110, a hub 76 that relays input / output of the CPU 60, and a speaker 78 that outputs warning sounds and the like are provided. Configured.

The triaxial attitude sensor 70 includes a gyroscope or an acceleration sensor, or both, and detects the inclination of the attitude of the leg wheel type robot 100 with respect to the ground axis.
The CPU 60 outputs motor command signals to the drivers 44 and 54 via the motor command output I / F 61 and inputs output signals of the encoders 42 and 52 via the angle fetch I / F 62. Although not shown, a motor command signal is output to the driver of the motor 82 via the motor command output I / F 61.

The CPU 60 inputs sensor signals from the leg tip sensor 38, the three-dimensional distance measuring device 200, and the three-axis posture sensor 70 via the sensor input I / F 63, respectively. Further, signals are input / output to / from the hub 76 via the communication I / F 64, and an audio signal is output to the speaker 78 via the sound output I / F 65.
Next, processing executed by the CPU 60 will be described.

  When the surface data is input from the three-dimensional distance measuring apparatus 200, the CPU 60 performs a coordinate conversion process for converting the coordinates of the sensor coordinate system into the global coordinate system. In the coordinate conversion processing, the global coordinate of the center with respect to the leg wheel type robot 100 is Xc, Yc, Zc, and the posture (roll angle, pitch angle, yaw angle) of the leg wheel type robot 100 is φ, θ, ψ, and the sensor coordinate system. Each measurement point is converted into measurement points (Xs, Ys, Zs) in the global coordinate system.

  Further, inverse kinematics calculation and center-of-gravity calculation are performed based on the sensor signal of the three-axis posture sensor 70 and each measurement point converted into the global coordinate system, and the landing position of the leg tip (drive wheel) is calculated based on the calculation result. To decide. Further, the distance to the object plane is calculated, the positional relationship between the leg tip and the object plane is calculated, and the motor command signals to the drivers 44 and 54 are generated based on the determined landing position and the calculated both distances. A motor command signal is output to the drivers 44 and 54.

Further, it is determined whether or not the leg tip has landed on the object plane. If it is determined that the leg tip has landed, the series of processes is terminated and the process returns to the original process. On the other hand, when it is determined that the leg tip does not land, the above-described series of processes is performed from the process of inputting the surface data.
Next, the operation of the present embodiment will be described with reference to FIGS.
Here, FIG. 11 is a diagram illustrating a measurement result by the first scanning process when there is a staircase in the measurement range. FIG. 12 is a diagram showing a measurement result when the measurement distance in the rotating coordinate system of each measurement point in FIG. 11 is converted into the measurement distance in the orthogonal coordinate system. FIGS. 13A and 13B are diagrams illustrating an example of gradients in the x direction and the z direction calculated using the least square method. FIG. 14 is a diagram illustrating an example of an appearance frequency of a gradient with respect to a certain measurement plane.

First, the three-dimensional distance measuring apparatus 200 sets a scanning angle range and a scanning angle unit in the sensing processor 110 in response to a command signal from the CPU 60 (step S100).
Here, it is assumed that the two-dimensional distance measuring device 112 is a two-dimensional range sensor having a distance measuring range of 20 to 4095 [mm], a maximum scanning angle range of 240 [°], and an angular resolution of 0.36 [°]. If the scanning angle range is 240 [°], it becomes the same as the scanning angle range forming the measurement plane shown in FIG. 3 described above, and the support member, pulley 20 and belt 121 at the top of the two-dimensional distance measuring device 112 are scanned. It will not be included in the range.

  In addition, a scan angle range of 240 [°] and a scan angle unit of 0.36 [°] are set for the first scan process, and a scan angle range of −40 [°] to +40 [+] for the second scan process. [°] and the scanning angle unit 10 [°] are set (in this case, nine measurement planes are formed). If the scanning angle range is −40 [°] to +40 [°], the motor 116 and the encoder 118 provided side by side in the horizontal direction of the two-dimensional distance measuring device 112 are not included in the scanning range.

First, the sensing processor 110 outputs a command signal to the driver 112b based on the scanning angle range and the scanning angle unit (θ) set for the first scanning process.
In the two-dimensional distance measuring device 112, the driver 112 b rotates the rotation shaft of the motor 112 c based on the command signal from the sensing processor 110 and the output signal from the encoder 112 d, thereby causing the distance measuring sensor 112 a to move in the horizontal direction. A process (first scanning process) of rotating around the (y-axis) by a rotation angle “θ = 0.36 [°]” and measuring a distance corresponding to each rotation angle is executed (step S102). Initially, the first scanning process is executed for a position (initial position) where the rotation angle φ is 0 [°] around the vertical axis (z-axis). Further, information on each measurement distance (rotating coordinate system) is output to the sensing processor 110 as a data string L (θ _i , φ _j ).

In the scanning range around the horizontal axis (y-axis), there are no obstacles other than the robot substrate, such as the driving mechanism of the two-dimensional distance measuring device 112, so that the scanning range is within the scanning range. Accurate distance information of existing objects can be obtained.
When the first scanning process on one measurement plane is completed, the sensing processor 110 performs a filtering process using a median filter on the distance information measured in the first scanning process (step S104). Thereby, the noise component in the measurement information is removed.

Here, the measurement distance L [mm] of each measurement point after removing the noise component is, for example, as shown in FIG. In FIG. 11, the horizontal axis represents the number of the measurement point corresponding to each scanning angle (first scanning angle number), and the vertical axis represents the measurement distance L [mm] to the measurement point of each scanning angle number.
In the example shown in FIG. 11, there are stairs on the walking path of the leg-wheel type robot 100 with a constant step and a continuous kick plate.

Next, the sensing processor 110 converts the distance information of the rotating coordinate system after the filtering process into the distance information of the orthogonal coordinate system (step S106).
That is, by converting the measurement distance in the rotational coordinate system of each measurement point shown in FIG. 11 into the measurement distance in the orthogonal coordinate system, as shown in FIG. 12, the distance in the x-axis direction corresponding to each rotation angle [ mm] and z-axis direction distance [mm]. In FIG. 12, the horizontal axis is the distance L _x [mm] in the x-axis direction, and the vertical axis is the distance L _z [mm] in the z-axis direction.

Next, the gradient of each measurement point is calculated based on the distance information converted into the orthogonal coordinate system (step S108).
Here, the gradient of each measurement point is calculated using the least square method. Here, when the least square method is used, as shown in FIGS. 13A and 13B, the gradient in the x direction and the gradient in the z direction are applied to a plurality of measurement points to which the least square method is applied. May be different. Therefore, in this embodiment, the least square method calculation is performed using two linear equations for obtaining the gradients in the x direction and the z direction shown in the following equations (1) and (2).

z = a _x · x + b _x (1)
x = a _z · z + b _z (2)

Then, a _x and b _x in the above equation (1) are calculated according to the following equation (3), and a _z and b _z in the above equation (2) are calculated according to the following equation (4).

When a _x and b _x and a _z and b _z are calculated, the distances between the straight lines of the above formulas (1) and (2) and the respective measurement points used for the least square calculation are substituted. and it calculates a residual h _x and h _z, and the sum of the squares of the residuals h _x wherein for each measuring point, calculates the square sum of residuals h _z (the following formula (5)).

When the square sum of the residual h _{x and} the square sum of the residual h _z are calculated, the straight line with the smaller value is compared as the correct straight line, and based on the slope of this straight line, Obtain the slope of the measurement point. Specifically, _if the square sum of the residual h _x is smaller, “a = a _x , b = b _x ”. _If the square sum of the residual h _z is smaller, “a = 1 / _az”. , B = 1 / b _z ”.

When the gradients of all the measurement points with respect to the measurement plane are calculated as described above, next, the appearance frequency of each gradient is calculated (step S110).
Here, it is assumed that the appearance frequency as shown in FIG. 14 is obtained.
As shown in FIG. 14, if the frequency of occurrence near the gradient is relatively high and the coordinate information of the measurement points corresponding to these gradients is continuous, it can be estimated that there is a horizontal plane there. it can.

  When the appearance frequency is calculated, the sensing processor 110 next selects an appearance frequency of an undetermined gradient (step S112), and determines whether the appearance frequency is equal to or higher than a preset threshold (step S112). S114). If it is equal to or greater than the threshold value ("Yes" branch in step S114), the gradient and the coordinate information of the measurement point are associated with each other and recorded in a RAM (not shown) (step S116). On the other hand, if it is less than the threshold value ("No" branch of step S114), the distance information of the measurement point is excluded as the measurement noise (step S134).

When the above determination processing is completed for all the gradients (the branch of “Yes” in step S118), next, based on the gradient recorded in the RAM, the appearance frequency is equal to or greater than the threshold, and the coordinate information of the measurement points, The presence / absence of a continuous surface is determined (step S120).
On the moving path of the leg-wheel type robot 100, there is a staircase having a constant step and a continuous surface of the kick plate. Appears as a series of coordinates. In such a case, the sensing processor 110 determines that there is a continuous surface (actually, a crossing line between the surface and the measurement plane) (a “Yes” branch in step S122). On the other hand, when the gradient suddenly deviates in the middle of the coordinates, it is determined that the surface (actually, the intersection line between the surface and the scanning plane) is interrupted. From these determination results, the unevenness of the object can be recognized.

Then, the measurement distance of the orthogonal coordinate system corresponding to the continuous surface is converted into the measurement distance of the sensor coordinate system (three-dimensional) and recorded in the RAM (step S124).
When the determination of the continuous surface is completed, the three-dimensional distance measuring apparatus 200 causes the driver 114 to rotationally drive the rotating shaft of the motor 116 based on the command signal from the sensing processor 110 and the output signal from the encoder 118, thereby two-dimensionally. A process (second scanning process) of rotating the distance measuring device 112 about the vertical axis (z axis) by a rotation angle of 10 [°] is executed (step S132). By this second scanning process, the position of the x-axis is changed by 10 [°] around the axis (z-axis) in the vertical direction with respect to the previous position while the z-axis remains unchanged. Then, the two-dimensional distance measuring device 112 executes the first scanning process again in this state (step S102). As a result, a new measurement plane is formed at a position shifted by 10 [°] around the z-axis.

Then, the two-dimensional distance measuring device 112 is rotated by the set scanning angle unit 10 [°] with respect to the set scanning angle range (−40 [°] to +40 [°]), and each rotation angle is set. The first scanning process is executed at φ.
Here, since the two-dimensional distance measuring device 112 is attached to the upper part of the base of the leg-wheel type robot 100, the base is included in the scanning range. Since there are no obstacles in the included range, it can be said that a sufficient scanning range for the walking control of the leg-wheel type robot 100 can be secured.

When the measurement of the distances to the nine measurement planes is completed (“Yes” branch of step S126), surface data is then generated based on the information on the continuous surface recorded in the RAM (step S128).
As described above, the surface data is generated by connecting (corresponding) continuous surfaces having close gradients and coordinates between adjacent measurement planes continuous around the z-axis. The generated surface data is output to the CPU 60 via the hub 76 and the communication I / F 64 (step S130). With this surface data, it is possible to recognize the detailed shape of the constituent surface of an object such as a stair existing within the measurement range.

On the other hand, the CPU 60 analyzes the input surface data, and in particular, when there is a surface having an intersection line whose slope is close to 0, it is regarded as a horizontal surface and is a surface on which the leg-wheel robot 100 can walk. Judge.
When it is determined that the surface indicated by the surface data is a surface that can be walked, the sensor coordinate system of the input surface data is converted to the global coordinate system, and the conversion result and the sensor signal of the three-axis posture sensor 70, etc. Based on the above, the landing position of the leg tip is determined.

  Further, based on the sensor signal from the leg tip sensor 38, for example, when the target is a staircase, the distance to the kick plate and the positional relationship between the leg tip and the tread plate are calculated. Then, a motor command signal is generated based on the determined landing position and the calculated both distances, and the generated motor command signal is output to the driver 44. As a result, the rotary joint is driven, and the leg-wheel type robot 100 gets over the stairs while keeping its posture properly. Depending on the situation, the stairs are avoided and stopped.

In addition, although the steps are constant and the staircase where the kick plate is a continuous surface is given as an example, the staircase where the step is not constant, the staircase where the kick plate is not a continuous surface, the staircase without the kick plate, etc. Since the surface shape can be recognized, the adaptability to the stairs of the leg-wheel type robot 100 can be enhanced.
Further, the leg-wheel type robot 100 can move by wheel running on a flat ground. Therefore, the mobility on a flat ground is high like the wheel type.

  As described above, the leg-wheel type robot 100 according to the present embodiment has a mechanical configuration of the three-dimensional distance measuring device 200 in which the motor 116, the encoder 118, and the two-dimensional distance measuring device 112 are arranged in the horizontal direction. The rotational driving force of the first rotating shaft of the motor 116 is transmitted to the second rotating shaft that rotates the two-dimensional distance measuring device 112 via the pulley 120a, the belt 121, and the pulley 120b. The distance measuring device 112 is configured to rotate around a vertical axis (z axis).

As a result, there is nothing to exclude scanning within the predetermined scanning angle range of the two-dimensional distance measuring device 112, so that accurate distance information can be obtained. Further, since the motor 116 and the encoder 118 and the two-dimensional distance measuring device 112 are arranged in the horizontal direction, the occupation ratio in the height direction of the three-dimensional distance measuring device 200 can be reduced.
Further, in the three-dimensional distance measuring device 200, the distance measuring sensor 112a is rotated by a set scanning angle unit around a horizontal axis, and a first scanning process for measuring a distance at each rotation angle is executed. By calculating the gradient of each measurement point and the appearance frequency of the gradient based on the information of the measurement distance, the unevenness of the object existing in the measurement plane can be recognized.

Thereby, it can be estimated that a continuous surface exists.
Further, every time the first scanning process for one measurement plane is completed, a second scanning process for rotating the two-dimensional distance measuring device 112 around the vertical axis by a set scanning angle unit is executed. Can do. As a result, the first scanning process can be executed on a plurality of measurement planes, so that the surface shape of an object existing within the measurement range can be recognized. Further, since the surface shape can be recognized, the leg wheel type robot 100 can realize high adaptability to an unknown obstacle. In addition, since it can operate in an environment where people are active, it is suitable for human assist robots such as home robots and personal robots that are used for acting with people.

  In the first embodiment, the motor 116 and the encoder 118 correspond to any one of the rotation driving devices of the inventions 2, 3, 4 and 5, and the pulley 116a, the belt 121 and the pulley 120b are used for the motor 116. The mechanism for transmitting the rotational driving force of the first rotational shaft to the second rotational shaft of the two-dimensional distance measuring device 112 corresponds to the power transmission mechanism of the second or third aspect, and the pulleys 120a and 120b are provided on the pulley of the third aspect. Correspond.

  In the first embodiment, the rotation mechanism of the distance measuring sensor 112a by the driver 112b, the motor 112c, and the encoder 112d corresponds to the first rotation mechanism of any one of the inventions 1, 2, 5, and 6. The function of executing the first scanning process by controlling the two-dimensional distance measuring device 112 by the sensing processor 110 corresponds to the first scanning means of the invention 5 or 6, and calculates the gradient of each measurement point in the sensing processor 110. The function for calculating the appearance frequency of each gradient in the sensing processor 110 corresponds to the gradient calculating means of the invention 5 or 6, and the function for calculating the appearance frequency calculating means of the sensing processor 110 is a continuous surface in the sensing processor 110. This determination processing corresponds to the shape recognition means of the fifth aspect.

  In the first embodiment, the rotating mechanism of the two-dimensional distance measuring device 112 using the driver 114, the motor 116, the encoder 118, the speed reducer and the pulley (not shown) is any one of the first, second, and sixth aspects. The function corresponding to the second rotation mechanism and executing the second scanning process by controlling the driver 114 by the sensing processor 110 corresponds to the second scanning means of the invention 6, and the surface data in the three-dimensional distance measuring apparatus 200. The function for generating the surface shape and the function for recognizing the surface shape by the CPU 60 correspond to the shape recognition means of the sixth aspect.

In the first embodiment, step S102 corresponds to the first scanning unit of the invention 5 or 6, steps S104 to S108 correspond to the gradient calculation unit of the invention 5 or 6, and step S110 includes Corresponding to the appearance frequency calculating means of the invention 5 or 6, steps S112 to S124 correspond to the shape recognition means of the invention 5 or 6, and steps S126 to S132 correspond to the second scanning means of the invention 6.
[Second Embodiment]
Next, a second embodiment of the present invention will be described with reference to the drawings. 16 to 22 are diagrams showing a second embodiment of the three-dimensional distance measuring device and the leg-wheel type robot according to the present invention.

In the present embodiment, similar to the first embodiment, the three-dimensional distance measuring device 200 described in the first embodiment is applied to a leg wheel type robot.
In addition to the functions of the leg wheel type robot 100 equipped with the three-dimensional distance measuring device 200 described in the first embodiment, the leg wheel type robot 100 of the present embodiment includes the leg wheel type robot 100 as a base 10. A function of running and moving in an arbitrary advancing direction while maintaining the direction of the head in a fixed direction, and a function of turning the leg wheel type robot 100 without moving back and forth at a predetermined turning center position (super turning) have.

  That is, the leg wheel type robot 100 according to the present embodiment is provided with an installation structure of the force sensor 82 only by adding an actuator control process (control process by executing a control program of the CPU 60) for realizing the above functions. Other configurations are the same as those of the leg-wheel type robot 100 of the first embodiment. Therefore, description of functions similar to those of the first embodiment will be omitted as appropriate, and added functional parts will be described in detail.

First, a detailed configuration of the leg wheel type robot 100 to which the present invention is applied will be described.
FIG. 16 is a front view of the leg wheel type robot 100.
FIG. 17 is a side view of the leg wheel type robot 100.
As shown in FIGS. 16 and 17, the leg-wheel type robot 100 includes a base body 10 and four leg portions 12 coupled to the base body 10.

In front of the base 10, two leg portions 12 are coupled to a symmetrical position via a rotary joint 14. In addition, two legs 12 are connected to the rear side of the base body 10 via a rotary joint 14 at a symmetrical position.
The rotary joint 14 rotates with the direction orthogonal to the bottom surface of the leg wheel type robot 100 as an axial direction. That is, it rotates around the yaw axis.

Each leg portion 12 is provided with two rotary joints 16 and 18. When the rotary joint 14 is in the state as shown in FIG. 16, the rotary joints 16 and 18 rotate with the direction orthogonal to the side surface of the leg wheel type robot 100 as the axial direction. Therefore, each leg 12 has three degrees of freedom.
A driving wheel 20 is rotatably provided at the tip of each leg 12 with the same axial direction as the rotary joints 16 and 18. The drive wheel 20 is rotated around the yaw axis by the rotation of the rotary joint 14. That is, by controlling the rotation of the rotary joint 14, steering control during traveling movement is performed.

A leg tip sensor 38 that measures the distance from the leg tip to an object existing on the movement path of the leg wheel type robot 100 is provided at the tip of each leg 12.
On the other hand, a horizontal laser 26 for irradiating a horizontal plane laser beam is provided at the lower center of the front surface of the substrate 10. In addition, vertical lasers 28 and 30 for irradiating vertical surface laser light are respectively provided on the center left and right of the front surface of the substrate 10.

A camera 32 that captures an image including reflected light of a horizontal plane laser beam and a vertical plane laser beam is provided at the upper center of the front surface of the substrate 10.
Further, as described in the first embodiment, the camera 32 and the three-dimensional distance measuring device 200 are arranged such that the camera 32 is on the upper side and the three-dimensional distance measuring device 200 is on the lower side via the support member and the pulley. It is arranged.

The horizontal laser 26 is provided so as to be inclined downward by a predetermined angle so that the camera 32 can capture an image including the reflected light of the horizontal laser beam. Similarly, the vertical laser 28 is tilted to the right by a predetermined angle so that the camera 32 can capture an image including reflected light of the vertical plane laser beam, and the vertical laser 30 is tilted to the left by a predetermined angle. ing.
Obstacle sensors 34 and 36 for detecting an obstacle are provided on the left and right of the camera 32, respectively.

In addition, as shown in FIG. 10 in the said 1st Embodiment, the joint motor 40 which rotationally drives the rotation joints 14-18 is provided in the rotation joints 14-18 of each leg part 12, respectively. Moreover, as shown in FIG. 10, the wheel motor 50 which rotationally drives the drive wheel 20 is provided in the drive wheel 20 of each leg part 12, respectively.
Next, based on FIG. 18 thru | or FIG. 20, the traveling control process performed with CPU60 of this Embodiment is demonstrated.

Here, FIGS. 18A and 18B are views showing the posture of the leg-wheel type robot 100 when traveling on wheels.
In each of the following travel control processes, each joint motor 40 is controlled so that the posture of the leg-wheel type robot 100 becomes a knee flexion posture as shown in FIG. However, when the leg portions 12 interfere with each other (contact or the like) during running control in the knee flexion posture, the joint motors 40 are controlled so as to be in the knee extension posture as shown in FIG.

First, a travel control process (hereinafter referred to as a non-directed travel control process) for causing the leg-wheel type robot 100 to travel in a target traveling direction while holding (fixing) the orientation of the base body 10 in a certain direction. explain.
The CPU 60 activates a control program stored in a predetermined area such as a ROM, and executes a non-turning traveling control process according to the control program.

  Here, the non-turning traveling control process moves the leg-wheel type robot 100 in the target traveling direction while keeping the direction of the base body 10 in a fixed direction. It is desirable to provide the camera 32, the three-dimensional distance measuring device 200, the obstacle sensors 34, 36, and the like provided in the rear side and the side. When the leg-wheel type robot 100 is moved in a direction different from the direction of the base body 10 by a camera capable of covering the rear and the side, a three-dimensional distance measuring device, an obstacle sensor, and the like, the environment in the traveling direction (terrain) And the like, and appropriate control can be performed.

Further, the non-turning traveling control process is executed when there is a non-turning traveling control command, and the robot traveling direction (angle α) and the robot traveling direction speed Vc are input as the input. The angle (steering angle) of the rotary joint 14 (joint0) of each leg 12 for causing the leg-wheel type robot 100 to travel in the input traveling direction while maintaining the orientation of the base body 10 in a certain direction. θ _i0 (i = 0,1,2,3,...), and the rotational angular velocity ω _i (i = 0,1,2,3,...) of each drive wheel 20 of each leg 12 are calculated. A command is given to each actuator.

As described above, the leg-wheel type robot 100 of the present embodiment has a total of four leg portions 12 of a pair of left and right in front of the base body 10 and a pair of left and right in the rear.
Therefore, here, the steering angle θ _i0 of the driving wheel 20 of each leg 12 is viewed from the upper surface side of the base 10, the left front wheel steering angle θ ₀₀ , the right front wheel steering angle θ ₁₀ , and the left rear wheel steering angle θ _20. The right rear wheel steering angle θ ₃₀ is set. Incidentally, the rotation joint 14 when rotated each leg 12 about the yaw axis, as viewed from the top side of the base body 10, a left front-wheel steering angle theta ₀₀ and the right rear wheel steer angle theta ₃₀ is counterclockwise The direction is a positive direction, and the right front wheel steering angle θ ₁₀ and the left rear wheel steering angle θ ₂₀ are clockwise directions.

Further, the rotational angular velocities ω _i of the drive wheels 20 of the respective leg portions 12 are defined as a left front wheel rotational angular velocity ω ₀ , a right front wheel rotational angular velocity ω ₁ , a left rear wheel rotational angular velocity ω ₂ , and a right rear wheel rotational angular velocity ω ₃ .
Further, the linear velocity V _i (i = 0, 1, 2, 3,...) Of the driving wheel 20 of each leg 12 is set to the left front wheel linear velocity V ₀ , the right front wheel linear velocity V ₁ , and the left rear wheel lane. A speed V ₂ and a right rear wheel linear speed V ₃ are used.
Here, FIGS. 19A to 19C are diagrams illustrating an example of the traveling state of the leg-wheel type robot 100 during the non-turning traveling control. FIGS. 19A to 19C are views of the leg wheel type robot 100 as viewed from the upper surface side, and the black semicircle mark attached to each drive wheel 20 is “θ ₀₀ = θ ₁₀ = θ ₂₀ = θ ₃₀ = 0 [°] ”indicates a reference direction.

In addition, the rotation direction of the left and right front wheels traveling in the direction with the mark is the positive rotation direction, and the rotation direction of the left and right rear wheels traveling in the direction without the mark is the positive rotation direction.
First, based on FIG. 19A, when the leg-wheel type robot 100 is made to travel straight in the direction (forward direction) in which the base body 10 is facing with the orientation of the base body 10 being held (fixed) in a certain direction. The non-turning traveling control process will be described.

In the present embodiment, when the direction in which the base body 10 is facing (forward direction) is the traveling direction of the robot, the angle α representing the traveling direction is set to “0 [°]”. Then, α corresponding to each traveling direction is determined with reference to 0 [°] in the forward direction.
Here, since the leg wheel type robot 100 is caused to advance straight forward of the base body 10, “0 [°]” is input as the traveling direction α, and the traveling direction speed Vc is further input.

When the traveling direction α (0 [°]) and the traveling direction speed Vc are input, the steering angles θ ₀₀ , θ ₁₀ , θ ₂₀ , θ ₃₀ of each driving wheel 20 and the linear speed V ₀ of each driving wheel 20 V ₁ , V ₂ and V ₃ are calculated.
In order to cause the leg wheel type robot 100 to travel straight forward while maintaining the orientation of the base body 10, as shown by the arrows extending from the drive wheels 20 in FIG. It is necessary to align all the traveling directions in the direction in which the base body 10 is facing (forward direction). Accordingly, the steering angles θ ₀₀ , θ ₁₀ , θ ₂₀ , and θ ₃₀ are calculated as, for example, “θ ₀₀ = θ ₁₀ = θ ₂₀ = θ ₃₀ = 0 [°]”.

Further, in this case, the linear velocities V ₀ , V ₁ , V ₂ , and V ₃ of the drive wheels 20 for running straight ahead are “V ₀ = V ₁ = Vc” and “V ₂ = V ₃ = −Vc”. Is calculated.
If the legs 12 do not interfere with each other, for example, “θ ₀₀ = θ ₁₀ = 0 [°]”, “θ ₂₀ = θ ₃₀ = π (180 [°])” or −π (−180 [° ]) ”,“ V ₀ = V ₁ = V ₂ = V ₃ = Vc ”, etc.

Further, the linear velocities V ₀ , V ₁ , V ₂ , V ₃ are converted into rotational angular velocities ω ₀ , ω ₁ , ω ₂ , ω ₃ according to the following equation (6).

ω _i = 2V _i / D (6)

However, in the above formula (6), D is a wheel diameter.

Once the steering angles θ ₀₀ , θ ₁₀ , θ ₂₀ , θ ₃₀ and the rotational angular velocities ω ₀ , ω ₁ , ω ₂ , ω _{3 of the} drive wheels 20 are calculated, 20 current steering angles and rotational angular velocities are obtained. Then, a command value for driving the joint motor 40 of the rotary joint 14 is calculated from the current steering angle and the calculated steering angle for moving straight forward. Further, a command value for driving the wheel motor 50 of the drive wheel 20 is calculated from the current rotation angular velocity and the rotation angular velocity when the vehicle travels straight in the forward direction.

  When the steering control command value and the speed control command value of each drive wheel 20 are calculated in this way, these command values are input to the drivers of the motors. Then, when each joint motor 40 and wheel motor 50 are driven based on the input command value, the steering angle and the rotational angular velocity of each drive wheel 20 of the leg wheel type robot 100 are thereby changed, and the leg wheel type robot is changed. In a state where the orientation of the base body 10 is maintained, the vehicle 100 travels straight in the facing direction (forward direction).

In the case where the leg-wheel type robot 100 is caused to travel straight in the direction opposite to the direction in which the base wheel type robot 100 is directed (directly rearward) with the base 10 kept in a certain direction, What is necessary is just to make the rotation direction of the driving wheel 20 opposite.
For example, “θ ₀₀ = θ ₁₀ = θ ₂₀ = θ ₃₀ = 0 [°]” and “V ₀ = V ₁ = V ₂ = V ₃ = −Vc” or “θ ₀₀ = θ ₁₀ = 0 [°] “Θ ₂₀ = θ ₃₀ = π or −π [°]”, “V ₀ = V ₁ = −Vc”, “V ₂ = V ₃ = Vc”, and the like.

Next, based on FIG. 19B, with the orientation of the base body 10 maintained (ie, with the front facing forward), the leg wheel type robot 100 is tilted forward to the right with respect to the direction it is facing. Next, the non-turning traveling control process when traveling straight ahead will be described.
Here, since the leg-wheel type robot 100 moves straight forward in the diagonally right direction of the base body 10, the traveling direction α is “α (−90 <α <0) [°] (where counterclockwise is the positive direction)”. Is input, and the traveling direction speed Vc is further input.

When the traveling direction α [°] and the traveling direction speed Vc are input, the steering angles θ ₀₀ , θ ₁₀ , θ ₂₀ , θ ₃₀ of each driving wheel 20 and the linear speeds V ₀ , V of each driving wheel 20 are input. ₁ , V ₂ and V ₃ are calculated.
In order to cause the leg wheel type robot 100 to travel straight ahead in the diagonally right direction (α [°] direction) while maintaining the orientation of the base body 10, the arrows extending from the drive wheels 20 in FIG. As shown, it is necessary to align all the traveling directions of the drive wheels 20 in the diagonally forward right direction with respect to the direction in which the base body 10 is directed.

Therefore, the steering angles θ ₀₀ , θ ₁₀ , θ ₂₀ , θ ₃₀ are calculated as, for example, “θ ₀₀ = θ ₃₀ = α [°]”, “θ ₁₀ = θ ₂₀ = −α [°]”.
Further, in this case, the linear velocities V ₀ , V ₁ , V ₂ , and V ₃ of the drive wheels 20 for running straight ahead are “V ₀ = V ₁ = Vc” and “V ₂ = V ₃ = −Vc”. Is calculated.
If the legs 12 do not interfere with each other, for example, “θ ₀₀ = θ ₃₀ = α [°]”, “θ ₁₀ = θ ₂₀ = −α−π [°]”, “V ₀ = V ₂ = Vc ", may be as a combination, such as" V ₁ = V ₃ = -Vc ".

Further, the linear velocities V ₀ , V ₁ , V ₂ , V ₃ are converted into rotational angular velocities ω ₀ , ω ₁ , ω ₂ , ω ₃ according to the above equation (6).
When the steering angles θ ₀₀ , θ ₁₀ , θ ₂₀ , θ ₃₀ and the rotational angular velocities ω ₀ , ω ₁ , ω ₂ , ω _{3 of the} respective drive wheels 20 are calculated, next, the straight traveling in the forward direction is performed. Similarly, the command value for driving the joint motor 40 of the rotary joint 14 and the command value for driving the wheel motor 50 of the drive wheel 20 are calculated.

  When the steering control command value and the speed control command value of each drive wheel 20 are calculated in this way, these command values are input to the drivers of the motors. Then, when each joint motor 40 and wheel motor 50 are driven based on the input command value, the steering angle and the rotational angular velocity of each drive wheel 20 of the leg wheel type robot 100 are thereby changed, and the leg wheel type robot is changed. In a state where the orientation of the base body 10 is maintained, the vehicle 100 travels straight forward in a diagonally forward right direction with respect to the facing direction.

  When the leg-wheel type robot 100 travels straight diagonally to the left and backward with respect to the direction in which the base body 10 is held, each drive wheel 20 in the right diagonal forward direction is used. What is necessary is just to make the rotation direction opposite. Further, when moving straight ahead in the diagonally left direction, the traveling direction α may be set in a range of “0 <α <90 [°]”, and the sign of the steering angle may be opposite. Further, in the case where the vehicle travels straight diagonally to the right, the rotational direction of each drive wheel 20 in the diagonally forward left direction may be opposite to that of the front.

Next, based on FIG. 19 (c), the non-turning traveling control in the case where the leg-wheel type robot 100 travels straight in the left lateral direction with respect to the direction in which the base body 10 is maintained. Processing will be described.
Here, since the leg-wheel type robot 100 is moved straight in the left lateral direction of the base body 10, “α (90 (π / 2)) [°]” is input as the traveling direction α, and the traveling direction velocity Vc is Entered.

When the traveling direction α [°] and the traveling direction speed Vc are input, the steering angles θ ₀₀ , θ ₁₀ , θ ₂₀ , θ ₃₀ of each driving wheel 20 and the linear speeds V ₀ , V of each driving wheel 20 are input. ₁ , V ₂ and V ₃ are calculated.
In order to cause the leg wheel type robot 100 to travel straight in the left lateral direction (direction of π / 2 [°]) with respect to the direction in which the base body 10 is maintained, FIG. ), It is necessary to align all the traveling directions of the driving wheels 20 in the left lateral direction.

Accordingly, the steering angles θ ₀₀ , θ ₁₀ , θ ₂₀ , θ ₃₀ are calculated as, for example, “θ ₀₀ = θ ₁₀ = θ ₂₀ = θ ₃₀ = π / 2 [°]”.
In this case, the linear velocities V ₀ , V ₁ , V ₂ , and V ₃ of the drive wheels 20 for running straight ahead are “V ₀ = V ₂ = Vc” and “V ₁ = V ₃ = −Vc”. Is calculated.
If the legs 12 do not interfere with each other, for example, “θ ₀₀ = θ ₂₀ = π / 2 [°]”, “θ ₁₀ = θ ₃₀ = −π / 2 [°]”, “V ₀ = A combination such as “V ₁ = V ₂ = V ₃ = Vc” may be adopted.

Further, the linear velocities V ₀ , V ₁ , V ₂ , V ₃ are converted into rotational angular velocities ω ₀ , ω ₁ , ω ₂ , ω ₃ according to the above equation (6).
When the steering angles θ ₀₀ , θ ₁₀ , θ ₂₀ , θ ₃₀ and the rotational angular velocities ω ₀ , ω ₁ , ω ₂ , ω _{3 of the} respective drive wheels 20 are calculated, next, the straight traveling in the forward direction is performed. Similarly, the command value for driving the joint motor 40 of the rotary joint 14 and the command value for driving the wheel motor 50 of the drive wheel 20 are calculated.

  When the steering control command value and the speed control command value of each drive wheel 20 are calculated in this way, these command values are input to the drivers of the motors. Then, when each joint motor 40 and wheel motor 50 are driven based on the input command value, the steering angle and the rotational angular velocity of each drive wheel 20 of the leg wheel type robot 100 are thereby changed, and the leg wheel type robot is changed. In a state in which the orientation of the base body 10 is maintained, the vehicle 100 travels straight in the left lateral direction with respect to the facing direction.

When the leg-wheel type robot 100 travels straight in the right lateral direction with respect to the direction in which the base body 10 is held, the rotation of each driving wheel 20 in the left lateral direction is performed. The direction may be opposite.
Next, a description will be given of a travel control process for turning the leg-wheel robot 100 without moving back and forth at a predetermined turning center position (hereinafter referred to as a super turning control process).

The CPU 60 activates a control program stored in a predetermined area such as a ROM, and executes super turning control processing according to the control program.
Here, FIGS. 20 (a) and 20 (b) show the leg-wheel type robot 100 at the time of super-superior turning control with the coordinates (0, 0) and the coordinates (xc, yc) of the base body 10 as the rotation center. It is a figure which shows a driving | running | working state.

20 (a) and 20 (b), in the plane when the base body 10 is viewed from the upper surface side, the longitudinal axis is the x-axis and the direction perpendicular thereto is the y-axis. The coordinates are (x, y) = (0, 0).
The super-revolution turning control process causes the leg wheel type robot 100 to perform a turning operation equivalent to the super-reflex turn performed by a vehicle such as a power shovel or a tank having a crawler mechanism.

Here, super turning is a turning method in which a vehicle having a crawler mechanism rotates left and right crawlers in the opposite directions at the same speed, so that the vehicle body does not move forward and backward and changes the direction of the vehicle body. Is a turning method that can be realized not only in the crawler mechanism but also in a vehicle having at least two independent drive wheels on the left and right.
In the present embodiment, specifically, it is executed when there is a super turning control command, and the turning angular velocity Ω of the robot and the turning center (xc, yc) are used as inputs to turn the leg wheel type robot 100 into the turning center. Angles (steering angles) θ ₀₀ , θ ₁₀ , θ ₂₀ , θ ₃₀ of the rotary joints 14 (joint 0) of the respective leg portions 12 for driving each of the leg portions 12 in order to make a super turn at (xc, yc) The rotational angular velocities ω ₀ , ω ₁ , ω ₂ , and ω ₃ of the wheel 20 are calculated, and commands are given to each actuator.

  In order to cause the leg wheel type robot 100 to make a super turn, when the base body 10 is rotated about the yaw axis at the turning center (xc, yc), the rotation about the yaw axis at the time of steering of each drive wheel 20 is performed. The joint motor 40 of each rotary joint 14 is controlled so that the direction of movement at the contact point between the arc trajectory drawn by the center and the rotation center of each drive wheel 20 matches the traveling direction of each drive wheel 20, and each drive It is necessary to control each wheel motor 50 so that the wheel 20 rotates at a constant speed in the rotation direction corresponding to the movement direction.

First, based on FIG. 20 (a), a description will be given of super turning control processing in the case where the coordinate (0, 0) of the center position of the base body 10 is set as the turning center.
In this case, the turning angular velocity Ω and the turning center coordinates (0, 0) are input.
When the turning angular velocity Ω and the turning center coordinates (0, 0) are input, the angles φ ₀ , φ ₁ , φ formed by the x axis of the base 10 and the rotation center of each drive wheel 20 based on the following equation (7). _2, phi ₃ is calculated.

tanφ = W _t / W _b (7)

However, the above expression (7) is an expression when the center coordinate (0, 0) of the base 10 is set as the turning center. In the above equation (7), W _t is a tread (wheel spacing), and W _b is a wheel base.

Note that φ _{0 to} φ ₃ are the minimum angles among the angles formed between the x-axis and the rotation center of each drive wheel 20, and the positive direction of φ differs from the positive direction of Ω depending on the drive wheels.
Since W _t and W _b are known (information is stored in advance), the angles φ ₀ , φ ₁ , φ ₂ , and φ ₃ can be calculated from the above equation (7).
Since the turning center is the center coordinate (0, 0) of the base body 10, the angles φ ₀ , φ ₁ , φ ₂ , and φ ₃ are all equal angles “φ ₀ = φ ₁ = φ ₂ = φ ₃ = φ. "

In addition, when the base body 10 rotates around the yaw axis at the turning coordinates (0, 0), the movement direction at the contact point between the circular arc drawn by the rotation center around the yaw axis and the rotation center at the time of steering of each drive wheel 20 is: As indicated by the arrow lines extending from each drive wheel 20 in FIG. 20A, the direction is a tangential direction (in the direction of the arrow line in the figure) passing through each rotation center on the circular arc track.
Once the angle “φ ₀ = φ ₁ = φ ₂ = φ ₃ = φ” with respect to the rotation center of each drive wheel 20 is calculated, the movement direction of each rotation center and the traveling direction of each drive wheel 20 are matched. Steering angles θ ₀₀ , θ ₁₀ , θ ₂₀ , and θ ₃₀ are calculated.

In order to make the movement direction of each rotation center coincide with the traveling direction of each driving wheel 20, as shown in FIG. 20A, a line connecting the traveling direction of each driving wheel 20, the turning center and the rotation center. It is only necessary to perform steering so that the angle formed by the minute is a right angle (π / 2 (90 [°])). Therefore, the steering angles θ ₀₀ , θ ₁₀ , θ ₂₀ , and θ ₃₀ are “θ ₀₀ = θ ₁₀ = Θ ₂₀ = θ ₃₀ = − (π / 2−φ) ”.
On the other hand, the distances L ₀ , L ₁ , L ₂ , L ₃ between the rotation center around the yaw axis and the turning center (0, 0) when each drive wheel 20 is steered are calculated based on the following equation (8). .

However, the above equation (8) is an equation in the case where the center coordinate (0, 0) of the base body 10 is set as the turning center.
From the tread W _t and the wheel base W _b , the coordinates of the rotation center of the left front wheel are (W _b / 2, W _t / 2), and the coordinates of the rotation center of the right front wheel are (W _b / 2, −W _t / 2) The coordinates of the center of rotation of the left rear wheel are represented as (−W _b / 2, W _t / 2), and the coordinates of the center of rotation of the right rear wheel are represented as (−W _b / 2, −W _t / 2). Can do.

Since the turning center is the center coordinate (0, 0) of the base body 10, the distances L ₀ , L ₁ , L ₂ , L ₃ are equal distances “L ₀ = L ₁ = L ₂ = L ₃ = L”. It becomes.
Once the distances L ₀ , L ₁ , L ₂ , and L ₃ are calculated, the linear velocities V ₀ , V ₀ , and V of each drive wheel 20 are then calculated from these distances L and the turning angular velocity Ω based on the following equation (9). V ₁ , V ₂ and V ₃ are calculated.

V ₀ = V ₁ = V ₂ = V ₃ = LΩ (9)

However, the above equation (9) is an equation in the case where the center coordinate (0, 0) of the base body 10 is set as the turning center.

Here, as described above, the rotation direction of the left and right front wheels traveling in the direction with the black semicircle mark is the positive rotation direction, and the rotation direction of the left and right rear wheels traveling in the direction without the mark is the positive rotation direction.
Further, since the directions of the drive wheels 20 are as shown in FIG. 20A, the linear velocities V ₀ , V ₁ , V ₂ , and V ₃ are “V ₀ = V ₃ = −LΩ”, “V ₁ = V ₂ = LΩ ”is calculated.
Further, the linear velocities V ₀ , V ₁ , V ₂ , V ₃ are converted into rotational angular velocities ω ₀ , ω ₁ , ω ₂ , ω ₃ according to the above equation (6).

Once the steering angles θ ₀₀ , θ ₁₀ , θ ₂₀ , θ ₃₀ and the rotational angular velocities ω ₀ , ω ₁ , ω ₂ , ω ₃ of each drive wheel 20 are calculated, the current steering of each drive wheel 20 is then performed. Get the angle and rotational angular velocity. Then, a command value for driving the joint motor 40 of the rotary joint 14 is calculated from the current steering angle and the calculated steering angle. Further, a command value for driving the wheel motor 50 of the drive wheel 20 is calculated from the current rotation angular velocity and the calculated rotation angular velocity.

  When the steering control command value and the speed control command value of each drive wheel 20 are calculated in this way, these command values are input to the drivers of the motors. Then, when each joint motor 40 and wheel motor 50 are driven based on the input command value, the steering angle and the rotational angular velocity of each drive wheel 20 of the leg wheel type robot 100 are thereby changed, and the center of the base body 10 is changed. With the coordinates (0, 0) as the turning center, the leg-wheel type robot 100 turns on the spot without making a back-and-forth movement.

Next, the super turning control process when coordinates other than the center coordinate (0, 0) of the base 10 are set as the turning center will be described. Hereinafter, this super turning control process is referred to as a turning center offset type super turning control process.
In this case, the turning angular velocity Ω and turning center coordinates (xc, yc) ≠ (0, 0) which are coordinates other than the center coordinates (0, 0) of the base body 10 are input.

When the turning angular velocity Ω and the turning center coordinates (xc, yc) are input, the angles φ ₀ , φ ₁ formed by the x axis of the base 10 and the rotation center of each drive wheel 20 based on the following equation (10). , Φ ₂ , φ ₃ are calculated.

tan φ ₀ = (W _t / 2-yc) / (W _b / 2-xc)
tan φ ₁ = (W _t / 2 + yc) / (W _b / 2-xc)
tanφ ₂ = (W _t / 2-yc) / (W _b / 2 + xc)
tan φ ₃ = (W _t / 2 + yc) / (W _b / 2 + xc) (10)

However, the above equation (10) is an equation in the case where the leg portions 12 of the leg-wheel type robot 100 are a total of four pairs of left and right pairs in front of the base 10 and left and right pairs in the rear.

Specifically, the input turning center coordinates (xc, yc) and known W _t and W _b are substituted into the above equation (10), and the x axis of the base 10 and the rotation center of each driving wheel 20 are substituted. Are calculated. Φ ₀ , φ ₁ , φ ₂ , φ ₃ are calculated.
Since the turning center is a coordinate other than the center coordinate of the base body 10, the angles φ ₀ , φ ₁ , φ ₂ , and φ ₃ are different from each other.

  Further, when the base body 10 rotates around the yaw axis at the turning center coordinates (xc, yc), the direction of motion at the contact point between the circular arc drawn by the rotation center around the yaw axis and the rotation center at the time of steering of each drive wheel 20. As shown by the arrow lines extending from the drive wheels 20 in FIG. 20 (b), they are tangential directions (in the direction of the arrow lines in the figure) passing through the rotation centers on the circular arc tracks. Further, since the distance between the turning center and the rotation center of each drive wheel 20 is different, the arc trajectory drawn by each rotation center is also different.

Once the angles φ ₀ , φ ₁ , φ ₂ , and φ ₃ with respect to the rotation center of each drive wheel 20 are calculated, next, steering for making the movement direction of each rotation wheel coincide with the traveling direction of each drive wheel 20. The angles θ ₀₀ , θ ₁₀ , θ ₂₀ , and θ ₃₀ are calculated.
In order to make the movement direction of each rotation center coincide with the traveling direction of each drive wheel 20, as shown in FIG. 20B, a line connecting the rotation direction of each drive wheel 20, the turning center and the rotation center. It is only necessary to steer so that the angle formed by the minute becomes a right angle (π / 2 (90 [°])). Therefore, the steering angles θ ₀₀ , θ ₁₀ , θ ₂₀ , and θ ₃₀ are “θ _i0 = − ( π / 2-φ _i) is calculated as (i = 0, 1, 2, 3) ".

On the other hand, the distances L ₀ , L ₁ , L ₂ , L ₃ between the rotation center around the yaw axis and the turning center (xc, yc) during the steering of each drive wheel 20 are calculated based on the following equation (11). .

As described above, since the turning center is a coordinate other than the center coordinates (0, 0) of the base body 10, the distances L ₀ , L ₁ , L ₂ , and L ₃ are different distances.
Once the distances L ₀ , L ₁ , L ₂ , L ₃ are calculated, the linear velocities V ₀ , V ₀ of the drive wheels 20 are then calculated from these distances and the turning angular velocity Ω based on the following equation (12). ₁ , V ₂ and V ₃ are calculated.

| V _i | = | L _i Ω | (12)

Therefore, the linear velocities V ₀ , V ₁ , V ₂ , V ₃ are “V ₀ = −L ₀ Ω”, “V ₁ = L ₁ Ω”, “V ₂ = L ₂ Ω”, “V ₃ = − L ₃ Ω ”.

Further, the linear velocities V ₀ , V ₁ , V ₂ , V ₃ are converted into rotational angular velocities ω ₀ , ω ₁ , ω ₂ , ω ₃ according to the above equation (6).
Once the steering angles θ ₀₀ , θ ₁₀ , θ ₂₀ , θ ₃₀ and the rotational angular velocities ω ₀ , ω ₁ , ω ₂ , ω ₃ of each drive wheel 20 are calculated, the current steering of each drive wheel 20 is then performed. Get the angle and rotational angular velocity. Then, a command value for driving the joint motor 40 of the rotary joint 14 is calculated from the current steering angle and the calculated steering angle. Further, a command value for driving the wheel motor 50 of the drive wheel 20 is calculated from the current rotation angular velocity and the calculated rotation angular velocity.

  When the steering control command value and the speed control command value of each drive wheel 20 are calculated in this way, these command values are input to the drivers of the motors. Then, when each joint motor 40 and wheel motor 50 are driven based on the input command value, the steering angle and the rotational angular velocity of each drive wheel 20 of the leg wheel type robot 100 are thereby changed. The leg wheel type robot 100 turns (super-confidence turning) with the coordinates (xc, yc) other than the position (0, 0) as the turning center.

Next, based on FIG.21 and FIG.22, operation | movement of this Embodiment is demonstrated.
Here, FIGS. 21A and 21B are diagrams illustrating an example of a travel route of the leg-wheel type robot 100. FIG. FIG. 22 is a diagram illustrating an example of the position of the center of gravity of the robot.
When the leg-wheel type robot 100 confirms that an obstacle (here, a staircase) exists on the movement path of the leg-wheel type robot 100 by the obstacle sensors 34 and 36, the three-dimensional distance measuring device 200 The distance measurement and surface data generation processing described in the first embodiment is performed, and the generated surface data is input to the CPU 60 via the hub 76 and the communication I / F 64. On the other hand, the CPU 60 analyzes the input surface data, and in particular, when there is a surface having an intersection line whose slope is close to 0, the CPU 60 regards it as a horizontal surface and is a surface on which the leg-wheel robot 100 can walk. Judge. When it is determined that the surface indicated by the surface data is a surface that can be walked, the sensor coordinate system of the input surface data is converted to the global coordinate system, and the conversion result and the sensor signal of the three-axis posture sensor 70, etc. Based on the above, the landing position of the leg tip is determined. Further, based on the sensor signal from the leg tip sensor 38, the distance to the kick plate and the positional relationship between the leg tip and the tread board are calculated. Then, a motor command signal is generated based on the determined landing position and the calculated both distances, and the generated motor command signal is output to the driver 44. As a result, each joint motor is driven, and the leg wheel type robot 100 gets over the stairs while keeping its posture properly. Depending on the situation, the stairs are avoided and stopped. Therefore, the adaptability to the stairs is high like the legged robot.

On the other hand, when the leg-wheel type robot 100 determines that there is no obstacle on its own movement path based on the sensor signals of the obstacle sensors 34 and 36 (flat ground), the leg-wheel type robot 100 sets the movement mode to the leg portion. 12 is switched from the leg movement mode using 12 to the wheel traveling movement mode using drive wheels 20.
On flat ground, the leg-wheel type robot 100 can move in the above-mentioned non-turning traveling. Furthermore, the above-described super-spinning and turning center offset type super-spinning can be performed.

When the mode is switched to the wheel traveling mode, the leg wheel type robot 100 controls each joint motor 40 of the leg 12 to shift to the knee flexion posture. Then, when the leg-wheel type robot 100 shifts to the knee bending posture, various traveling controls are started.
First, the operation of the leg wheel type robot 100 during the non-turning traveling control will be described.
Here, it is assumed that the leg wheel type robot 100 travels and moves along a passage as shown in FIG. FIGS. 21A and 21B are overhead views of a part of the travel route as viewed from directly above.

As shown in FIG. 21 (a), the width of the passage is initially narrow enough to pass through in the direction (front direction) in which the base body 10 is facing, so first, before entering the passage, The direction in which the passage extends and the direction of the base 10 are matched, and the entry position and the entry angle are adjusted.
Then, the non-turning traveling control command is input, and the leg wheel type robot 100 is shifted to the non-turning traveling control mode. As a result, the leg wheel type robot 100 controls the CPU 60 to travel and move in the target traveling direction while maintaining the orientation of the base 10.

First, since the leg wheel type robot 100 is desired to move straight forward, the robot traveling direction α = 0 [°] and the robot traveling direction speed Vc are input. Thereby, the steering angle of each drive wheel 20 is calculated as “θ ₀₀ = θ ₁₀ = θ ₂₀ = θ ₃₀ = 0 [°]”, and the linear velocity of each drive wheel 20 is “V ₀ = V ₁ = V _2”. = V ₃ = Vc ”.
Further, the linear velocities V ₀ , V ₁ , V ₂ , V ₃ are converted into rotational angular velocities ω ₀ , ω ₁ , ω ₂ , ω ₃ according to the above equation (6).

When the CPU 60 calculates the steering angles θ ₀₀ , θ ₁₀ , θ ₂₀ , θ ₃₀ , and the rotational angular velocities ω ₀ , ω ₁ , ω ₂ , ω ₃ of the drive wheels 20, the angle capture I / F 62 is then obtained. The current steering angle and rotational angular velocity of each drive wheel 20 are acquired. Here, a difference value between the acquired steering angle and the calculated steering angle is calculated, and a command value for driving the joint motor 40 of the rotary joint 14 is calculated based on the difference value. Further, a difference value between the acquired current rotation angular velocity and the calculated rotation angular velocity is calculated, and a command value for driving the wheel motor 50 of the drive wheel 20 is calculated based on the difference value.

  The CPU 60 inputs the calculated steering control command value and speed control command value of each drive wheel 20 to the driver of each motor. With this command value, each joint motor 40 is driven, and the rotary joint 14 of each leg 12 rotates about the yaw axis to change to a target steering angle. Thereafter, each wheel motor 50 is driven, and each drive wheel 20 is rotationally driven at a rotational angular velocity corresponding to the command value. As a result, the leg wheel type robot 100 enters the passage in the direction (front direction) in which the base body 10 is facing while keeping the orientation of the base body 10 and travels straight in the passage.

When the leg-wheel type robot 100 goes straight forward for a while, the passage bends to the right at a substantially right angle and further widens the road width. Control to move straight in the direction. That is, α = −90 (−π / 2) [°] is input as the traveling direction, and further the traveling direction velocity Vc is input.
Thereby, for example, “θ ₀₀ = θ ₁₀ = θ ₂₀ = θ ₃₀ = α [°]” is calculated as the steering angle, and “V ₀ = V ₂ = −Vc”, “V ₁ = V ₃ = Vc ”is calculated.

Further, the linear velocities V ₀ , V ₁ , V ₂ , V ₃ are converted into rotational angular velocities ω ₀ , ω ₁ , ω ₂ , ω ₃ according to the above equation (6).
The CPU 60 acquires the current steering angle and rotational angular velocity of each drive wheel 20, and calculates each command value from these and the calculated steering angle and rotational angular velocity. By this command value, each joint motor 40 and each wheel motor 50 are driven, so that the leg wheel type robot 100 holds the direction of the base body 10 in the right lateral direction with respect to the direction in which it is directed. Drive straight ahead. Thus, the leg wheel type robot 100 can be caused to travel to the bent point without turning at the corner of the passage.

When the leg wheel type robot 100 goes straight to the right side for a while, the passage bends diagonally downward to the right. Control to move straight. That is, α (passage angle <0) [°] is input as the traveling direction, and further the traveling direction velocity Vc is input.
Thereby, “θ ₀₀ = θ ₃₀ = π + α [°]” and “θ ₁₀ = θ ₂₀ = − (π + α) [°]” are calculated as the steering angle, and “V ₀ = V ₁ = -Vc "and" V ₂ = V ₃ = Vc "is calculated.

Further, the linear velocities V ₀ , V ₁ , V ₂ , V ₃ are converted into rotational angular velocities ω ₀ , ω ₁ , ω ₂ , ω ₃ according to the above equation (6).
Then, the current steering angle and rotation angular velocity of each drive wheel 20 are acquired, and each command value is calculated from these and the calculated steering angle and rotation angular velocity. By this command value, each joint motor 40 and each wheel motor 50 are driven, so that the leg wheel type robot 100 holds the orientation of the base body 10 in the diagonally rightward direction with respect to the direction in which the base 10 is facing. Go straight ahead.

Next, the operation of the leg wheel type robot 100 at the time of super-trust turning control will be described.
Here, it is assumed that the leg wheel type robot 100 travels and moves along a passage as shown in FIG.
As shown in FIG. 21 (b), the passage extends straight at first, then turns to the right at a substantially right angle, and reaches a dead end after that.

First, the direction in which the passage extends and the direction of the base body 10 are matched and the approach position is finely adjusted, and then the leg-wheel type robot 100 is caused to travel straight ahead in the forward direction. Thereby, the leg-wheel type robot 100 enters the passage and travels straight in the passage.
When the leg wheel type robot 100 goes straight for a while, the leg wheel type robot 100 eventually reaches the turning corner, and therefore turns clockwise to change the direction of the base body 10 so that the leg wheel type robot 100 can advance.

  If it is the corner shown in FIG. 21 (b), it is possible to make a right turn even with a turning motion accompanied by a forward movement, but here it is assumed that a right turn is made by a super turn. For this reason, a super-trust turning control command is input, and the leg wheel type robot 100 is shifted to the super-trust turning control mode by this command. As a result, the leg wheel type robot 100 performs a control process in which the CPU 60 turns the robot at a predetermined turning center position without moving back and forth.

First, a turning angular velocity Ω (an angular velocity for rotating the base body 10 clockwise) and a turning center coordinate (0, 0) are input. Further, since the right turn is performed at a substantially right angle, a turning angle of −90 [°] is input.
When the turning angular velocity Ω, the turning center coordinates (0, 0), and the turning angle (−90 [°]) are input, the x-axis of the base 10 and the rotation center of each drive wheel 20 are determined based on the above equation (7). Are calculated. Φ ₀ , φ ₁ , φ ₂ , φ ₃ are calculated.

Once the angle “φ ₀ = φ ₁ = φ ₂ = φ ₃ = φ” with respect to the rotation center of each drive wheel 20 is calculated, the movement direction of each rotation center and the traveling direction of each drive wheel 20 are matched. Steering angles θ ₀₀ , θ ₁₀ , θ ₂₀ , and θ ₃₀ are calculated.
Specifically, for the steering angles θ ₀₀ , θ ₁₀ , θ ₂₀ , and θ ₃₀ , “θ ₀₀ = θ ₁₀ = θ ₂₀ = θ ₃₀ = − (π / 2−φ)” is calculated.

Further, based on the above equation (8), distances L ₀ , L ₁ , L ₂ , L ₃ between the rotation center around the yaw axis and the turning center (0, 0) at the time of steering of each drive wheel 20 are calculated. .
Since the turning center is the center coordinates (0, 0) of the base body 10, the distances L ₀ , L ₁ , L ₂ , L ₃ are equal distances “L ₀ = L ₁ = L ₂ = L ₃ = L”. It becomes.
Next, the linear velocities V ₀ , V ₁ , V ₂ , V ₃ of the drive wheels 20 are calculated from the distance L and the turning angular velocity Ω based on the above equation (9).

Here, since the leg-wheel type robot 100 is turned clockwise (Ω <0), the linear velocities V ₀ , V ₁ , V ₂ , V ₃ are “V ₁ = V ₂ = LΩ”, “V ₀ = V ₃ = −LΩ ”.
Further, the linear velocities V ₀ , V ₁ , V ₂ , V ₃ are converted into rotational angular velocities ω ₀ , ω ₁ , ω ₂ , ω ₃ according to the above equation (6).
Once the steering angles θ ₀₀ , θ ₁₀ , θ ₂₀ , θ ₃₀ and the rotational angular velocities ω ₀ , ω ₁ , ω ₂ , ω _{3 of the} drive wheels 20 are calculated, then, via the angle capture I / F 62. Thus, the current steering angle and rotational angular velocity of each drive wheel 20 are acquired. Here, a difference value between the acquired steering angle and the calculated steering angle is calculated, and a command value for driving the joint motor 40 of the rotary joint 14 is calculated based on the difference value. Further, a difference value between the acquired current rotation angular velocity and the calculated rotation angular velocity is calculated, and a command value for driving the wheel motor 50 of the drive wheel 20 is calculated based on the difference value.

  The CPU 60 inputs the calculated steering control command value and speed control command value of each drive wheel 20 to the driver of each motor. With this command value, each joint motor 40 is driven, and the rotary joint 14 of each leg 12 rotates about the yaw axis to change to a target steering angle. Thereafter, each wheel motor 50 is driven, and each drive wheel 20 is rotationally driven at a rotational angular velocity corresponding to the command value. As a result, the leg-wheel type robot 100 turns 90 [°] clockwise (superficial turning) on the spot without moving back and forth with the center coordinate (0, 0) of the base body 10 as the turning center.

When the leg wheel type robot 100 turns 90 [°] clockwise and turns right, the robot travels forward in the forward direction along the passage.
As shown in FIG. 21B, the right-turned passage is a dead end, so the leg-wheel type robot 100 eventually reaches the dead end of the passage.
Since the leg-wheel type robot 100 cannot proceed any further (ascertained by various sensors), the leg-wheel type robot 100 turns 180 [°] and turns back the passage.

Here, since the width of the passage is not wide enough to make a turning motion (for example, U-turn) with back-and-forth movement, as in the case of the right turn, the mode shifts to the super turning control mode, It turns 180 [°], changes the direction of the leg wheel type robot 100, and turns it back.
Further, as shown in FIG. 21 (b), since the passage width is the minimum width of rotation, it is necessary to turn the leg wheel type robot 100 with the minimum turning radius. Therefore, the turning center coordinates (0, 0), the turning angular velocity Ω (the angular velocity for rotating the base 10 clockwise), and the turning angle 180 [°] are input. Note that the posture of each leg 12 of the leg wheel type robot 100 is changed to the knee extension posture so that the leg 12 does not hit the passage.

When the turning center coordinates (0, 0), turning angular velocity Ω, and turning angle 180 [°] are input, the steering angles θ ₀₀ , θ ₁₀ , θ ₂₀ , θ _{30 are set} as “θ _{00 = θ 10 = θ 20 =} θ 30 = - (π / 2-φ) "is calculated as the linear velocity _{V 0, V 1, V 2} , V 3, " V ₁ = V ₂ = LΩ "," V ₀ = V ₃ = −LΩ ”is calculated.
Further, the linear velocities V ₀ , V ₁ , V ₂ , V ₃ are converted into rotational angular velocities ω ₀ , ω ₁ , ω ₂ , ω ₃ according to the above equation (6).

When the steering angles θ ₀₀ , θ ₁₀ , θ ₂₀ , θ ₃₀ and the rotational angular velocities ω ₀ , ω ₁ , ω ₂ , ω _{3 of the} drive wheels 20 are calculated, the command values are calculated, and the joint motor 40 and The wheel motor 50 is driven.
As a result, the leg wheel type robot 100 turns 180 [°] clockwise (super turning) on the spot without moving back and forth, with the center coordinate (0, 0) of the base body 10 as the turning center, and in the back direction. Change direction.

  Here, for example, when the leg-wheel type robot 100 is travel-controlled manually by remote operation, or when it is automatically controlled and has various sensors and cameras on the back side of the base body 10, no change is made. By running control, it is also possible to run straight back while turning forward and turn the path back. However, in the case of automatic control and when various sensors are provided only on the front side of the base body 10, it is necessary to match the direction of the base body 10 with the traveling direction. Accordingly, in the latter case, the super-symbol turning is a useful turning means.

Next, the operation of the leg-wheel type robot 100 at the time of turning center offset type super-revolution turning control will be described.
Now, it is assumed that the mode is shifted to the wheel traveling mode, each joint motor 40 of the leg wheel type robot 100 is controlled, and the posture of the robot is changed to the knee flexion posture. At this time, it is assumed that the center of gravity of the robot becomes a position deviated from the coordinates of the center position of the base 10 as shown in FIG.

In this case, since the position of the center of gravity is deviated from the coordinates (0, 0) of the center position of the base body 10, if super turning is performed with the center position as the turning center, the turning becomes unbalanced and a problem may occur. There is.
In such a case, in the super-revolution turn control command mode of the present embodiment, since the super-revolution can be made at an arbitrary turning center, the turning angular velocity Ω is input and the leg wheel in the base body 10 is input. The coordinates (xg, yg) corresponding to the center of gravity position of the robot 100 are input as the turning center coordinates (xc, yc). When the turning angular velocity Ω and the turning center coordinates (xc, yc) = (xg, yg) are input, the x axis of the base 10 and the rotation center of each drive wheel 20 are formed based on the above equation (10). The angles φ ₀ , φ ₁ , φ ₂ , φ ₃ are calculated.

Once the angles φ ₀ , φ ₁ , φ ₂ , and φ ₃ with respect to the rotation center of each drive wheel 20 are calculated, next, steering for making the movement direction of each rotation wheel coincide with the traveling direction of each drive wheel 20. The angles θ ₀₀ , θ ₁₀ , θ ₂₀ , and θ ₃₀ are calculated.
The steering angles θ ₀₀ , θ ₁₀ , θ ₂₀ , and θ ₃₀ are calculated as “θ _i0 = − (π / 2−φ _i ) (i = 0, 1, 2, 3)”.

Next, distances L ₀ , L ₁ , L ₂ , and L ₃ between the rotation center around the yaw axis and the turning center (xc, yc) during steering of each drive wheel 20 are calculated based on the above equation (11). .
Once the distances L ₀ , L ₁ , L ₂ , L ₃ are calculated, the linear velocities V ₀ , V ₀ of each drive wheel 20 are then calculated from these distances and the turning angular velocity Ω based on the above equation (12). ₁ , V ₂ and V ₃ are calculated.
Here, assuming that the leg-wheel type robot 100 is turned counterclockwise (Ω> 0), the linear velocities V ₀ , V ₁ , V ₂ , V ₃ are “V ₀ = −L ₀ Ω”, “V ₁ = L ₁ Ω ”,“ V ₂ = L ₂ Ω ”, and“ V ₃ = −L ₃ Ω ”.

Further, the linear velocities V ₀ , V ₁ , V ₂ , V ₃ are converted into rotational angular velocities ω ₀ , ω ₁ , ω ₂ , ω ₃ according to the above equation (6).
Once the steering angles θ ₀₀ , θ ₁₀ , θ ₂₀ , θ ₃₀ and the rotational angular velocities ω ₀ , ω ₁ , ω ₂ , ω _{3 of the} drive wheels 20 are calculated, then, via the angle capture I / F 62. Thus, the current steering angle and rotational angular velocity of each drive wheel 20 are acquired. Here, a difference value between the acquired steering angle and the calculated steering angle is calculated, and a command value for driving the joint motor 40 of the rotary joint 14 is calculated based on the difference value. Further, a difference value between the acquired current rotation angular velocity and the calculated rotation angular velocity is calculated, and a command value for driving the wheel motor 50 of the drive wheel 20 is calculated based on the difference value.

  The CPU 60 inputs the calculated steering control command value and speed control command value of each drive wheel 20 to the driver of each motor. By this command value, each joint motor 40 and each wheel motor 50 are driven, whereby the leg wheel type robot 100 is balanced with the coordinates (xg, yg) corresponding to the center of gravity coordinates in the base body 10 as the turning center. Turn the superstrate counterclockwise in the state.

  Thus, in the present embodiment, the mechanical configuration of the three-dimensional distance measuring device 200 is arranged such that the motor 116 and the encoder 118 and the two-dimensional distance measuring device 112 are arranged in the horizontal direction. The rotational driving force of the first rotating shaft is transmitted to the second rotating shaft that rotates the two-dimensional distance measuring device 112 via the pulley 120a, the belt 121, and the pulley 120b, and the two-dimensional distance measuring device 112 is moved vertically. It was set as the structure which rotates around the direction axis | shaft (y-axis).

As a result, there is nothing to exclude scanning within the predetermined scanning angle range of the two-dimensional distance measuring device 112, so that accurate distance information can be obtained. Further, since the motor 116 and the encoder 118 and the two-dimensional distance measuring device 112 are arranged in the horizontal direction, the occupation ratio in the height direction of the three-dimensional distance measuring device 200 can be reduced.
Furthermore, in the present embodiment, the three-dimensional distance measuring apparatus 200 can recognize the unevenness of the object existing in the measurement plane. Thereby, it can be estimated that a continuous surface exists. Furthermore, since the first scanning process can be performed on a plurality of measurement planes, the surface shape of an object existing within the measurement range can be recognized.

As a result, the distance to the obstacle (including the wall of the passage) and the shape of the obstacle can be accurately grasped, so that the movement of the leg wheel type robot 100 such as avoiding the obstacle and traveling in the passage is performed. Can be performed more safely and reliably.
Further, in the present embodiment, the joint motor 40 and the wheel motor 50 are controlled so that the leg-wheel type robot 100 travels in the target traveling direction while maintaining the orientation of the base body 10 in a certain direction.

Thereby, since it is possible to move in any direction without turning (without changing the direction), it is possible to realize quick movement in each direction, and it is impossible to make a turn because each component of the leg-wheel type robot 100 is hindered. Even in areas where it is difficult to change the direction of the leg-wheel type robot, such as a narrow and intricate area, activities can be performed.
Further, in the present embodiment, the joint motor 40 and the wheel motor 50 are controlled so that the leg wheel type robot 100 turns (super turning) without moving back and forth at a predetermined turning center position. At this time, control using the coordinates of the center position on the base body 10 as the turning center coordinates (super-symbol turning control) and control using coordinates other than the center position on the base body 10 as the turning center coordinates (turning center offset type super-sponsored turning) Control).

  Thereby, when the center position of the base body 10 is set as the turning center position, the leg-wheel type robot can be turned with the minimum turning radius. Further, when the center position other than the center position of the base body 10 is set as the turning center position, when the center position of the base body 10 and the center of gravity position are different, the center wheel position can be used as a turning center, so that the super wheel can be turned. It is possible to turn the robot 100 in a balanced manner.

  In the second embodiment, the three-dimensional distance measuring device 200 corresponds to the three-dimensional distance measuring device of inventions 1 to 6, and the leg-wheel type robot 100 corresponds to the leg-wheel type robot of inventions 8 to 10. The drive wheel 20 corresponds to the wheels of the inventions 7 to 9, the joint motor 40 corresponds to the first actuator and the second actuator of the inventions 8 to 10, and the CPU 60 corresponds to the control means of the inventions 8 to 10. ing.

In the first and second embodiments, the encoder 118 is provided on the first rotating shaft of the motor 116. However, the present invention is not limited to this, and the second rotating shaft of the two-dimensional distance measuring device 112 is used. The encoder may be provided with an encoder.
As a result, the scanning angle in the second scanning process can be detected with high accuracy without being affected by the transmission error.

In the first and second embodiments, the rotational driving force of the first rotating shaft of the motor 116 is driven to rotate the two-dimensional distance measuring device 112 via the pulley 20 and the belt 121. However, the present invention is not limited to this, and the configuration may be such that the rotational driving force of the first rotating shaft is transmitted to the second rotating shaft via a plurality of gears.
In the first and second embodiments, the distance information in the first scanning process is acquired discretely (the distance information is acquired in the first scanning process after being rotationally driven in the second scanning process). However, the present invention is not limited to this, and as a configuration in which the scanning angle and the measurement distance are associated with each other continuously (distance information is acquired by the first scanning process while being rotationally driven by the second scanning process). Also good.

In the first and second embodiments, the CPU 60 recognizes the surface shape of the object based on the surface data generated by the three-dimensional distance measuring apparatus 200. However, the present invention is not limited to this. The dimension distance measuring apparatus 200 may perform the process up to the recognition of the surface shape, and the CPU 60 may output data indicating the recognition result of the surface shape.
In the first and second embodiments, the scanning angle range and the scanning angle unit are set based on the command signal from the CPU 60. However, the present invention is not limited to this. Another configuration such as presetting may be used.

In the first and second embodiments, the example in which the three-dimensional distance measuring device 200 according to the present invention is applied to a leg-wheel type robot has been described. The dimensional distance measuring apparatus 200 can also be applied to a robot having another configuration, an apparatus that recognizes the outside world of an automobile, an apparatus that assists a visually impaired person to recognize the outside world, and the like.
In the first and second embodiments, the three-dimensional distance measuring device 200 is driven by rotating the distance measuring sensor 112a around the horizontal axis (y-axis). However, the present invention is not limited to this configuration, and the two-dimensional distance measuring device 112 is rotated around the horizontal axis by rotating the distance measuring sensor 112a around the vertical axis. It is good also as a structure driven to rotate. Further, not only the horizontal and vertical axes, but any two axes that are orthogonal to the measurement direction of the distance measuring sensor 112a may be used.

In the first and second embodiments, the two-dimensional distance measuring device 112 itself is rotated as shown in FIG. 15A. However, the present invention is not limited to this, and an optical distance measuring sensor is used. As shown in FIG. 15B, a mirror inserted on the optical axis in the measurement direction may be rotated.
In the first and second embodiments, the configuration of the leg-wheel type robot 100 according to the present invention is such that the base 10 has a pair of four legs 12 on the front side and a pair of left and right sides on the rear side. However, the present invention is not limited to this, such as a configuration in which a pair of left and right legs 12 is provided in the center of the base 10, a configuration in which three legs 12 are provided symmetrically, a configuration in which five or more legs 12 are provided, and the like. Other configurations may be used without departing from the spirit of the invention. In the case of a multi-legged configuration, the unnecessary leg 12 may be controlled so as not to be used for traveling control.

1 is a side view of a leg wheel type robot 100 according to the present invention. (A) is a figure which shows the mechanical structure of the three-dimensional distance measuring apparatus 200, (b) is the figure which looked at (a) from right above. It is a figure which shows an example of the scanning range of the ranging sensor 112a. (A) is a block diagram which shows the structure of the three-dimensional distance measuring apparatus 200, (b) is a block diagram which shows the detailed structure of the two-dimensional distance measuring apparatus 112 which concerns on this Embodiment. It is. It is a figure explaining the principle of the distance measurement of the ranging sensor 112a. (A) is a figure which shows the relationship between the measurement distance and rotation angle (theta) when the ranging sensor 112a is rotationally driven around a horizontal direction axis | shaft (y-axis), (b) is an axis | shaft of a perpendicular direction. It is a figure which shows the relationship between the measurement plane and rotation angle (phi) when the two-dimensional distance measuring device 112 is rotationally driven around (z axis). It is a figure which shows the example of a distance measurement of the three-dimensional distance measuring apparatus 200. FIG. 5 is a flowchart showing object recognition processing in the three-dimensional distance measuring apparatus 200. (A) And (b) is a figure which shows the example of calculation of a gradient. 2 is a block diagram showing a movement control system of a leg wheel type robot 100. FIG. It is a figure which shows the measurement result by a 1st scanning process in case there exists a stair within a measurement range. It is a figure which shows the measurement result at the time of converting the measurement distance of the rotation coordinate system of each measurement point of FIG. 11 into the measurement distance of a rectangular coordinate system. (A) And (b) is a figure which shows an example of the gradient of the x direction and z direction computed using the least squares method. It is a figure which shows an example of the appearance frequency of the gradient with respect to a certain measurement plane. It is a figure which shows the structure in the case of changing the measurement direction of a ranging sensor. 1 is a front view of a leg wheel type robot 100. FIG. 1 is a side view of a leg wheel type robot 100. FIG. (A) And (b) is a figure which shows the attitude | position at the time of wheel running movement of the leg wheel type robot 100. FIG. It is a figure which shows the example of a driving | running | working state of the leg wheel type robot 100 at the time of non-change driving control. FIGS. 7A and 7B are diagrams illustrating a traveling state of the leg-wheel type robot 100 during super turn control when the coordinates (0, 0) and the coordinates (xc, yc) of the base body 10 are used as the rotation center. is there. (A) And (b) is a figure which shows an example of the driving | running route of the leg wheel type robot 100. FIG. It is a figure which shows an example of the gravity center position of a robot.

Explanation of symbols

DESCRIPTION OF SYMBOLS 100 Leg wheel type robot 200 Three-dimensional distance measuring device 110 Sensing processor 112 Two-dimensional distance measuring device 112a Distance sensor 10 Base body 12 Legs 14-18 Rotary joint 20 Driving wheel 26 Horizontal laser 28, 30 Vertical laser 32 Camera 34, 36 Obstacle sensor 38 Leg tip sensor 112c, 116, 40, 50 Motor 112d, 118, 42, 52 Encoder 112b, 114, 44, 54 Driver 120a, 120b Pulley 21 Belt 60 CPU
64 Communication I / F
76 hub

Claims (7)

A distance measuring sensor that measures a distance to a measurement point on an object existing within a measurement distance range, and the distance measurement sensor is driven to rotate around one of two axes orthogonal to the measurement direction. The first rotation mechanism and the distance measuring sensor are driven to rotate around an axis in a direction different from the first rotation mechanism, of the two axes orthogonal to the measurement direction, together with the first rotation mechanism. And measuring the distance by the distance measuring sensor while rotating the first rotation mechanism and the second rotation mechanism by a predetermined rotation angle. A three-dimensional distance measuring device for measuring a dimensional distance,
The rotation drive of at least one of the first rotation mechanism and the second rotation mechanism is performed within a predetermined rotation angle range, and outside the scanning range scanned by the distance measuring sensor by the rotation drive within the rotation angle range. A three-dimensional distance measuring device provided with a self-driving mechanism including the first rotating mechanism and the second rotating mechanism. The second rotation mechanism includes a rotation driving device that generates a rotation driving force,
A first rotation shaft of the rotation drive device and a second rotation shaft that rotates both the distance measuring sensor and the first rotation mechanism are provided to be shifted in the horizontal direction, and the first rotation shaft of the rotation drive device is provided. 2. The three-dimensional distance measuring apparatus according to claim 1, wherein a rotational driving force of the rotating shaft is transmitted to the second rotating shaft through a predetermined power transmission mechanism.   The power transmission mechanism is configured to transmit the rotational driving force of the first rotational shaft of the rotational driving device to the second rotational shaft via a plurality of pulleys and a belt that transmits power to each pulley. The three-dimensional distance measuring device according to claim 2, wherein   The power transmission mechanism is configured to transmit the rotational driving force of the first rotational shaft of the rotational driving device to the second rotational shaft via a plurality of gears. The three-dimensional distance measuring apparatus described. The first rotation mechanism is controlled to rotate the distance measurement sensor by a predetermined rotation angle in the rotation direction of the first rotation mechanism, and the distance corresponding to each rotation angle by the distance measurement sensor. First scanning means for performing a first scanning process that is a process for measuring information;
A gradient calculating means for calculating a gradient for each measurement point based on the distance information of each measurement point and the distance information of the surrounding measurement points measured by the first scanning means;
An appearance frequency calculating means for calculating an appearance frequency of each gradient based on the gradient of each measurement point calculated by the gradient calculating means;
Shape recognition means for recognizing the shape of an object existing in the measurement plane of the distance measuring sensor based on the appearance frequency of each gradient calculated by the appearance frequency calculation means and the coordinate information of the measurement point corresponding to each gradient; 5. The three-dimensional distance measuring device according to claim 1, comprising: Each time the first scanning process for one measurement plane is completed, the second rotation mechanism is controlled so that the distance measuring sensor and the first rotation mechanism rotate together with the rotation direction of the second rotation mechanism. A second scanning means for performing a second scanning process, which is a process of rotating by a predetermined rotation angle,
In the second scanning process, the first scanning unit performs the first scanning process each time the distance measuring sensor rotates by the predetermined rotation angle.
The gradient calculation means calculates a gradient for each measurement point based on the distance information of each measurement point and the distance information of the surrounding measurement points measured by the first scanning process for each measurement plane. ,
The appearance frequency calculation means calculates the appearance frequency of each gradient based on the gradient of each measurement point calculated by the gradient calculation means for each measurement plane,
The shape recognition means, for each measurement plane, based on the appearance frequency of each gradient calculated by the appearance frequency calculation means and the coordinate information of the measurement point corresponding to each gradient, the object existing in the measurement plane The three-dimensional distance measuring apparatus according to claim 5, wherein the shape is recognized. A base, a leg connected to the base with a degree of freedom, a wheel rotatably provided on the leg, and an actuator for applying power for driving the leg and the wheel And a control means for controlling the actuator, a leg wheel type robot that moves by driving the leg and rotating the wheel,
A three-dimensional distance measuring device according to any one of claims 1 to 6,
The leg wheel type robot characterized in that the control means controls the actuator based on a measurement distance of the three-dimensional distance measuring device.

Images