JP5510081B2 - Obstacle avoidance support device, obstacle avoidance support method, and moving object - Google Patents

Description

  The present invention relates to a technique for avoiding obstacles in a moving object using a potential method.

2. Description of the Related Art Conventionally, a potential method is used as a route planning method for avoiding an obstacle present on a moving path of a moving body that can be moved by power of an actuator such as an actuator such as a mobile robot.
In path planning using the potential method, a potential function that receives an attractive force from the target position of movement and a repulsive force from an obstacle is designed, and the obstacle is avoided by moving the moving object in the direction where the designed potential function is lower. A route that can reach the target position is generated.

As a movement control technique of a moving body using the potential method, for example, a moving body of Patent Document 1 and its movement control method, an obstacle avoidance device and a moving body of Patent Document 2 are disclosed.
In Patent Document 1, when performing a robot movement control using the potential method, a time corresponding to the predicted span T _i that is set when the travel control controller calculates a virtual repulsive force related to the detected obstacle has elapsed. Calculated at least in consideration of the predicted nearest neighbor distance ρ _Ti obtained from the predicted position of the robot and obstacle, the relative speed between the robot and the obstacle, and the allowable minimum distance ρ ₀ between the robot and the obstacle The invention to perform is described.

  Further, in Patent Document 2, the avoidance device mounted on the electric wheelchair indicates the user instruction speed W input from the operation unit and the surrounding obstacles input from the objective sensor, the storage unit of the learning unit And the speed U is corrected accordingly and output to the drive unit. Furthermore, the information stored in the storage unit and the actual speed Y detected by the speed sensor are compared by the extraction unit, and reinforcement learning is performed. Furthermore, the extraction unit learns an appropriate coefficient of repulsive force, the repulsive force generation unit calculates the repulsive force F, and adds it to the speed Um based on the user's designated speed W, thereby correcting the speed U, An invention is described in which the speed U of the wheelchair is realized by a drive device and a power unit.

JP 2009-288930 A JP 2005-293154 A

  However, in the techniques of Patent Document 1 and Patent Document 2, the closest (neighboring) distance to the obstacle is used to calculate the repulsive force (repulsive force) received from the obstacle. Therefore, for example, in the case where there are a plurality of obstacles on a corner as shown in FIG. 9A, a passageway (such as a hallway) on both sides shown in FIG. 9B, or a moving route. As the moving body moves, the closest distance point changes. For example, as shown in FIGS. 9A and 9B, when the moving body moves from the point A to the point B and the point of the closest distance is changed, FIG. 11 corresponding to FIG. As shown in FIG. 11B corresponding to (a) and FIG. 9B, a sudden change in the repulsive force vector occurs. For this reason, when the movement control is performed using the repulsive force vector at the change point, there is a risk that a problem such as a rapid speed change of the moving body may occur. In FIG. 11, the horizontal axis is the position of the moving body, and the vertical axis is the repulsive force that the moving body virtually receives from the obstacle indicated by the oblique lines in FIG. 9. Further, the black dots in FIG. 9 indicate distance measurement points.

  Therefore, the present invention has been made paying attention to such an unsolved problem of the conventional technology, and in the situation where the closest distance point switching occurs, the obstacle avoidance operation of the moving body It is an object of the present invention to provide an obstacle avoidance support apparatus, an obstacle avoidance support method, and a mobile object equipped with an obstacle avoidance support apparatus suitable for assisting the vehicle.

[Invention 1] In order to achieve the above object, an obstacle avoidance assistance device according to an invention 1 is an obstacle avoidance assistance device that supports an avoidance operation of an obstacle existing on a moving path of a moving object,
A distance information storage unit for storing distance information measured for a plurality of measurement points along the shape of an obstacle present on the moving path of the moving body;
Based on the distance information stored in the distance information storage unit, a repulsive force calculation unit that calculates a repulsive force virtually received from each measurement point of the plurality of measurement points of the obstacle using a potential function;
A repulsive force component separating unit that separates repulsive force corresponding to each measurement point calculated by the repulsive force calculating unit into axial components of each axis defining coordinates of the measuring point;
An avoidance repulsive force generation unit that generates a repulsive force for avoidance, which is a repulsive force related to the avoidance operation of the obstacle, based on the separated axial component of each measurement point;
The avoidance repulsive force generation unit calculates, for each axis, the sum of the maximum value of the positive direction component and the maximum value of the negative direction component of each axis in the repulsive force axis component corresponding to each measurement point, Based on the result, the avoiding repulsive force is generated.

In such a configuration, the repulsive force calculation unit uses the potential function to check the obstacle based on the distance information measured for a plurality of measurement points along the shape of the obstacle present on the moving path of the moving object. The repulsive force virtually received from each measurement point of the plurality of measurement points of the object can be calculated. Furthermore, the repulsive force component separating unit can separate repulsive force corresponding to each measurement point calculated by the repulsive force calculating unit into axial components. Furthermore, the avoidance repulsive force generation unit calculates the sum of the maximum value in the positive direction and the maximum value in the negative direction of each axis based on the repulsive force axial component corresponding to each measurement point separated by the axial component separation unit, A repulsive force for avoiding an obstacle can be generated based on the calculation result.
For example, as the repulsive force for avoidance, a repulsive force (vector) having the sum of the calculated maximum values of the axis components of the respective axes as the components of the respective axes can be generated.

[Invention 2] Further, the obstacle avoidance assistance device of Invention 2 is the obstacle avoidance assistance device of Invention 1, wherein the coordinates of the measurement point are defined by two-dimensional coordinates (x , Y), and the component in the x-axis direction in the repulsive force corresponding to the nth measurement point (n is a natural number of 1 ≦ n ≦ N where N is the total number of measurement points) is F _px (n) , The y-axis direction component is represented by F _py (n), the avoidance repulsive force generation unit reduces the x-axis direction component F _px and the y-axis direction component F _py in the avoidance repulsion force Calculate based on the formulas (1) to (2),
F _px = F _pxp + F _pxn (1)
F _py = F _pyp + F _pyn (2)

In the above formulas (1) to (2), F _pxp is the maximum value of the positive component of the x axis in the repulsive force corresponding to the N measurement points, and F _pxn corresponds to the N measurement points. The maximum value of the negative component of the x-axis in the repulsive force, F _pyp is the maximum value of the positive component of the y-axis in the repulsive force corresponding to the N measurement points, and F _pyn is the N measurement The maximum value of the negative component of the y-axis in the repulsive force corresponding to the point. In the equations (1) to (2), when the F _pxp is a negative value, the value of the F _pxp is set to zero. When F _pyp is a negative value, the value of the F _pyp is zero, when the F _pxn is a positive value, the value of the F _pxn is zero, and when the F _pyn is a positive value, the F _pyn The value of is zero.
With this configuration, the avoiding repulsive force generation unit uses the x-axis direction component and the y-axis direction component separated by the repulsive force component separation unit based on the above formulas (1) to (2). Repulsive forces F _px and F _py can be generated.

[Invention 3] The obstacle avoidance assistance device according to Invention 3 is the obstacle avoidance assistance device according to Invention 2, wherein the coordinates of the measurement point further include a z-axis orthogonal to both the x-axis and the y-axis. When the component in the z-axis direction of the repulsive force corresponding to the n-th measurement point is represented by F _pz (n), represented by three-dimensional coordinates (x, y, z), the avoiding repulsive force generating unit is A component F _pz in the z-axis direction of the repulsive force for avoidance is calculated based on the following equation (3):
F _pz = F _pzp + F _pzn (3)

In Formula (3), F _pzp is the maximum value of the positive component of the z axis in the repulsive force corresponding to the N measurement points, and F _pzn is the z axis in the repulsive force corresponding to the N measurement points. When the F _pzp is a negative value, the value of the F _pzp is zero, and when the F _pzn is a positive value, the value of the F _pzn is zero.
In such a configuration, the avoiding repulsive force generation unit uses the z-axis direction component separated by the repulsive force separating unit in addition to the avoiding repulsive forces F _px and F _py , based on the above equation (3). The avoiding repulsive force F _pz can be generated.

[Invention 4] The obstacle avoidance assistance device according to Invention 4 is the obstacle avoidance assistance device according to Invention 1 or 2, wherein the coordinates of the measurement point are defined by an x axis and a y axis orthogonal to each other. In the case of (x, y), the repulsive force calculation unit calculates the repulsive force U (n corresponding to the n-th measurement point (n is a natural number of 1 to N where N is the total number of measurement points). ) Based on the potential function shown in the following expression (4), and the repulsive force component separating unit calculates the repulsive force U (n) based on the following expressions (5) to (6) as the component F _{px in the} x-axis direction. Separated into (n) and a component F _py (n) in the y-axis direction.
U (n) = η / 2 · (1 / ρ _n −1 / ρ ₀ ) ² (4)
F _px (n) = ∂U (n) / ∂x (5)
F _py (n) = ∂U (n) / ∂y (6)

However, in said Formula (4), (eta) is a positive weight constant, (rho) ₀ is a positive constant, the range which the said potential function influences, (rho) _n is the present position of the said mobile body To the nth measurement point.
With such a configuration, the repulsive force calculation unit can calculate the repulsive force U (n) corresponding to the nth measurement point based on the potential function shown in the above equation (4). Further, the repulsive force component separating unit separates the repulsive force U (n) into the x-axis direction component F _px (n) and the y-axis direction component F _py (n) based on the above equations (5) to (6). can do.

[Invention 5] The obstacle avoidance assistance device according to Invention 5 is the obstacle avoidance assistance device according to Invention 1 or 3, wherein the coordinates of the measurement points are defined by the x-axis, the y-axis, and the z-axis that are orthogonal to each other. The repulsive force calculation unit is represented by the nth measurement point (where n is a natural number of 1 to N where N is the total number of measurement points) when expressed in three-dimensional coordinates (x, y, z). The repulsive force U (n) corresponding to is calculated based on the potential function shown in the following equation (7), and the repulsive force component separating unit calculates the repulsive force U (n) based on the following equations (8) to (10): The component is separated into a component F _px (n) in the x-axis direction, a component F _py (n) in the y-axis direction, and a component F _pz (n) in the z-axis direction.
U (n) = η / 2 · (1 / ρ _n −1 / ρ ₀ ) ² (7)
F _px (n) = ∂U (n) / ∂x (8)
F _py (n) = ∂U (n) / ∂y (9)
F _pz (n) = ∂U (n) / ∂z (10)

However, in the above formula (7), η is a positive weight constant, ρ ₀ is a positive constant, and the range in which the potential function affects, and ρ _n is the current position of the moving object. To the nth measurement point.
With such a configuration, the repulsive force calculation unit can calculate the repulsive force U (n) corresponding to the nth measurement point based on the potential function shown in the above equation (7). Further, the repulsive force component separation unit converts the repulsive force U (n) into the x-axis direction component F _px (n), the y-axis direction components F _py (n) and z based on the above equations (8) to (10). It can be separated into axial components F _pz (n).

[Invention 6] On the other hand, in order to achieve the above object, the obstacle avoidance support method of Invention 6 is an obstacle avoidance support method for supporting an avoidance operation of an obstacle existing on a moving path of a moving body,
Based on the distance information of the distance information storage means storing the distance information measured for a plurality of measurement points along the shape of the obstacle present on the moving path of the moving body, the potential function is used. Repulsive force calculating step for calculating repulsive force virtually received from each of the plurality of measuring points of the obstacle;
A repulsive force component separating step for separating repulsive force corresponding to each measurement point calculated in the repulsive force calculating step into axial components of each axis defining coordinates of the measuring point;
A repulsive force generation step for avoidance that generates a repulsive force for avoidance that is a repulsive force related to the avoidance operation of the obstacle based on the separated axial component of each measurement point,
In the repulsive repulsive force generation step, the sum of the maximum value of the positive direction component and the maximum value of the negative direction component of each axis in the repulsive axis component corresponding to each measurement point is calculated for each axis, Based on the calculation result, the avoidance repulsive force is generated.
Thereby, an operation equivalent to that of the obstacle avoidance assistance device of aspect 1 can be obtained.

[Invention 7] In order to achieve the above object, a mobile object of Invention 7 includes a substrate,
A moving mechanism having an actuator for moving the substrate;
A distance information measurement unit that measures distance information for a plurality of measurement points along the shape of an obstacle present on the moving path;
An instruction input receiving unit that receives an instruction input related to movement including information on the moving direction of the moving body from the user via the operation unit;
The obstacle avoidance assistance device according to any one of claims 1 to 5,
An actuator control unit for controlling the actuator based on the repulsive force for avoidance calculated based on the distance information measured by the distance information measurement unit in the obstacle avoidance support device and the instruction input received by the instruction input reception unit; Prepare.
With such a configuration, an action equivalent to that of any one of the obstacle avoidance assistance devices according to the first to fifth aspects of the invention can be obtained.

  As described above, according to the obstacle avoidance support device of the inventions 1 to 5, the obstacle avoidance support method of the invention 6, and the mobile object of the invention 7, the repulsive force vector related to obstacle avoidance (path planning) is generated. At this time, the repulsive force received from each measurement point of the obstacle is separated into axial components, and the sum of the maximum value of the positive direction component and the maximum value of the negative direction component among the separated axial components is calculated for each axis. Then, it is possible to generate an avoidance repulsive force having the sum of the calculated maximum values of each axis as a component of each axis.

  Thus, since the maximum repulsive force is used in obstacle avoidance (path planning), the closest distance is not affected by the switching as compared with the repulsive force vector generated using the conventional closest distance. Specifically, a repulsive force vector that changes continuously can be generated. Accordingly, it is possible to prevent the use of repulsive force vectors that change rapidly due to switching of the closest distance in the movement control when the obstacle avoids the moving object, and performs safe and stable obstacle avoidance control of the moving object. The effect that it can be obtained.

Further, it is conceivable to calculate the repulsive force for avoidance using the sum total of repulsive forces obtained from the distance information of the measurement point group measured by a range sensor such as a laser range finder or a distance image sensor. In this case, in the distance measurement using the range sensor, the density (granularity) of the measurement point group on the obstacle changes according to the distance from the range sensor to the obstacle. Therefore, even with an object of the same size, the density of the measurement point group changes depending on the distance, and the number of measurement points on the obstacle changes. In addition, a relatively small obstacle has a smaller number of measurement points, so the influence of repulsive force is reduced, and the possibility of contact and collision increases.
According to the first to seventh aspects of the present invention, since the sum of the positive and negative maximum values of the repulsive axial component is used, the avoiding repulsive force that is not affected by the size of the obstacle or the density of the measurement point group is calculated. The effect that it can be obtained.

(A) is a front view of the leg-wheel type robot 1, and (b) is a side view of the leg-wheel type robot 1. It is a figure explaining the principle of the distance measurement of the laser range sensor. (A) is a figure which shows the relationship between the measurement distance and rotation angle (theta) when a ranging sensor is rotated about the axis | shaft (z axis) of a perpendicular direction, (b) is a figure which shows a ranging sensor. It is a figure which shows the relationship between the measurement plane when rotating around the horizontal axis (y-axis) and the rotation angle φ. 1 is a block diagram showing a movement control system of a leg wheel type robot 1. It is a block diagram which shows the function structure which concerns on the movement control at the time of the obstacle avoidance with which a movement control system is provided. 3 is a block diagram illustrating a detailed functional configuration of an obstacle avoidance support unit 101. FIG. It is a flowchart showing generation processing of avoiding repulsive force F _p. It is a flowchart which shows the motor control process at the time of obstacle avoidance. (A) And (b) is a figure which shows the example of the positional relationship of an obstruction and the leg wheel type robot 1. FIG. (A) And (b) is a change of the repulsive force for avoidance of the present invention with respect to a change in position when the leg-wheel type robot 1 moves from the point A to the point B shown in FIGS. FIG. FIGS. 9A and 9B show changes in the conventional avoiding repulsive force with respect to changes in position when the leg-wheel type robot 1 moves from the point A to the point B shown in FIGS. 9A and 9B. FIG. FIG. 3 is a side view of the guidance robot according to the embodiment, and shows the configuration of the guidance robot 81.

[First Embodiment]
Hereinafter, a first embodiment of the present invention will be described with reference to the drawings. 1 to 10 are diagrams showing a first embodiment of an obstacle avoidance support device, an obstacle avoidance support method, and a moving body according to the present invention.
In the present embodiment, the obstacle avoidance assisting apparatus according to the present invention is applied to a leg-wheel type robot that is a moving body and that can be walked by a leg and traveled by a wheel.

First, based on FIG. 1, the external configuration of the leg-wheel type robot according to the present embodiment will be described.
FIG. 1A is a front view of the leg wheel type robot 1, and FIG. 1B is a side view of the leg wheel type robot 1.
As shown in FIGS. 1A and 1B, the leg-wheel type robot 1 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 1 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. 14, the rotary joints 16 and 18 rotate with the direction orthogonal to the side surface of the leg wheel type robot 1 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 1 is provided at the tip of each leg 12.

On the other hand, a horizontal laser 26 that irradiates a horizontal laser beam is provided in 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.
In the upper center 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.

A laser range sensor 200 is disposed below the camera 32.
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 plane 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.

A joint motor 40 that rotationally drives the rotary joints 14 to 18 is provided in each of the rotary joints 14 to 18 of each leg portion 12. In addition, the wheel 20 of each leg 12 is provided with a wheel motor 50 that rotationally drives the wheel 20.
Although not shown, the laser range sensor 200 includes a sensing processor, a distance measuring sensor that measures a distance to a measurement point on an object that exists in the measurement range, and a vertical axis and a horizontal axis. And a rotating mechanism that rotates around.

Further, although not shown, the laser range sensor 200 includes a first motor that rotates the rotation mechanism around a vertical axis, a second motor that rotates the rotation mechanism around a horizontal axis, and rotational angle positions of these motors. The encoder includes a detection encoder, and a driver that controls each motor based on a command signal from the sensing processor and a signal from each encoder.
The driver is designated with the rotation axes of the first and second motors 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. 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 (the position where the scanning angle is 0 °) is the x axis, and one of the two axes orthogonal to the x axis is the y axis. In the embodiment, the horizontal and horizontal axes of the sensor are used), and the other axis is the z-axis (vertical axis in this embodiment).

  The first motor 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 around a vertical axis (z axis), and the second motor is an output unit. In addition, the light receiving unit is provided to rotate about a horizontal axis (y-axis). Then, according to the control signal from the driver, the first motor rotates its own rotation axis by the designated scanning angle unit (rotation angle θ), and the second motor has its own rotation axis specified. It is rotated by scanning angle units (rotation angle φ).

Here, FIG. 2 is a diagram for explaining the principle of distance measurement of the laser range sensor 200.
As shown in FIG. 2, the laser range sensor 200 outputs laser light from the laser output unit and outputs the laser light every time the distance measurement sensor rotates by the designated scanning angle unit and rotates. Reflected light from the object (obstacle in FIG. 2) is received by the light receiving unit. Then, a distance (measurement distance L in FIG. 2 (distance between the object and the light receiving unit)) corresponding to each rotation angle (scanning angle) is measured.

FIG. 3A is a diagram showing the relationship between the measurement distance and the rotation angle θ when the distance measuring sensor is driven to rotate about the vertical axis (z-axis), and FIG. It is a figure which shows the relationship between the measurement plane and rotation angle (phi) when a ranging sensor is rotationally driven around the axis | shaft of a horizontal direction (y-axis).
For example, as shown in FIG. 3 (a), each rotation angle (θ ₁ in FIG. 3 (a), Distance information (L (θ ₁ ), L (θ ₂ ), L (θ ₃ ) in FIG. 3A) corresponding to θ ₂ , θ ₃ ) is measured.

A plane formed by connecting the rotation center of the rotation shaft of the first motor and both ends of the scanning trajectory line of the laser is an xy measurement plane (a fan-shaped plane when no object is present).
Each time the xy measurement plane scanning process (first scanning process) is completed, as shown in FIG. 3B, the distance measuring sensor is rotated by a specified scanning angle unit (φ) around the y-axis. . This scanning process is referred to as a second scanning process.

Then, by performing the first scanning process and the second scanning process alternately and continuously, the measurement plane formed by the first scanning process is continuously formed around the y axis. Thereby, it is possible to measure the three-dimensional distance information of the object existing within the measurement range (scanning angle range).
Further, as shown in FIG. 3B, the distance information of each measurement point after the second scanning process is expressed as L (θ _i , φ _j ). Here, i is a serial number given to each measurement point according to the scanning angle around the z axis, and j is a serial number given to each measurement point according to the scanning angle around the y axis.

In the present embodiment, the leg-wheel type robot 1 uses the potential method based on the distance information and the speed information of the obstacle existing on its own movement path measured by the laser range sensor 200 in the CPU 60. Movement control including avoidance operation is performed.
Further, in the sensing processor of the laser range sensor 200, a filtering process using a median filter is performed on the distance information of the rotating coordinate system measured by the distance measuring sensor in order to remove noise. In the sensing processor, a process for calculating the moving speed of the object to be measured is executed.

Next, a movement control system for the leg wheel type robot 1 will be described with reference to FIG.
Here, FIG. 4 is a block diagram showing a movement control system of the leg-wheel type robot 1.
As shown in FIG. 4, joint motors 40 that rotationally drive the rotary joints 14, 16, and 18 are provided at the rotary joints 14, 16, and 18 of the four leg portions 12 of the leg-wheel type robot 1. ing. 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 that rotationally drives the drive wheel 20 is provided on the drive wheel 20 of each leg 12. 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 type robot 1 further includes a CPU 60, a three-axis attitude sensor 70 that detects the attitude of the leg wheel type robot 1, a vision processor 72 that processes an image signal of the camera 32, and a laser range sensor 200. Configured.

The triaxial posture sensor 70 has a gyroscope or an acceleration sensor, or both, and detects the inclination of the posture of the leg-wheel type robot 1 with respect to the ground axis.
The leg-wheel type robot 1 further includes a radio communication unit 74 that performs radio communication with an operation unit 300 that can remotely control the leg-wheel type robot 1 using radio communication, a vision processor 72, a radio communication unit 74, and a laser range. A hub 76 that relays the input and output of the sensor 200 and the CPU 60 and a speaker 78 that outputs a warning sound or the like are included.

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.
The CPU 60 inputs the output signals of the encoders that detect the rotation angle positions of the first and second motors to the laser range sensor 200 via the communication I / F 64 and the hub 76, and outputs the first and second motors. The motor command signal is output to the driver.

The CPU 60 inputs sensor signals from the in-plane scanning leg tip sensor 38, the laser range sensor 200, and the triaxial 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.
When the distance information is input from the laser range sensor 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 transformation process, the global coordinates of the leg-wheel robot 1 center are Xc, Yc, Zc, and the posture (roll angle, pitch angle, yaw angle) of the leg-wheel robot 1 is φ, θ, ψ in the sensor coordinate system. Each measurement point is converted into a measurement point (Xs, Ys, Zs) in the global coordinate system.

  Further, during walking motion, inverse kinematics calculation and centroid 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 leg tip (drive) is calculated based on the calculation result. Wheel) landing position, 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 to the drivers 44 and 54 is calculated based on the determined landing position and the calculated both distances. A signal is generated, and the generated motor command signal is output to the drivers 44 and 54.

Further, during the wheel running operation, repulsive force is calculated using the potential method based on each measurement point of the obstacle converted into the global coordinate system, and the motor command signal to the drivers 44 and 54 is calculated based on the calculation result. And outputs the generated motor command signal to the drivers 44 and 54.
Although not shown, the leg-wheel type robot 1 has a memory such as a ROM for storing a dedicated program and parameter values necessary for controlling the robot, and a RAM for temporarily storing data necessary for executing the program. I have.

Next, based on FIG.5 and FIG.6, the function structure which concerns on the movement control process at the time of the obstacle avoidance at the time of wheel driving | running | working of the leg-wheel type robot 1 performed in a movement control system is demonstrated.
Here, FIG. 5 is a block diagram showing a functional configuration related to movement control at the time of obstacle avoidance provided in the movement control system. FIG. 6 is a block diagram illustrating a detailed functional configuration of the obstacle avoidance support unit 101.

As shown in FIG. 5, the movement control system includes a movement control unit 100 including an obstacle avoidance support unit 101 and a motor control unit 102, which are functional components related to movement control during obstacle avoidance during wheel travel. It has.
The obstacle avoidance support unit 101 calculates the repulsive force that the leg-wheel type robot 1 receives from each measurement point of the obstacle using the potential method based on the distance information of each measurement point of the obstacle measured by the laser range sensor 200. It has a function to do. Furthermore, it has a function of calculating a repulsive force for avoidance, which is a repulsive force for avoiding an obstacle, based on a repulsive force calculation result at each measurement point.

Specifically, the obstacle avoidance support unit 101 includes a distance information storage unit 110, a repulsive force calculation unit 111, a repulsive force component separation unit 112, and an avoiding repulsive force generation unit 113, as shown in FIG. Is done.
The distance information storage unit 110 has a function of storing distance information input from the laser range sensor 200 via the communication I / F 64 in a memory such as a RAM.
The repulsive force calculation unit 111 uses the distance information of N measurement points 1 to N (N is a natural number of 2 or more) stored in the memory based on the potential function of the above formula (4) or (7), It has a function of calculating repulsive force U (n) (n is a natural number of 1 to N) corresponding to each of N measurement points.

Here, the above equations (4) and (7) are known potential functions designed by Khatib to calculate the repulsive force received from an obstacle, η is a positive weight constant, and ρ ₀ is It is a positive constant and the range in which the potential function affects, and ρ _n is the distance between the leg wheel type robot 1 and the nth measurement point on the obstacle. At this time, the repulsive force receives a force in the direction in which the potential function is inclined.

  In the present embodiment, the two-dimensional mode and the three-dimensional mode can be set as the obstacle avoidance support mode. The two-dimensional mode uses the distance information of the measurement point on the xy plane of a predetermined z coordinate among the distance information of the obstacle measured by executing the first scanning process and the second scanning process described above. The avoidance repulsive force is calculated, and the three-dimensional mode is a mode in which avoidance repulsive force is calculated using distance information of all measurement points. Therefore, the value of N changes depending on the mode.

Specifically, when the two-dimensional mode is set, the repulsive force calculation unit 111 calculates repulsive force using measurement information on the xy plane of a predetermined one z coordinate (for example, the coordinate of the origin). When the dimension mode is set, the repulsive force is calculated using all or some of the three-dimensional measurement information corresponding to the range related to movement.
The repulsive force component separating unit 112 calculates the repulsive forces U (1) to U (N) corresponding to the N measurement points on the obstacle calculated by the repulsive force calculating unit 111 from the above equations (5) to (6) or Based on the equations (8) to (10), it has a function of separating into components for each axis.

Specifically, when the two-dimensional mode is set, the repulsive force component separation unit 112 converts the repulsive force U (n) to the x-axis component F _px (n) based on the above equations (5) to (6). , And y-axis component F _py (n). Further, when the three-dimensional mode is set, the repulsive force U (n) is converted into the x-axis component F _px (n) and the y-axis component F _py (n) based on the above equations (8) to (10). And z-axis component F _pz (n). Here, (x, y, z) in each expression corresponds to (Xs, Ys, Zs) of the global coordinate system coordinate-converted by the CPU 60.
The avoiding repulsive force generation unit 113 is based on the following formulas (11) to (16), and among the axial components of the repulsive forces U (1) to U (N) separated by the repulsive force component separation unit 112, It has a function of extracting the maximum value and the maximum value in the negative direction for each axis.

The above equation (11) is a function for extracting the maximum value F _pxp from the x-axis components F _px (1) to (N), and the above equation (13) is the y-axis component F _py (1) to ( _This is a function for extracting the maximum value F _pyp from N). The above equation (15) is a function for extracting the maximum value F _pzp from the z-axis components F _pz (1) to (N). Basically, the above equations (11) and (13) are functions aimed at extracting the maximum value in the positive direction of each axis component. Thereafter, F _pxp , F _pyp, and F _pzp are treated as the maximum value in the positive direction of each axis component regardless of the sign of the extracted value.

The above equation (12) is a function for extracting the minimum value F _pxn from the x-axis components F _px (1) to (N), and the above equation (14) is the y-axis component F _py (1) to ( N) is a function for extracting the minimum value F _pyn from N). The above equation (16) is a function for extracting the minimum value F _pzn from the z-axis components F _pz (1) to (N). Basically, the above equations (12) and (14) are functions aimed at extracting the maximum value in the negative direction of each axis component. Hereinafter, F _pxn , F _pyn, and F _pzn are treated as the maximum value in the negative direction of each axis component regardless of whether the extracted value is positive or negative.

Furthermore, the avoidance repulsive force generation unit 113 calculates the sum of the maximum value in the positive direction and the maximum value in the negative direction for each extracted axis based on the above formulas (1) to (3), and avoids the calculation result. and it has a function of generating a use repulsive force F _p. This avoiding repulsive force F _p is a repulsive force used in movement control to avoid an obstacle.
Specifically, when the two-dimensional mode is set, the avoidance repulsive force generation unit 113 first determines the maximum value F _{pxp in} the positive direction of the repulsive force component on the x axis based on the above equations (11) to (14). and a negative direction of the maximum value F _pxn calculated, to calculate the positive direction of the maximum value F _PYP and the negative direction of the maximum value F _Pyn repulsive component of the y-axis.

Next, the avoiding repulsive force generation unit 113, based on the above formulas (1) to (2), F _px that is the sum of the maximum value F _pxp in the positive direction and the maximum value F _{pxn in the} negative direction of the x-axis component, A maximum value F _{pyp in} the positive direction of the y-axis component and F _py which is the sum of the maximum value F _{pyn in} the negative direction are calculated. At this time, when F _pxp is a negative value, the value of F _pxp is set to zero (0), and when F _pyp is a negative value, the value of F _pyp is set to zero (0). Such a situation occurs, for example, when only a negative repulsive force is received from an obstacle. Further, when F _pxn is a positive value, the value of F _pxn is set to zero (0), and when F _pyn is a positive value, the value of F _pyn is set to zero (0). Such a situation occurs, for example, when only positive repulsion is received from an obstacle.
Further, the repulsive repulsive force generator 113 represents the following formula (17) based on the calculation results F _px and F _py of the sum of the maximum value in the positive direction and the maximum value in the negative direction for the x axis and the y axis. As described above, the avoiding repulsive force F _p that is a vector having F _px as the x-axis component and F _py as the y-axis component is generated.

Further, the avoidance repulsive force generation unit 113, when the three-dimensional mode is set, based on the above equations (11) to (16), the maximum value F _{pxp in} the positive direction of the repulsive force component on the x axis and the negative direction calculates the maximum value F _pxn, to calculate the positive direction of the maximum value F _PYP and the negative direction of the maximum value F _Pyn repulsive component of the y-axis, the repulsive component of the z-axis positive direction maximum value F _PZP and negative direction The maximum value F _pzn is calculated. Furthermore, based on the above equation (1) ~ (3), F px and the maximum positive direction of the y-axis component is the sum of the positive maximum value F _pxp the negative direction of the maximum value F _pxn the x-axis component calculating a F _py is the sum of the values F _PYP and negative maximum value F _Pyn, and F _pz is the sum of the positive maximum value F _PZP and negative maximum value F _pzn the z-axis component . Then, as shown in the following equation (18), an avoiding repulsive force F _p that is a vector having F _px as an x-axis component, F _py as a y-axis component, and F _pz as a z-axis component is generated.
Here, also for the z-axis component, if F _PZP is a positive value to zero (0) the value of F _PZP, F _pzn the case of a positive value to zero (0) the value of F _pzn. }

When the avoiding repulsive force generation unit 113 generates the avoiding repulsive force F _p , it outputs the generated F _p to the motor control unit 102.
Returning to FIG. 5, the motor control unit 102 inputs the instruction from the operation unit 300, the position information of the obstacle from the laser range sensor 200, the avoidance repulsive force F _p from the obstacle avoidance support unit 101, Based on the rotation angle information from the encoder, it has a function of generating a motor command signal for travel control including an obstacle avoidance operation.

For example, when the user's operation input indicates movement in the direction of an obstacle, a command signal for stopping the motor is generated, or a command signal for automatically moving a trajectory that avoids the obstacle is generated. .
In this embodiment, each component of the obstacle avoidance support unit 101 and each function of the motor control unit 102 are realized by executing a dedicated program in the CPU 60.

Next, based on FIG. 7, the flow of the avoidance repulsive force F _p generation process of the obstacle avoidance support unit 101 will be described. Here, FIG. 7 is a flowchart illustrating a process of creating avoiding repulsive force F _p.
The generation process of the avoiding repulsive force F _p is a process realized by the CPU 60 reading a dedicated program stored in the ROM and executing the read program. As shown, first, the process proceeds to step S100.

In step S100, the repulsive force calculation unit 111 determines whether or not there has been an operation input via the communication I / F 64 from the operation unit 300 according to the user's operation, and when it is determined that there has been an operation input (Yes) Shifts to step S102. On the other hand, if it is determined that there is no operation input (No), the determination process is repeated until there is an operation input.
When the process proceeds to step S102, the repulsive force calculation unit 111 acquires distance information from the memory, and the process proceeds to step S104.

In step S104, the repulsive force calculation unit 111 determines whether or not there is an obstacle based on the acquired distance information. If it is determined that there is an obstacle (Yes), the process proceeds to step S106; (No) ends a series of processing.
When the process proceeds to step S106, the repulsive force calculation unit 111 uses the distance information of the N pieces of distance information based on the potential function of the above formula (4) or (7) to repulsive force U ( n) is calculated, and the process proceeds to step S108.

In step S108, the repulsive force component separation unit 112 separates the repulsive force U (n) calculated in step S106 into each axial component based on the above formulas (5) to (6) or (8) to (10). The process proceeds to step S110.
In step S110, the repulsive force component separation unit 112 determines whether or not the calculation of the repulsive forces U (1) to U (N) and the separation processing into these axis components have been completed for N measurement points. When it is determined that the process has been completed (Yes), the process proceeds to step S112. If it is determined that the process has not been completed (No), the process proceeds to step S106.

When the process proceeds to step S112, the avoidance repulsive force generation unit 113, based on the above formula (11) and (13) or the above formula (11), (13) and (15), according to the set mode, The maximum value is extracted for each axis from the axis components corresponding to the N measurement points, and the process proceeds to step S114.
In step S114, the avoidance repulsive force generation unit 113 performs N measurements based on the above formulas (12) and (14) or the above formulas (12), (14), and (16) according to the set mode. The maximum value in the negative direction is extracted for each axis from the axis components corresponding to the points, and the process proceeds to step S116.

In step S116, the avoidance repulsive force generation unit 113 determines whether there is a negative value among the maximum values in the positive direction of all axes according to the set mode. If it is determined that there is (Yes), the process proceeds to step S118. If not (No), the process proceeds to step S120.
When the process proceeds to step S118, the in avoiding repulsive force generating unit 113, by changing the positive maximum value F _pp, which has a negative value to zero (0), the process proceeds to step S120. The “−” portion of F _pp corresponds to all negative axes among x, y, and z.

In step S120, the avoidance repulsive force generation unit 113 determines whether there is a positive value among the maximum values in the negative direction of all axes according to the set mode. If it is determined that there is a positive value (Yes), the process proceeds to step S122. If not (No), the process proceeds to step S124.
When the process proceeds to step S122, the avoidance repulsive force generation unit 113 changes the negative maximum value F _pn that is a positive value to zero (0), and the process proceeds to step S124. It should be noted that the “−” portion of F _pn corresponds to all the positive axes of x, y, and z.

On the other hand, when the process proceeds to step S124, the avoiding repulsive force generation unit 113 determines the maximum value in the positive direction and the negative direction for each axis based on the above formulas (1) to (2) or (1) to (3). The sum is calculated, and the process proceeds to step S126.
In step S126, the avoidance repulsion generating unit 113, based on the calculation result of the step S124, the generate the avoidance repulsive force F _p, the process proceeds to step S128.
In step S128, the avoiding repulsive force generating unit 113 outputs the avoiding repulsive force F _p generated in step S126 to the motor control unit 102, and the series of processing ends.
The series of processes is repeated each time N pieces of measurement information to be newly measured are stored as the leg wheel type robot 1 moves.

Next, the flow of the motor control process at the time of obstacle avoidance in the motor control unit 102 will be described based on FIG.
Here, FIG. 8 is a flowchart showing a motor control process at the time of obstacle avoidance.
The motor control process at the time of obstacle avoidance is a process realized by the CPU 60 reading a dedicated program stored in the ROM and executing the read program. As shown, first, the process proceeds to step S200.
In step S200, the motor control unit 102 determines whether or not there is an operation input via the communication I / F 64 from the operation unit 300 according to the user's operation, and when it is determined that there is an operation input (Yes) Shifts to step S202. On the other hand, if it is determined that there is no operation input (No), the determination process is repeated until there is an operation input.

When the process proceeds to step S202, the motor control unit 102 determines whether or not to get the avoidance repulsive force F _p from the obstacle avoidance assist unit 101, when determining that the acquired (Yes), the step S204 If not (No), the determination process is repeated until acquisition.
When the process proceeds to step S204, the motor control unit 102 acquires obstacle position information via the communication I / F 64, and the process proceeds to step S206.

In step S206, the motor control unit 102 acquires the rotation angle information of each motor via the angle fetch I / F 62, and the process proceeds to step S208.
In step S208, the motor control unit 102 generates a motor command for avoiding an obstacle based on the operation input, the repulsive force for avoidance, the position information, and the rotation angle information, and the process proceeds to step S210.
In step S210, the motor control unit 102 outputs a motor command signal to the driver of each motor via the motor command output I / F 61, and the process proceeds to step S200.

Next, the operation of the present embodiment will be described with reference to FIGS.
Here, FIG. 9A is a diagram illustrating an example of a positional relationship between a wall (obstacle) that forms a corner and the leg-wheel type robot 1, and FIG. 9B is a diagram illustrating a wall and a leg that form a straight path. It is a figure which shows an example of the positional relationship with the wheel type robot. FIG. 10A shows a change in the repulsive repulsive force according to the present embodiment with respect to a change in position when the leg-wheel type robot 1 moves from the point A to the point B at the turn of FIG. 9A. FIG. FIG. 10B shows the change in the repulsive repulsive force of the present embodiment with respect to the change in position when the leg-wheel type robot 1 moves from the point A to the point B on the straight road in FIG. 9B. FIG. In addition, the black point in FIG. 9 shows the measurement point of the distance with respect to the obstruction shown with a diagonal line.
The leg wheel type robot 1 gives a command signal from the CPU 60 to the laser range sensor 200 in response to power-on, and starts distance measurement in the designated scanning angle range and designated scanning angle unit.

On the other hand, the laser range sensor 200 sets a scanning angle range and a scanning angle unit designated by the sensing processor in response to a command signal from the CPU 60.
Here, it is assumed that the laser range sensor 200 includes a two-dimensional range sensor having a distance measurement range of 20 to 4095 [mm], a maximum scanning angle range of 240 °, and an angular resolution of 0.36 °.
Further, a scan angle range of −40 ° to + 40 ° and a scan angle unit of 0.36 ° are set for the first scan process, and a scan angle range of 240 ° and a scan angle unit of 10 ° are set for the second scan process. Is set (in this case, nine fan-shaped measurement planes are formed in the xy plane).

Next, the sensing processor outputs a command signal to the driver based on the scanning angle range and the scanning angle unit (θ) set for the first scanning process.
In the two-dimensional range sensor, in the driver, based on the command signal from the sensing processor and the output signal from the encoder, the rotary shaft of the first motor is rotated to drive the two-dimensional range sensor to the vertical axis (z axis). Ranging is performed while scanning around the range of −40 ° to + 40 ° in steps of 0.36 °.
Subsequently, the rotation angle is rotated by 10 ° around the horizontal axis (y-axis), and distance measurement is performed again while scanning the range of −40 ° to + 40 ° in increments of 0.36 ° (second scan). processing).
As described above, the distance information of the obstacle in the scanning range is measured by performing the first scanning process and the second scanning process.

Information on each measurement distance (rotational coordinate system) is output to the sensing processor as a data string L (θ _i , φ _j ).
When the first scanning process for one measurement plane is completed, the sensing processor performs a filtering process using a median filter on the distance information measured in the first scanning process. Thereby, the noise component in the measurement information is removed.

  Next, the sensing processor converts the distance information of the rotating coordinate system after the filtering process into the distance information of the orthogonal coordinate system, and the distance information converted into the orthogonal coordinate system is converted to the CPU 60 via the hub 76 and the communication I / F 64. To enter. Further, the sensing processor calculates the speed of the obstacle based on the measured distance information, and inputs the calculated speed information to the CPU 60 via the hub 76 and the communication I / F 64. The input distance information is stored in the memory in the distance information storage unit 110.

  When there is a turning angle as shown in FIG. 9A on the movement path of the leg-wheel type robot 1, the distance information is such as a wall constituting the turning angle as shown by a black dot in FIG. 9A. It has coordinate information along the shape of the obstacle. Further, when the leg-wheel type robot 1 moves on the straight road shown in FIG. 9B, as shown by the black dots in FIG. 9B, it follows the shape of an obstacle such as a wall forming a passage. Coordinate information. However, the examples shown in FIGS. 9A and 9B show measurement points of two-dimensional coordinates.

  On the other hand, when the obstacle avoidance support unit 101 determines in the repulsive force calculation unit 111 that there has been an operation input from the user via the operation unit 300 ("Yes" branch of step S100), the distance information from the memory. (N) are acquired (step S102). The operation unit 300 corresponds to a button operation such as a forward button, a reverse button, or a turn button, or an operation using an analog lever (running at a speed corresponding to an angle at which the lever is tilted).

Next, in the repulsive force calculation unit 111, the obtained distance information (based on the above equation (4) when the two-dimensional mode is set and the above equation (7) when the three-dimensional mode is set ( The repulsive force U (n) with respect to (measurement point) is calculated (step S104). Here, n is a natural number of 1 to N (N is the total number of measurement points).
When the repulsive force U (n) is calculated, the repulsive force component separation unit 112 converts the repulsive force U (n) into the above equations (5) to (6) when the two-dimensional mode is set. If set, the axis components are separated based on the above equations (8) to (10) (step S108). The processes in steps S106 to S108 are repeated until all the N measurement points are completed.
As a result, the repulsive forces U (1) to U (N) for N measurement points are expressed as “η / 2 · (1 / ρ ₁ −1 / ρ ₀ ) ² ” to “η / 2 · (1 / ρ _N −1 / ρ ₀ ) ² ”is calculated.

When the repulsive forces U (1) to U (N) are in the two-dimensional mode, the x-axis component “F _px (1) to F _px (N)” and the y-axis component “F _py (1) to F _py (N) ". In the case of the three-dimensional mode, the x-axis component “F _px (1) to F _px (N)”, the y-axis component “F _py (1) to F _py (N)”, and the z-axis component “F _pz (1) to F _pz (N) ”.
When the above calculation is completed for N measurement points (“Yes” branch in step S110), the avoidance repulsive force generation unit 113 then determines the maximum value and negative value in the positive direction for each axis from the separated axis components. The maximum value in the direction is extracted (steps S112 to S114).
Specifically, when the two-dimensional mode is set, the maximum value in the positive direction of the x-axis component from “F _px (1) to F _px (N)” based on the above formulas (11) to (14) extracting F _pxp and negative directions of the maximum value F _pxn, from the _{"F py (1) ~F py (} N) ", the positive direction of the maximum value F _PYP and the negative direction of the maximum value F _Pyn the y-axis component extraction To do.

Further, based on the above equation if the 3D mode is set (11) to (16), the positive maximum value F _pxp and the negative direction of the maximum value F _pxn of the x-axis component, the y-axis component in addition to the positive direction of the maximum value F _PYP and the negative direction of the maximum value F _Pyn, from the _{"F pz (1) ~F pz (} N) ", the z-axis component positive maximum value F _PZP and negative directions of the The maximum value F _pzn is extracted.
Next, the avoidance repulsive force generation unit 113 determines whether or not negative values are included in the maximum values in the positive direction of all the axis components (step S116). If it is included (“Yes” branch in step S116), the maximum negative values are all changed to zero (0) (step S118). Specifically, when the two-dimensional mode is set, the avoidance repulsive force generation unit 113 _changes the negative value of F _pxp and F _pyp to zero and sets the three-dimensional mode. If it is, the negative value of F _pxp , F _pyp and F _pzp is changed to zero.

That is, when the leg wheel type robot 1 receives only a negative repulsive force from an obstacle for each axis component, the maximum value in the positive direction of the axis component of the corresponding axis is set to zero.
Subsequently, the avoidance repulsive force generation unit 113 determines whether or not a positive value is included in the negative maximum values of all the axis components (step S120). If positive values are included ("Yes" branch in step S120), the maximum values that are positive values are all changed to zero (0) (step S122).

That is, when the leg wheel type robot 1 receives only a positive repulsive force from an obstacle for each axis component, the maximum value in the negative direction of the axis component of the corresponding axis is set to zero. Specifically, when the two-dimensional mode is set, the avoidance repulsive force generation unit 113 _{changes the} positive value of F _pxn and F _pyn to zero and sets the three-dimensional mode. If it is, the positive value _among F _pxn , F _pyn and F _pzn is changed to zero.

Further, when the maximum values in the positive direction of all the axis components are all positive values and the maximum values in the negative direction are both negative values ("No" branch in both steps S116 and S120). ), The maximum values in the positive direction and the negative direction are both left as they are.
After the determination and change processing, the avoiding repulsive force generation unit 113 calculates the sum of the maximum value in the positive direction and the maximum value in the negative direction for each axis (step S124).

Specifically, when the two-dimensional mode is set, the avoidance repulsive force generation unit 113 is based on the above formulas (1) to (2), and “F _px = F _pxp + F _pxn ” and “F _py = F _pyp + F _pyn "is calculated.
Further, when the three-dimensional mode is set, the avoidance repulsive force generation unit 113 is based on the above formulas (1) to (3), and “F _px = F _pxp + F _pxn ” and “F _py = F _pyp + F and _pyn ", to calculate the" _{F pz} = F pzp + F _pzn ".

When the two-dimensional mode is set, the avoidance repulsive force generation unit 113 uses the above-described calculation result of the x-axis component “F _px ” and the y-axis component “F _py ” as axis components. generating a avoidance repulsive force F _p of the formula (17) (step S126).
In addition, when the three-dimensional mode is set, the avoidance repulsive force generation unit 113 has the x-axis component “F _px ”, the y-axis component “F _py ”, and the z-axis component “F _pz ” that are the above calculation results. ”As a shaft component, the avoiding repulsive force F _{p represented} by the above equation (18) is generated (step S126).
Further, the avoiding repulsive force generating unit 113 outputs the generated avoiding repulsive force F _p to the motor control unit 102 (step S128).

On the other hand, when the motor control unit 102 detects the user's operation input and acquires the avoidance repulsive force F _p from the obstacle avoidance support unit 101 (“Yes” branch in steps S200 and S202), the motor control unit 102 receives The obstacle position information input via the communication I / F 64 is acquired (step S204). Here, since the obstacle is a corner (a fixed object), the speed of the obstacle is “0”.

Next, the rotation angle information of each motor input via the angle take-in I / F 62 is acquired (step S206), and the obstacle is detected based on the operation input, the avoidance repulsive force F _p , the position information, and the rotation angle information. While avoiding, a motor command for moving from point A to point B is generated (step S208). Based on the generated motor command, a motor command signal is output to each motor via the motor command output I / F 61 (step S210).
Thereby, each motor is controlled, and the leg-wheel type robot 1 travels on wheels in a direction corresponding to the motor command signal at a speed corresponding to the motor command signal.
The series of processes of the obstacle avoidance support unit 101 and the motor control unit 102 are repeatedly performed while there is an operation input.

In this way, it is assumed that the leg-wheel type robot 1 has moved from point A to point B with respect to the corner as shown in FIG. In this case, avoiding repulsive force F _p, when relative positional change from point A to point B, the two-dimensional mode is set, as shown in FIG. 10 (a), switching of the closest distance However, both the x-axis component F _px and the y-axis component F _py show continuous changes.

On the other hand, when the movement from the point A to the point B shown in FIG. 9A is performed, the avoiding repulsive force calculated using the closest distance as in the prior art is as shown in FIG. 11A. In the intermediate position from the point A to the point B, the closest distance is switched, and F _px and F _py are rapidly changed at the intermediate position.
Further, as shown in FIG. 9B, there is a straight line where there are obstacles on the positive direction side and negative direction side of the x-axis, and there are no obstacles within the measurement range on the positive direction side and negative direction side of the y-axis. road and, even when moving from point a in FIG. 9 (b) to point B, it is possible to calculate the repulsive force F _p for avoiding similarly to the case of the corner.

However, since there is no obstacle in the y-axis direction, the y-axis component F _py in the avoiding repulsive force F _p becomes zero. Therefore, the change in the avoiding repulsive force F _{p is} the change in the x-axis component F _px . Also in this case, as shown in FIG. 10B, the x-axis component F _px shows a continuous change even when the closest distance is switched. _Further , the change in the maximum value F _pxp of the x-axis component at this time is a continuous change as indicated by the broken line in the figure, and the change in the maximum value F _{pxn in} the negative direction of the x-axis component is also _indicated by the dotted line in the figure. As shown in the figure, it is a continuous change.

On the other hand, when the movement from the point A to the point B shown in FIG. 9B is performed, the avoiding repulsive force F _p calculated using the closest distance as in the prior art is shown in FIG. As described above, the closest distance is switched at the intermediate position from the point A to the point B, and a rapid change of F _p (F _px ) occurs at the intermediate position.
As described above, the leg-wheel type robot 1 of the present embodiment measures the two-dimensional or three-dimensional distance information of the obstacle in the laser range sensor 200, and based on the measured distance information, the obstacle avoidance support unit In 101, the repulsive force virtually received from each measurement point of the obstacle can be calculated.

Further, in the obstacle avoidance support unit 101, the calculated repulsive force is separated into axial components, the maximum value in the positive direction and the maximum value in the negative direction are extracted for each axis from the separated axial components, and the extracted positive direction and negative direction are extracted. Is calculated for each axis, and the avoidance repulsive force F _p can be generated from the calculation result.
Further, the rotation angle information in the motor control unit 102, where the operation input from the operation unit 300, and the repulsive force F _p for avoiding, and speed information of the obstacle input from the laser range sensor 200, is input from the encoder of the motors A motor command signal can be generated based on the above.

As a result, even if the closest distance is switched according to the moving position of the moving body, the repulsive force for avoidance changes continuously, so that stable movement control that does not cause a rapid change in speed or the like can be performed. .
Also, since to generate a avoidance repulsive force F _p by using the sum of the positive maximum value and the maximum value in the negative direction of the axis components of the repulsive force, impact, etc. on the density of the magnitude and the measurement point group of the obstacle it is possible to generate a work around for the repulsive force F _p which is not subject to.

[Second Embodiment]
Next, a second embodiment of the present invention will be described with reference to the drawings. FIG. 12 is a diagram showing an obstacle avoidance support device, an obstacle avoidance support method, and a mobile object according to a second embodiment of the present invention.
In the present embodiment, the obstacle avoidance assisting apparatus according to the present invention is applied to a guidance robot that is a moving body and that can travel by wheels.

FIG. 12 is a side view of the guidance robot according to the present embodiment, and is a diagram showing the configuration of the guidance robot 81.
As shown in FIG. 12, the guidance robot 81 of the present embodiment includes a base 82, a pair of left and right moving wheels 84R and 84L, two distance information measuring units 86F and 86B, a gripping unit 88, and an input. A value detection unit 90, a movement mode switching operation unit 92, a speaker 94, and two caster devices 96F and 96B are provided.
The base 82 is formed of a hollow body having an octagonal shape when viewed from above. In addition, the shape of the base | substrate 82 is not limited to this, For example, you may form in the shape which makes | forms a circle by the top view.

  The moving wheels 84R and 84L are wheels that are rotated by a driving force generated by a moving actuator 162 described later. The moving wheels 84R and 84L are connected to the moving actuator 162 via a pulley or the like that can transmit the driving force generated by the moving actuator 162. In addition, the pair of wheels are respectively disposed on the lower surface of the base 82 so as to be rotatable to both side surfaces with respect to the center of the base 82. In FIG. 12, only the left moving wheel 84 </ b> L is shown among the pair of wheels forming the moving wheel 84.

  The distance information measurement units 86F and 86B are formed from sensors having the same configuration as the laser range sensor 200 in the first embodiment. The distance information measurement unit 86F is disposed on the front side of the base 82, and the distance information measurement unit 86B is disposed on the rear side of the base 82. The distance information measurement units 86F and 86B are not limited to laser range sensors, and may be formed using, for example, an infrared sensor or an ultrasonic sensor.

  Specifically, the distance information measuring units 86F and 86B perform the first scanning process and the second scanning process in the same manner as the laser range sensor 200 of the first embodiment, and the front side and the rear side of the guidance robot 81 Measure distance information of obstacles within the three-dimensional measurement range on the side. The distance information measuring units 86F and 86B may be configured to perform only the first scanning process and measure the distance information of the obstacle in the two-dimensional measurement range.

The grip portion 8 includes a frame portion (not shown) and a grip portion (not shown), and the frame portion is attached to the upper surface of the base 82 via an input value detection portion (not shown).
The grip portion is formed in a substantially cylindrical shape that can be held by a guided person (user) with one hand.
The input value detection unit is a six-axis force sensor capable of detecting forces applied in directions of three axes (x, y, z) orthogonal to each other and moments around the axes of these three axes. It is interposed between the upper surface of the base body 82.

As a result, the input value detection unit applies the input to the grip unit 88 via the grip unit by the guided person who grips the grip unit in the directions of the three axes orthogonal to each other, and the three axis Each of the moments around the axis is formed so as to be detectable.
The caster device 96 includes a caster (not shown), an expansion / contraction actuator (not shown), and a floor reaction force detector (not shown).

The caster includes a balancing wheel, a balancing wheel support part, and a caster support shaft.
The telescopic actuator is an actuator that causes the caster device 96 to expand and contract by moving the caster up and down. The expansion / contraction actuator has a linear motion shaft that linearly moves and is supported by a caster frame. Therefore, the caster forms an expansion / contraction portion that is interposed between the base 82 and the balancing wheel and that expands and contracts in a direction perpendicular to the rotation axis of the balancing wheel.

  Further, the guidance robot 81 includes a movement system similar to the movement system shown in FIG. 4 in the first embodiment. Specifically, unnecessary components such as the joint motor 40, the encoder 42, the driver 44, and the leg tip sensor 38 related to the leg 12 are removed from the movement system shown in FIG. 4, and the laser range sensor 200 is replaced with the distance information measuring unit. 86F and 86B, the operation unit 300 is replaced with a gripping unit 88, and the speaker 78 is replaced with a speaker 94. Further, each wheel motor 50, encoder 52, and driver 54 for driving the wheel 20 (× 4) is replaced with an actuator 162, encoder, and driver for driving the moving wheels 96F and 96B. In addition, elements that are unique to the guiding robot 81, such as an expansion / contraction actuator that drives the caster device 96, a driver, a floor reaction force detection unit, and an input / output I / F, are added.

In addition, the guidance robot 81 is an obstacle avoidance support unit 101 that is a functional component related to movement control during obstacle avoidance during wheel traveling, as with the leg wheel type robot 1 of the first embodiment. And a movement control unit 100 including a motor control unit 102.
In the present embodiment, the obstacle avoidance support unit 101 is based on the distance information of the obstacles from the distance information measurement units 86F and 86B, as in the first embodiment, the above formulas (1) to (18). based on), calculate the avoidance repulsive force F _p.

The motor control unit 102 also receives force input from the grip unit 88 via the six-axis force sensor, obstacle position information from the distance information measurement unit 86F or 86B, and avoidance from the obstacle avoidance support unit 101. The repulsive force F _p and the rotation angle information from each encoder are acquired. And based on these acquired information, the command signal to the driver of actuator 162 for driving control including the obstacle avoidance operation is generated.

  Thus, the guidance robot 81 of the present embodiment can measure the two-dimensional or three-dimensional distance information of the obstacle in the distance information measuring units 86F and 86B. Based on the measured distance information, the obstacle avoidance support unit 101 virtually receives from each measurement point of the obstacle present on the moving path of the guidance robot 81 (within the measurement range on the front side or the rear side). The repulsive force can be calculated.

Further, in the obstacle avoidance support unit 101, the calculated repulsive force is separated into axial components, the maximum value in the positive direction and the maximum value in the negative direction are extracted for each axis from the separated axial components, and the extracted positive direction and negative direction are extracted. Is calculated for each axis, and the avoidance repulsive force F _p can be generated from the calculation result.
Further, in the motor control unit 102, is input and power input from the gripping portion 88, and the repulsive force F _p for avoiding the position information of the obstacle input from the distance information measuring section 86F and 86B, the encoder of the motors A motor command signal can be generated based on the rotation angle information.

Thereby, even if the closest distance is changed according to the movement position of the guidance robot 81, the repulsive force for avoidance continuously changes, and therefore, stable movement control is performed so that a rapid change in speed does not occur. Can do.
Also, since to generate a avoidance repulsive force F _p by using the sum of the positive maximum value and the maximum value in the negative direction of the axis components of the repulsive force, impact, etc. on the density of the magnitude and the measurement point group of the obstacle it is possible to generate a work around for the repulsive force F _p which is not subject to.

In each of the above embodiments, the distance information storage unit 110 and the memory correspond to the distance information storage unit or the distance information storage unit, the repulsive force calculation unit 111 corresponds to the repulsive force calculation unit, and the repulsive force component separation unit 112 Corresponds to the component separation unit.
In the first embodiment, the avoiding repulsive force generating unit 113 corresponds to the avoiding repulsive force generating unit, and the motor control unit 102 and the drivers 44 and 54 correspond to the actuator control unit.

In the second embodiment, the avoiding repulsive force generating unit 113 corresponds to the avoiding repulsive force generating unit, and the motor control unit 102 and the driver 54 correspond to the actuator control unit.
In the first embodiment, step S106 corresponds to a repulsive force calculation step, step S108 corresponds to a repulsive force component separation step, and steps S110 to S126 correspond to an avoiding repulsive force generation step.

In each of the above-described embodiments, the component configured by the CPU 60, the memory, and the obstacle avoidance support unit 101 of the leg wheel type robot 1 or the guidance robot 81 corresponds to the obstacle avoidance support device. The robot 1 and the guidance robot 81 correspond to a moving body.
In each of the above embodiments, the obstacle avoidance assisting device is a dedicated program stored in the memory by the CPU 60 by sharing the hardware resources such as the CPU 60 and the memory of the leg-wheel robot 1 and the guiding robot 81. However, the present invention is not limited to this configuration.

For example, a separate processor and memory may be provided, and the processor may be configured as an independent device that implements each function of the obstacle avoidance support unit 101 by executing a dedicated program.
Moreover, it is good also as a structure which performs not all the processes by the software which executes a program for exclusive use, but a part or all of each process of the obstruction avoidance assistance part 101 using hardware, such as an electric circuit.

Further, in each of the above embodiments, the configuration using the potential function designed by Hathib shown in the above equations (4) and (7) has been described for calculating the repulsive force. However, the present invention is not limited to this configuration, and the repulsive force can be calculated. Other known potential functions may be used.
Further, in the first embodiment, the obstacle avoidance assistance device according to the present invention is applied to a leg wheel type robot, and in the second embodiment, the obstacle avoidance assistance device according to the present invention is Although the example applied to the guidance robot has been described, the present invention is not limited to this, and the present invention can also be applied to other moving bodies such as a robot that moves using other moving means, a ship, and an automobile.

Further, in each of the above embodiments, the obstacle avoidance support device according to the present invention is applied to a robot that performs a movement operation in response to a user's operation. However, the present invention is not limited to this configuration. It is also possible to apply to a moving body capable of autonomous movement.
In the first embodiment, the distance information to the obstacle is measured by the laser range sensor 200. However, the present invention is not limited to this configuration, and other distance measuring sensors such as a distance image sensor are used. It is good.

In each of the above embodiments, the range sensor is rotated by the laser range sensor 200 to scan the measurement range. However, the present invention is not limited to this, and a mirror inserted on the optical axis of the range sensor is rotated. It is also possible to employ a configuration in which scanning is performed.
Each of the above embodiments is a preferred specific example of the present invention, and various technically preferable limitations are given. However, the scope of the present invention is particularly limited in the above description. Unless otherwise stated, the present invention is not limited to these forms. In the drawings used in the above description, for convenience of illustration, the vertical and horizontal scales of members or parts are schematic views different from actual ones.
Further, the present invention is not limited to the above-described embodiments, and modifications, improvements, and the like within the scope that can achieve the object of the present invention are included in the present invention.

1 leg-wheel type robot 100 movement control unit 101 obstacle avoidance support unit 102 motor control unit 110 distance information storage unit 111 repulsive force calculation unit 112 repulsive force component separation unit 113 repulsive repulsive force generation unit 200 laser range sensor 300 operation units 40 and 50 motor 42, 52 Encoder 12b, 14, 44, 54 Driver 60 CPU
61 Motor command output I / F
62 Angle capture I / F
64 Communication I / F
74 Wireless communication unit 76 Hub 81 Guide robots 86F and 86B Distance information measurement unit

Claims (7)

An obstacle avoidance support apparatus that supports an avoidance operation of an obstacle existing on a moving path of a moving body,
A distance information storage unit for storing distance information measured for a plurality of measurement points along the shape of an obstacle present on the moving path of the moving body;
Based on the distance information stored in the distance information storage unit, a repulsive force calculation unit that calculates a repulsive force virtually received from each measurement point of the plurality of measurement points of the obstacle using a potential function;
A repulsive force component separating unit that separates repulsive force corresponding to each measurement point calculated by the repulsive force calculating unit into axial components of each axis defining coordinates of the measuring point;
An avoidance repulsive force generation unit that generates a repulsive force for avoidance, which is a repulsive force related to the avoidance operation of the obstacle, based on the separated axial component of each measurement point;
The avoidance repulsive force generation unit calculates, for each axis, the sum of the maximum value of the positive direction component and the maximum value of the negative direction component of each axis in the repulsive force axis component corresponding to each measurement point, An obstacle avoidance support device that generates the repulsive force for avoidance based on a result. The coordinates of the measurement point are expressed by two-dimensional coordinates (x, y) defined by the x axis and the y axis orthogonal to each other, and the nth (n is 1 ≦ n where N is the total number of measurement points) When the component in the x-axis direction in the repulsive force corresponding to the measurement point ≦ N natural number) is expressed as F _px (n) and the component in the y-axis direction is expressed as F _py (n), the avoiding repulsive force generating unit , A component F _px in the x-axis direction and a component F _py in the y-axis direction in the repulsive force for avoidance are calculated based on the following equations (1) to (2):
F _px = F _pxp + F _pxn (1)
F _py = F _pyp + F _pyn (2)
In the above formulas (1) to (2), F _pxp is the maximum value of the positive component of the x axis in the repulsive force corresponding to the N measurement points, and F _pxn corresponds to the N measurement points. The maximum value of the negative component of the x-axis in the repulsive force, F _pyp is the maximum value of the positive component of the y-axis in the repulsive force corresponding to the N measurement points, and F _pyn is the N measurement The maximum value of the negative component of the y-axis in the repulsive force corresponding to the point. In the equations (1) to (2), when the F _pxp is a negative value, the value of the F _pxp is set to zero. When F _pyp is a negative value, the value of the F _pyp is zero, when the F _pxn is a positive value, the value of the F _pxn is zero, and when the F _pyn is a positive value, the F _pyn The obstacle avoidance support device according to claim 1, wherein the value of the obstacle is set to zero. The coordinate of the measurement point is represented by three-dimensional coordinates (x, y, z) further including a z-axis orthogonal to both the x-axis and the y-axis, and the z-axis at the repulsive force corresponding to the nth measurement point When the direction component is represented by F _pz (n), the avoidance repulsive force generation unit calculates a component F _pz in the z-axis direction of the avoidance repulsion based on the following equation (3):
F _pz = F _pzp + F _pzn (3)
In Formula (3), F _pzp is the maximum value of the positive component of the z axis in the repulsive force corresponding to the N measurement points, and F _pzn is the z axis in the repulsive force corresponding to the N measurement points. _Is the maximum value of the negative component, and when the F _pzp is a negative value, the value of the F _pzp is zero, and when the F _pzn is a positive value, the value of the F _pzn is zero The obstacle avoidance assistance device according to claim 2, wherein: When the coordinates of the measurement point are represented by two-dimensional coordinates (x, y) defined by the x axis and the y axis orthogonal to each other, the repulsive force calculation unit is nth (n is the total number of measurement points) Repulsive force U (n) corresponding to a measurement point of 1 to N where N is a natural number) is calculated based on the potential function shown in the following equation (4), and the repulsive force component separation unit calculates the repulsive force U (n ) _Is separated into a component F _px (n) in the x-axis direction and a component F _py (n) in the y-axis direction based on the following formulas (5) to (6): 2. The obstacle avoidance support device according to 2.
U (n) = η / 2 · (1 / ρ _n −1 / ρ ₀ ) ² (4)
F _px (n) = ∂U (n) / ∂x (5)
F _py (n) = ∂U (n) / ∂y (6)
However, in said Formula (4), (eta) is a positive weight constant, (rho) ₀ is a positive constant, the range which the said potential function influences, (rho) _n is the present position of the said mobile body To the nth measurement point. When the coordinates of the measurement point are represented by three-dimensional coordinates (x, y, z) defined by the x-axis, the y-axis, and the z-axis that are orthogonal to each other, n is a repulsive force component separating unit that calculates a repulsive force U (n) corresponding to a measuring point of 1 to N where N is the total number of measuring points based on the potential function shown in the following equation (7). Is based on the repulsive force U (n) based on the following equations (8) to (10), the component F _px (n) in the x-axis direction, the component F _py (n) in the y-axis direction, and the component F in the z-axis direction. The obstacle avoidance assistance device according to claim 1, wherein the obstacle avoidance assistance device is separated into _pz (n).
U (n) = η / 2 · (1 / ρ _n −1 / ρ ₀ ) ² (7)
F _px (n) = ∂U (n) / ∂x (8)
F _py (n) = ∂U (n) / ∂y (9)
F _pz (n) = ∂U (n) / ∂z (10)
However, in the above formula (7), η is a positive weight constant, ρ ₀ is a positive constant, and the range in which the potential function affects, and ρ _n is the current position of the moving object. To the nth measurement point. An obstacle avoidance support method for supporting an obstacle avoidance operation on a moving path of a moving object,
Based on the distance information of the distance information storage means storing the distance information measured for a plurality of measurement points along the shape of the obstacle present on the moving path of the moving body, the potential function is used. Repulsive force calculating step for calculating repulsive force virtually received from each of the plurality of measuring points of the obstacle;
A repulsive force component separating step for separating repulsive force corresponding to each measurement point calculated in the repulsive force calculating step into axial components of each axis defining coordinates of the measuring point;
A repulsive force generation step for avoidance that generates a repulsive force for avoidance, which is a repulsive force related to the avoidance operation of the obstacle, based on the separated axial component of each measurement point,
In the repulsive repulsive force generation step, the sum of the maximum value of the positive direction component and the maximum value of the negative direction component of each axis in the repulsive axis component corresponding to each measurement point is calculated for each axis, An obstacle avoidance support method, wherein the avoidance repulsion is generated based on a calculation result. A substrate;
A moving mechanism having an actuator for moving the substrate;
A distance information measurement unit that measures distance information for a plurality of measurement points along the shape of an obstacle present on the moving path;
An instruction input receiving unit that receives an instruction input related to movement including information on the moving direction of the moving body from the user via the operation unit;
The obstacle avoidance assistance device according to any one of claims 1 to 5,
An actuator control unit for controlling the actuator based on the repulsive force for avoidance calculated based on the distance information measured by the distance information measurement unit in the obstacle avoidance support device and the instruction input received by the instruction input reception unit; A moving object comprising:

Images