JP2005128722A - Vehicle with obstruction avoidance function - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To avoid a collision with a step and a rollover into a ditch or the like in a vehicle with an obstruction avoidance function. <P>SOLUTION: During a travel of the vehicle 1, a two-dimensional laser radar LD2 scans ahead of the vehicle (#6), and observes and stores hazard points such as a step and a ditch (#7). The rotational speed of driving wheels is detected (#8) and the travel of the vehicle 1 is calculated (#9) for storage (#7). If collision predicting means produces a collision determination (#11) from the stored hazard points and changing vehicle travel information, and a vehicle travel direction (#10), a collision avoiding correction instruction is issued (#12) to correct the operator's steering angle setting (#2) and accelerator operation (#4) so that the vehicle avoids the collision. Measured points near the travel road surface by the two-dimensional laser radar LD2 are stored by coordinate conversion, so that obstruction positions can be recognized even in the radar dead angle. <P>COPYRIGHT: (C)2005,JPO&NCIPI

Description

  The present invention relates to a vehicle with an obstacle avoidance function.

Conventionally, a person uses a part of the body such as a hand such as a wheelbarrow, an operating table, a hand cart such as a factory transport cart, and a cart operated by senior cart, wheelchair, golf cart, etc. In a vehicle to be operated, a function for avoiding a collision is incorporated. For example, a small vehicle with driving assistance that includes an obstacle detection unit that detects an obstacle, turns off the driving force in the forward direction, and turns on the driving force in the backward direction based on the obstacle detection signal is known. There is also known an automatic steering device for a vehicle that avoids a collision by correcting a steering pattern when a possibility of a collision occurs (see, for example, Patent Document 1).
Japanese Patent Laid-Open No. 5-50934

  However, in a cart or the like, although the wheel diameter is relatively small and steps and grooves in the vicinity of the road surface cause a problem in traveling, the above-described small vehicle with driving assistance and the automatic steering device avoid collision. Steps and grooves are not taken into consideration as target obstacles. Although there is one using a three-dimensional distance sensor (3D distance sensor) for obstacle detection, it is not determined whether the object detected on the road surface in the vicinity of the vehicle is a road or a step, and the step is detected. It cannot be avoided. In addition, when an object detected below a predetermined height from the wheel ground contact surface of the vehicle is a road, a gently changing hill may be erroneously detected as an obstacle.

  SUMMARY OF THE INVENTION An object of the present invention is to provide a vehicle with an obstacle avoidance function capable of avoiding a collision with a step or a fall into a groove or the like in a vehicle operated by a person.

  In order to achieve the above object, the invention according to claim 1 is a three-dimensional obstacle capable of recognizing an obstacle near the traveling road surface as three-dimensional information in a vehicle that is travel-controlled so as to detect and avoid the obstacle. The object recognition means, the collision prediction means for judging the collision risk from the obstacle information obtained by the three-dimensional obstacle recognition means, and the traveling state are corrected so as not to collide when predicted to collide with the collision prediction means. A vehicle with an obstacle avoidance function having a travel correction means.

  According to a second aspect of the present invention, in the vehicle with an obstacle avoidance function according to the first aspect, the three-dimensional obstacle recognition means includes a plurality of ultrasonic sensors and / or one or more ultrasonic sensors with a swing mechanism. Is.

  According to a third aspect of the present invention, in the vehicle with the obstacle avoidance function according to the first aspect, a three-dimensional laser radar is provided as the three-dimensional obstacle recognition means.

  According to a fourth aspect of the present invention, in the vehicle with an obstacle avoidance function according to the first or third aspect, the vehicle further includes a vehicle movement amount measuring unit, and the three-dimensional obstacle recognition unit detects the obstacle and The position of the obstacle in the area where the obstacle is outside the detection area is estimated from the obstacle information obtained by the three-dimensional obstacle recognition means and the vehicle movement amount obtained by the vehicle movement amount measurement means. .

  According to a fifth aspect of the present invention, in the vehicle with an obstacle avoidance function according to the first aspect, the vehicle further includes a two-dimensional laser radar installed at a depression angle with respect to a horizontal plane of the vehicle main body, and a vehicle movement amount measuring means. The three-dimensional obstacle information is obtained from the obstacle information from the three-dimensional laser radar and the vehicle movement amount from the vehicle movement amount measuring means.

  According to a sixth aspect of the present invention, in the vehicle with an obstacle avoidance function according to the fourth or fifth aspect, the vehicle movement amount measuring means is a wheel rotation measuring device or an acceleration measuring device.

  According to a seventh aspect of the present invention, in the vehicle with an obstacle avoidance function according to any one of the first to sixth aspects, the obstacle recognizing means has a height change greater than a predetermined level from a predetermined horizontal plane. It is assumed to be an obstacle position.

  The invention according to claim 8 is the vehicle with an obstacle avoidance function according to any one of claims 1 to 7, further comprising a vehicle moving speed detector, wherein the collision prediction means is the obstacle recognition means. A collision is predicted from the recognized obstacle position and the vehicle movement direction obtained by the vehicle movement speed detector.

  According to a ninth aspect of the present invention, in the vehicle with an obstacle avoidance function according to the eighth aspect, the collision predicting means is a vehicle obtained by the obstacle position recognized by the obstacle recognizing means and the vehicle moving speed detector. The speed at which the vehicle approaches the obstacle is obtained from the moving speed, and the collision is predicted from the obstacle approach speed and the distance between the obstacle and the vehicle.

  According to a tenth aspect of the present invention, in the vehicle with an obstacle avoidance function according to any one of the first to ninth aspects, the travel correction means drives a turning force for turning in a safe direction to the left and right drive wheels. It is added as a difference in force.

  An eleventh aspect of the present invention is the vehicle with an obstacle avoidance function according to any one of the first to tenth aspects, wherein the travel correction means limits the speed of propulsion or turning or lateral movement so as not to collide. It is.

  According to the first aspect of the present invention, the three-dimensional obstacle recognizing means can detect a groove, a step or the like on the road, and can correct and avoid the traveling state.

  According to the invention of claim 2, the three-dimensional obstacle recognition means can be formed at low cost using a plurality of ultrasonic sensors or ultrasonic sensors with a swing mechanism, and three-dimensional information can be obtained.

  According to the invention of claim 3, it is possible to obtain three-dimensional information with high positional resolution in real time by high-speed scanning of a three-dimensional laser radar.

  According to the fourth aspect of the present invention, obstacle information in the vicinity of the vehicle can be stored even when the sensor detection area is left, and obstacles in the blind spot area can be avoided.

  According to the fifth aspect of the present invention, the three-dimensional obstacle information can be obtained at a relatively low cost by adding the traveling direction information to the obstacle information obtained by the two-dimensional laser radar.

  According to the invention of claim 6, the vehicle movement amount can be obtained relatively inexpensively and accurately by measuring the wheel rotation, and the vehicle movement amount can be accurately obtained even if the wheel or the like slips by acceleration measurement. .

  According to the seventh aspect of the present invention, it is possible to avoid a groove, a step, a wall, and the like as an obstacle based on an amount of change from a predetermined horizontal plane.

  According to the eighth and ninth aspects of the present invention, it is possible to easily predict a collision by calculation from information on an obstacle position and a vehicle moving direction.

  According to the invention of claims 10 to 11, the vehicle can be naturally guided to a safe state.

  Hereinafter, a vehicle with an obstacle avoidance function according to an embodiment of the present invention will be described with reference to the drawings. FIG. 1 shows a block configuration of a vehicle with an obstacle avoidance function, and FIGS. 2 to 4 show structures of the vehicle with an obstacle avoidance function as seen from the plane, side, and front. The vehicle 1 with an obstacle avoidance function (hereinafter, vehicle 1) is a vehicle that is travel-controlled so as to detect the obstacle B and avoid it. As shown in FIG. 1, the vehicle 1 has a three-dimensional obstacle recognition unit 20 that can recognize an obstacle B in the vicinity of the traveling road surface as three-dimensional information, and a collision risk from obstacle information obtained by the three-dimensional obstacle recognition unit 20. A collision predicting means 21 for determining the nature, a travel correcting means 22 for correcting the traveling state so as not to collide when the collision predicting means 21 predicts a collision, and further, a three-dimensional obstacle recognizing means 20 includes an obstacle B Vehicle movement amount measuring means 23 for estimating the position of the obstacle B in the area where the obstacle B is outside the detection area from the vehicle movement amount. These units are integrated and controlled by the control unit 12. A data processing unit including the control unit 12 and a memory 15 for storing data and the like is housed in the automatic control unit ACU. A detailed description of the block configuration will be given after the description of FIGS.

  As shown in FIGS. 2 to 4, the vehicle 1 includes a drive wheel 2, a steering wheel 3, a rotation speed detection sensor 5, a steering angle detection sensor 7, an ultrasonic distance sensor US (US1 to US9), and an automatic control unit ACU. It is a four-wheel riding type carriage equipped with the above. The drive wheel 2 is driven by a drive motor 4 via a differential gear 4a. The steered wheels 3 are steered by a handle 6 and a steering mechanism (a handle shaft 6a, a pinion gear 6b and a rack 6c ahead thereof). That is, when the operator operates the handle 6, the rotation of the handle 6 is transmitted to the rack 6c via the handle shaft 6a and the pinion gear 6b ahead of the handle 6 and the rack 6c moves to the left and right to move the steering wheel 3 to the left and right. The steering angle θ can be changed. The steered wheels 3 are also steered by a steering correction motor 8 and a correction drive mechanism (pinion 8a and rack 6c) (described later). The handle 6 is provided with a driving force adjusting lever (accelerator) 4a and a front / rear selector switch 4b. The output of the driving motor 4 is adjusted by the driving force adjusting lever 4a, and the rotation direction of the driving motor 4, and thus the forward / backward direction of the vehicle 1 is switched by the front / rear selector switch 4b.

  The rotational speed detection sensor 5 detects the rotational speed of the left and right drive wheels 2. The steering angle detection sensor 7 is composed of, for example, a rotary encoder, and detects the steering angle θ by detecting the rotation angle of the handle shaft 6a. The ultrasonic distance sensors US <b> 1 to US <b> 9 are fixed to the front of the vehicle 1 so as to share the detection direction so that obstacles in the front space of the vehicle 1 can be detected three-dimensionally. That is, as shown in FIG. 4, among the ultrasonic sensors US arranged in a 3 × 3 grid in the vertical and horizontal directions, the upper row is the upper front, the middle row is the front horizontal direction, and the lower row is the lower row. The detection area is the front and lower part. For example, the detection areas A4, A5, A6 of the ultrasonic distance sensors US4, US5, US6 arranged in the vertical direction are shown in FIG. Since the ultrasonic distance sensors US3, US6, and US9 have a detection area in the lower front part of the vehicle 1, obstacles such as a groove B1 and a step B2 near the road surface 10 can be detected. An automatic control unit ACU that performs data processing is housed under the seat S.

  Next, returning to FIG. 1, the details of the block configuration of the vehicle 1 will be described. The three-dimensional obstacle recognition unit 20 includes a plurality of ultrasonic distance sensors US and a distance data processing unit 11. The ultrasonic distance sensor US detects the position of the obstacle B, and the distance data processing unit 11 processes the distance data obtained by the ultrasonic distance sensor US. Obstacle information processed by the distance data processing unit 11 is stored in the memory 15 via the control unit 12.

  The collision prediction unit 21 includes a data comparison unit 13 and a collision determination unit 14. The data comparison unit 13 compares the data stored in the memory 15 during traveling of the vehicle 1 at regular time intervals, and the collision determination unit 14 determines the possibility of a collision with an obstacle based on the result of the data comparison. to decide.

  The travel correction means 22 includes a steering correction motor 8, a correction motor driver 8d, and a correction drive mechanism 8m. The correction motor driver 8d rotates the steering correction motor 8 in response to a command from the control unit 12, and performs the steering correction of the steering theory 3 through the correction drive mechanism 8m. The steering wheel 3 is normally steered via the steering mechanism 6m when the operator M operates the steering handle 6. The steering angle is detected by the steering angle detection sensor 7 and stored in the memory 15 via the control unit 12. Since the correction drive mechanism 8m and the steering mechanism 6m are coupled by the rack 6c (FIG. 2), a change in the steering angle by the steering correction motor 8 is also detected by the steering angle detection sensor 7.

  The operation information of the driving force adjusting lever (accelerator) 4a and the front / rear selector switch 4b by the operator M is transmitted to the drive motor control unit 16, and the state is transmitted to the control unit 12 and also sent to the drive motor driver 4d. . The drive motor driver 4 d receives the command from the drive motor control unit 16 and rotates the drive motor 4 to drive the drive wheels 2.

  The vehicle movement amount measuring means 23 includes a rotation speed detection sensor (encoder) 5 and a movement amount calculation unit 17. The rotation speed detection sensor (encoder) 5 detects the rotation speed of the drive wheel 2, and the movement amount calculation unit 17 calculates the movement amount and movement direction of the vehicle 1 from the rotation speed of the drive wheel 2 and the travel time. The movement amount and the movement direction information are stored in the memory via the control unit.

  Next, an operation example of obstacle avoidance of the vehicle 1 having the block configuration described above will be described. FIG. 5 shows the operation of the vehicle 1 paying attention to the flow of information, and FIG. 6 shows the relationship between the obstacle and the steering. The control unit 12 collects obstacle information from the plurality of ultrasonic distance sensors US, information on the steered wheels 3 from the steering angle sensor 7, and information on the drive wheels 2 from the encoder 5, and sends them to the collision determination unit 13. hand over. When an obstacle is detected, the collision determination unit 13 predicts and determines whether the vehicle 1 collides with the detected obstacle. Specifically, when the steering angle θ (positive counterclockwise) is positive and greater than or equal to a predetermined value θL (ie, turning left) in the forward state, one of the ultrasonic sensors US1 to US3 is predetermined as the obstacle distance. When the value less than the value L1 is returned (FIG. 6, obstacle B3), the collision determination unit 13 determines that the vehicle 1 may collide with the obstacle B3. The control unit 12 issues a steering correction command to the correction motor driver 8d to generate the steering correction force of the steering correction motor 8 in the direction of decreasing the steering angle θ.

  In addition, when any of the ultrasonic distance sensors US1 to US6 returns an obstacle distance equal to or smaller than a predetermined value L2 that is smaller than L1 (FIG. 6, obstacle B4), the control unit 12 responds to the drive motor driver 4d. And issue a drive correction command in the direction to decrease the forward speed. The drive correction command value from the control unit 12 and the operation command value from the motor control unit 16 by the operation of the accelerator 4a of the operator M are superimposed and input to the drive motor driver 4d.

  Further, when the steering angle θ is negative and the absolute value is equal to or greater than the predetermined value θR in the forward state (that is, turning right, and usually θR = θL), any one of the ultrasonic sensors US7 to US9 is the predetermined value as the obstacle distance. When the value less than R1 is returned, the collision determination unit 13 determines that the vehicle 1 may collide with an obstacle. The control unit 12 issues a steering correction command to the correction motor driver 8d, and causes the steering correction motor 8 to generate a steering correction force in a direction to decrease the absolute value of the steering angle θ. Further, when any of the ultrasonic distance sensors US4 to US9 returns an obstacle distance equal to or smaller than a predetermined value R2 that is smaller than R1, the control unit 12 advances the driving speed with respect to the drive motor driver 4d as described above. A drive correction command is issued in a direction to decrease the value.

  Further, when the forward and steering angle θ is a value between −θR and θL (that is, a substantially straight traveling state), when any of the ultrasonic distance sensors US1 to US9 returns a value equal to or smaller than the predetermined value Dc, A drive correction command is issued in a direction to decrease the forward speed. Although the foregoing has been described with respect to forward travel, a distance sensor may be provided at the rear of the vehicle in the same manner as described above, and similar obstacle avoidance control may be performed during reverse travel. Further, instead of the drive correction command described above, a drive stop command is input to the motor control unit 16, and the motor control unit 16 stops the drive motor 4 when the drive stop command is received. Alternatively, a brake mechanism may be provided and the avoidance determination unit may directly control it.

  Next, a three-dimensional laser radar used in place of the above-described plurality of fixed ultrasonic distance sensors US will be described. 7 and 8 show an example of a three-dimensional radar radar and its measurement. The three-dimensional laser radar shown in FIG. 7 reflects the laser light from the upper light emitting unit by the mirror 60 and projects it onto the target space. If there is reflected light, it reflects by the mirror 60 and is received by the upper light receiving unit. It is a radar that receives light and calculates the distance of the reflecting object from the time between laser light emission and reception. The mirror 60 scans the laser beam vertically and horizontally by a swinging motion around the flat axis and a rotational motion around the vertical axis.

  The oscillating motion around the horizontal axis of the mirror 60 is performed by applying a magnetic field generated by the coil 64 to the magnetic material constituting the mirror 60 via the yokes 62 and 63. The rotational movement around the vertical axis is performed by the motor 61. The reflection surface posture of the mirror 60 is performed by detecting the reflected light of the laser beam from the mirror 60 with the two position detectors PSD1 and PSD2. The direction of the object reflecting the laser beam is obtained from the reflecting surface posture of the mirror 60. Further, by shifting the phase of the oscillating motion and the rotational motion of the mirror 60, as shown in FIG. 8, the scanning trajectory can be shifted to perform a planar scan.

  Next, a vehicle equipped with a three-dimensional laser radar will be described. FIGS. 9A and 9B show the vehicle 1 in a traveling state equipped with a three-dimensional laser radar LD3 in front. The block configuration of the vehicle 1 is the same as that in the block configuration shown in FIG. 1 described above, in which a plurality of ultrasonic distance sensors US of the three-dimensional obstacle recognition means 20 are replaced with a three-dimensional laser radar LD3. The detection area A10 of the three-dimensional laser radar is a three-dimensional space of a cone extending forward with the vertex of the three-dimensional laser radar, and an obstacle can be recognized three-dimensionally for each spatial point in the space. However, as shown in FIG. 9A, even if the obstacle B5 is once detected in the radial detection area A10 by the three-dimensional laser radar LD3, as shown in FIG. If it is too close to the obstacle 5, it will fall out of the detection range. Similarly, an obstruction in the upper or left space outside the detection area A10 becomes a blind spot and is not detected.

  In order to deal with such a blind spot area and avoid an obstacle, if the obstacle once detected in the detection area moves away from the detection area, the obstacle is removed from the vehicle from the movement amount of the vehicle body. An estimate of where it is is made. Therefore, it is necessary for the vehicle 1 to grasp the current position at the present time. This self-position determination is performed by so-called dead reckoning (self-position estimation) that estimates a trajectory from a travel distance or the like and estimates a current position. The method will be described below.

  FIG. 10 shows an absolute coordinate system ΣF fixed on the floor surface in the building and the vehicle coordinate system ΣD (n−) fixed to the vehicle 1 at times t = t (n−1) and t = t (n). 1) shows the positional relationship between ΣD (n) and obstacle B. The coordinate axis of the absolute coordinate system is represented by XY, and the coordinate axis of the vehicle coordinate system moving with the vehicle is represented by xy. For simplicity, the XY plane and the xy plane are horizontal planes and the Z and z directions are not considered. A specific point fixed to the vehicle 1 can be arbitrarily selected as the origin of the vehicle coordinate system. Therefore, the center of the vehicle 1 is the origin, the front front direction of the vehicle 1 is the x-axis direction, and the left direction perpendicular to the x-axis is the y-axis direction.

  The obstacle distance data in the vehicle coordinate system obtained by a sensor such as a three-dimensional laser radar at each measurement time (for example, every control cycle ΔT) is expressed in a new vehicle coordinate system according to the movement of the vehicle. By converting into, the relationship between the current position of the vehicle 1 and the obstacle can be grasped. First, measured from the (current) position (X (n−1), Y (n−1)) of the vehicle predicted at time t = t (n−1) and the encoder value at the current time t = t (n). The vehicle position after the control cycle ΔT (t (n) = t (n−1) + ΔT) using the vehicle center speed (Vx (n), Vy (n), ω (n)) calculated as the value (X (n), Y (n)) is calculated. θ is the steering angle, and ω is the time change rate of each speed, that is, the steering angle θ. The vehicle position in the absolute coordinate system ΣF is expressed by the following expressions (1) to (5).

  Next, at time t = t (n), the coordinates (xb (n), yb (n)) of the obstacle B in the vehicle coordinate system ΣD (n) are obtained. This will be described with reference to FIG. The vehicle coordinate system ΣD (n) is expressed by various quantities at time t = t (n−1) before one control cycle ΔT. Therefore, the coordinates (xb (n), yb (n)) of the obstacle B in the vehicle coordinate system ΣD (n) are the coordinates of the origin of the vehicle coordinate system ΣD (n) in the vehicle coordinate system ΣD (n−1). Using (Δx (n), Δy (n)) and the coordinates (xb (n−1), yb (n−1)) of the obstacle B, the following equation (6) is obtained. Further, the coordinates (Δx (n), Δy (n)) of the origin of the vehicle coordinate system ΣD (n) are X (n−1), Y (n−1) in the above equations (1) and (2). ) Are set to 0, and X (n) and Y (n) are obtained as Δx (n) and Δy (n), respectively.

  Using the above equations (6) and (7), based on the position coordinates (xb (0), yb (0)) of the obstacle B measured by the three-dimensional laser radar LD3 at time t = 0, The position coordinates of the obstacle B at the time t = t (n) = n · ΔT are obtained by accumulating coordinate transformations according to the movement of the position for each control period ΔT of the vehicle without measuring the position of the obstacle B. It is done. Therefore, since the obstacle information can be held even in the blind spot of the aforementioned sensor, obstacle avoidance is possible. The vehicle speeds Vx and Vy may be obtained by differentiation of an angle measuring device or integration of an accelerometer.

  Next, a method for obtaining three-dimensional information of an obstacle using a two-dimensional laser radar instead of the above-described three-dimensional laser radar will be described. FIGS. 12A, 12B, and 12C show the vehicle 1 including the two-dimensional laser radar LD2. The block configuration of the vehicle 1 is the same as that obtained by replacing the plurality of ultrasonic distance sensors US of the three-dimensional obstacle recognition means 20 with the two-dimensional laser radar LD2 in the block configuration shown in FIG. In the two-dimensional laser radar LD2, the space through which the laser beam has passed is two-dimensional, and when the laser beam is irradiated onto a plane, the locus becomes linear (in the above-described three-dimensional laser radar LD3, the space through which the laser beam has passed). Is approximately three-dimensional and the locus is approximately planar).

  As shown in FIG. 12, the two-dimensional laser radar LD2 is used by scanning laser light left and right in the same plane as if a fan is spread forward and downward from one point in front of the vehicle 1. When most of the traveling road surface is substantially flat, the locus LS on the road surface scanned by the two-dimensional laser radar LD2 is linear. The detection area A11 of the two-dimensional laser radar LD2 has a fan shape, and the depression angle η and scan angle ξ (defined within the detection area A11 plane) and the linear distance r from the two-dimensional laser radar LD2 to the obstacle B7 are Three-dimensional information of the detected obstacle B7 is obtained. That is, when the origin of the vehicle coordinate axis ΣD is on the road surface directly below the two-dimensional laser radar LD2, and the height position of the two-dimensional laser radar LD2 is H, the coordinates (xb, yb, zb) of the obstacle B7 are xb = r · cos (ξ) · cos (η), yb = r · sin (ξ), and zb = Hr · cos (ξ) · sin (η) (see FIG. 13).

  Obstacles are obtained by performing acquisition of the three-dimensional information of the obstacles by such a single scan every control cycle ΔT of the automatic control unit and storing them in the memory together with the vehicle movement amount from the vehicle movement amount measuring means. More complete three-dimensional information can be obtained. FIGS. 14A and 14B show examples of acquiring obstacle information by a two-dimensional laser radar. During traveling while scanning with the two-dimensional laser radar LD2, as shown in FIG. 14A, a convex obstacle B7 in the vicinity of the road surface 10 is detected at time t = t (n−1), and FIG. As shown in b), it is assumed that the groove obstacle B8 in the vicinity of the road surface 10 is detected at time t = t (n) (see FIG. 12). The setting of the vehicle coordinate system is the same as that in FIG.

  When the measurement points by the two-dimensional laser radar LD2 are represented by points p1 to p9, at time t = t (n−1), the lines other than the measurement point p2 are arranged on a straight line, and at time t = t (n) Lines other than the points p6 and p7 are arranged on a straight line. That is, the measurement point p2 is a point on the convex obstacle B7, and the measurement points p6 and p7 are points in the groove obstacle B8. As described above, the obstacle information can be obtained from the coordinate comparison of the measurement points by one laser scan. For example, at time t = t (n−1), the xyz coordinate value of the measurement point p2 is different from other measurement points having the same xz coordinate value, and has unique coordinate values (xx2, yy2, zz2). doing. Similarly, at time t = t (n), the obstacle B8 can be recognized from the xz coordinate values of the measurement points p6 and p7.

  Also, by comparing the coordinate values of a set of measurement points obtained by different laser scans within a predetermined time period, an obstacle can be recognized in response to a case where the traveling road surface cannot be regarded as a substantially flat surface. . Such a method can cope with a case where the road surface shape changes gently. Since the moving vehicle exists at different positions at different times, it is necessary to express the measurement point coordinates in the same coordinate system in order to compare the measurement points at different times. For example, all the measurement points can be expressed by the absolute coordinate ΣF or can be expressed by the vehicle coordinate system ΣD.

  The method of expressing all measurement points in the vehicle coordinate system ΣD is excellent in that it is easy to recognize the position and direction of an obstacle at the current position of the vehicle. In this case, when comparing the measurement points, it is necessary to convert all of the stored past measurement points to be compared to the current vehicle coordinate system ΣD, but the calculation load is particularly problematic according to the current high-speed MPU or DSP. It will not be. In any case, since the vehicle is moving, coordinate conversion of measured values is necessary, and the above-described dead reckoning and coordinate conversion can be applied. The vehicle speed used for the conversion can be obtained by differentiation of an angle measuring device or integration of an accelerometer.

  Next, the operation of a vehicle with an obstacle avoidance function that travels while performing obstacle avoidance using the two-dimensional laser radar LD2 will be described. FIG. 15 shows the running state of the vehicle 1 (operator omitted). The block configuration shown in FIG. 1 will be referred to as appropriate. In the normal traveling state of the vehicle 1, a steering wheel angle change (# 2) and a steering angle change (# 3) are generated by the steering wheel operation (# 1) by the operator, and the accelerator operation (# 4) by the operator is also performed. ) Changes the output of the drive motor (# 5). While the vehicle 1 is traveling, a vehicle forward scan is performed by the two-dimensional laser radar LD2 (# 6), and the measurement result is processed in the automatic control unit ACU to recognize and store a dangerous part such as an obstacle. (# 7). Further, the rotational speed of the drive wheel is detected by the encoder (# 8), and the movement amount of the vehicle 1 is calculated (# 9) and stored in the automatic control unit ACU (# 7).

  When the collision determination is performed by the collision determination unit 14 of the collision prediction means 21 based on the stored dangerous part and the information on the moving amount of the vehicle every moment and the vehicle moving direction (# 10) obtained from the encoder ( # 11) A collision avoidance correction command is issued from the control unit 12 (# 12). The collision avoidance correction command includes a correction of the steering wheel angle and a correction of the accelerator, and correction is made for each of the setting of the steering wheel angle (# 2) and the accelerator operation (# 4) so that the vehicle does not collide. The running state is corrected at the beginning.

  Next, an example of collision prediction by the collision prediction unit will be described. FIG. 16 shows a left turning state of the vehicle 1. The possibility of collision between the obstacle and the vehicle is predicted from the obstacle position recognized by the obstacle recognition means and the vehicle movement direction obtained by the vehicle movement speed detector. The vehicle 1 is a powered vehicle capable of turning in addition to propulsion (x direction) and lateral movement (y direction). As shown in FIG. 16, it is assumed that the vehicle 1 is turning left while keeping the propulsion speed Vx, lateral movement speed Vy, and turning speed ω at the turning center constant, and is at the point P1 at the current time. . A collision area A21 having a predetermined divergence angle 2φ is set in front of the vehicle 1. It is assumed that the vehicle 1 moves while maintaining a constant speed and reaches a point P2 on the predicted movement route RT after a predetermined time Tp. Here, the intersection P4 between the circumference of the predetermined radius L centered on the point P1 and the right side of the collision area A21, the intersection P3 of the left side of the collision area A22 when the vehicle is at the point P2 and the circumference of the radius L Determine. When there is an obstacle in the area surrounded by the line connecting the points P4, P1, P2, P3 and the arc P3-P4, it is concluded that the vehicle 1 collides with the obstacle if the vehicle 1 continues to turn at a constant speed from the point P1. To do.

  Next, another example of collision prediction by the collision prediction unit will be described. 17A and 17B show the relationship between the relative speeds of the vehicle 1 and the obstacle B. FIG. The collision prediction means obtains the speed approaching the obstacle B from the position (xb, yb) of the obstacle B represented by the vehicle coordinate system ΣD and the vehicle movement speed (Vx, Vy) obtained by the vehicle movement speed detector. The collision is predicted from the obstacle approach speed Vbc and the distance Lb between the obstacle B and the vehicle 1. Observing the obstacle B stationary on the traveling road surface from the vehicle coordinate system ΣD fixed to the vehicle 1, due to the moving speed (Vx, Vy, ω) including turning of the vehicle 1 with respect to the traveling road surface (absolute coordinate system ΣF), Obstacle B appears to move. As shown in FIG. 17A, the obstacle B has a speed of (−V) depending on the linear movement speed V of the vehicle 1 and a speed of (Lb · ω) depending on the turning angular speed ω. Therefore, the velocity Vb of the obstacle B obtained by combining these velocities is (−Vx−yb · ω, −Vy + Lb · ω) in the ΣD coordinate system. For simplicity, the ω term may be ignored.

  When the obstacle speed Vb viewed from the vehicle 1 is obtained, Vb is divided into a component Vbc in the direction connecting the turning center of the vehicle 1 and the obstacle, and a component Vbt perpendicular thereto. The Vbc component is used as a speed at which the obstacle B tries to collide with the vehicle 1, and it is determined from the relationship between Vbc and Lb whether or not the obstacle B is in the danger zone. For example, with the hatched area DG in the relationship graph between the obstacle approach speed Vbc and the obstacle distance Lb shown in FIG. 18 as a dangerous area, a collision occurs if the obstacle distance Lb for a certain obstacle approach speed Vbc is close and enters the hatched area DG. Judged as dangerous.

  Next, an example of collision avoidance by the travel correction unit will be described. FIG. 19 shows a vehicle 1 having driving wheels to which driving force can be applied independently. FIGS. 20 (a) and 20 (b) show the obstacle avoidance operation during forward and reverse travel. The travel correction means can avoid obstacles by adding a turning force for turning in a safe direction as a difference in driving force with respect to the left and right drive wheels. The vehicle 1 can apply the left wheel driving force FL and the right wheel driving force FR to the left and right driving wheels 2 independently by the left and right independent driving sources 4L and 4R. Further, the front wheel 3a of the vehicle 1 is a free wheel, and therefore the vehicle 1 is steered by the driving force applied to the left and right drive wheels 2. The forward driving direction and turning driving force (Fx, M) of the vehicle 1 is Fx = FR + FL, M, where the center of the line segment connecting the left and right driving wheels 2 is the turning center and the distance between the left and right driving wheels 2 is Lt. = (FR-FL) · Lt / 2. The driving force M for turning is a moment of force. When M> 0, the vehicle 1 turns left.

  Obstacle avoidance by the generation of the driving force described above will be described. As shown in FIG. 20 (a), when the vehicle moves forward from the current position (Vx, Vy, ω) and current driving force (Fx, M) and turns left, the predicted movement path during the forward movement. A case will be described in which an obstacle B exists on the right side of RT and it is determined that the obstacle B needs to be avoided. The traveling correction means limits the speed of propulsion, turning, or lateral movement so as not to collide. Then, the obstacle is avoided by correcting the trajectory on the left side of the predicted movement route RT. Therefore, the traveling correction means corrects the driving force to the left and right driving wheels 2 to strengthen the left turning by setting the turning driving force M to M + ΔM (ΔM> 0) and the forward driving force Fx to Fx−ΔF (ΔF>). 0) The forward speed is decelerated. Similarly, as shown in FIG. 20B, in order to avoid a collision with an obstacle B on the opposite side of the center of curvature of the predicted movement route RT when the vehicle turns to the right, the traveling correction means uses a driving force M for turning. Is corrected so that the absolute value becomes larger, and the radius of curvature of the moving path is made smaller. The correction of the driving forces Fx and M may be determined as a function of the distance between the vehicle 1 and the obstacle and the approach speed (Lb and Vbc in FIG. 17). The present invention is not limited to the above-described configuration, and various modifications can be made.

The block block diagram of the vehicle with an obstacle avoidance function which concerns on one Embodiment of this invention. The top view which shows the structure of a vehicle same as the above. The side view of a vehicle same as the above. The front view of a vehicle same as the above. The schematic diagram explaining control of a vehicle same as the above. The top view explaining the obstacle avoidance of a vehicle same as the above. The perspective view which shows the structure of the three-dimensional laser radar used with a vehicle same as the above. Explanatory drawing of the angle scan by a three-dimensional laser radar same as the above. (A) (b) is a side view of the same vehicle provided with the three-dimensional laser radar. Explanatory drawing of the self-position estimation (dead reckoning) in a vehicle same as the above. Explanatory drawing of the self-position estimation (dead reckoning) in a vehicle same as the above. (A) is a top view of the same vehicle provided with the two-dimensional laser radar, (b) (c) is a side view of the same. Explanatory drawing of the measurement point coordinate by a two-dimensional laser radar same as the above. (A) (b) is a perspective view explaining the three-dimensional obstacle recognition by a two-dimensional laser radar same as the above. The perspective view which shows a mode that it drive | works while performing the three-dimensional obstacle recognition of a vehicle same as the above. The top view explaining the collision prediction at the time of turning driving | running | working of a vehicle same as the above. (A) (b) is a top view explaining the collision prediction in a vehicle same as the above. The graph for a collision danger area judgment based on the obstacle approach speed used with a vehicle same as the above. The top view which shows the other example of the vehicle with an obstacle avoidance function which concerns on this invention. (A) is a top view which shows the obstacle avoidance operation | movement at the time of advance in a vehicle same as the above, (b) is a top view which shows the obstacle avoidance operation at the time of reverse in the same vehicle.

Explanation of symbols

1 Vehicle with obstacle avoidance function 5 Encoder (wheel rotation measuring device)
20 3D obstacle recognition means 21 Collision prediction means 22 Travel correction means 23 Vehicle movement amount measurement means LD2 2D laser radar LD3 3D laser radar US, US1 to US9 Ultrasonic sensor

Claims (11)

In vehicles that are controlled to detect obstacles and avoid them,
3D obstacle recognition means capable of recognizing obstacles near the traveling road surface as 3D information;
A collision prediction means for judging a collision risk from the obstacle information obtained by the three-dimensional obstacle recognition means;
A vehicle with an obstacle avoidance function, comprising: a traveling correction unit that corrects a traveling state so that a collision does not occur when the collision prediction unit predicts a collision.   The vehicle with an obstacle avoidance function according to claim 1, wherein the three-dimensional obstacle recognition means includes a plurality of ultrasonic sensors and / or one or more ultrasonic sensors with a swing mechanism.   The vehicle with an obstacle avoidance function according to claim 1, further comprising a three-dimensional laser radar as the three-dimensional obstacle recognition means.   Vehicle movement amount measuring means is further included, and the position of the obstacle in an area where the obstacle is outside the detection area after the three-dimensional obstacle recognition means detects the obstacle is determined as the three-dimensional obstacle recognition means. The vehicle with an obstacle avoidance function according to claim 2 or 3, wherein the vehicle is estimated from the obstacle information obtained by the above and the vehicle movement amount obtained by the vehicle movement amount measuring means. A two-dimensional laser radar installed at a depression angle with respect to the horizontal plane of the vehicle body, and a vehicle movement amount measuring means;
2. The vehicle with an obstacle avoidance function according to claim 1, wherein three-dimensional obstacle information is obtained from the obstacle information from the two-dimensional laser radar and the vehicle movement amount from the vehicle movement amount measuring means.   The vehicle with an obstacle avoidance function according to claim 4 or 5, wherein the vehicle movement amount measuring means is a wheel rotation measuring device or an acceleration measuring device.   The obstacle avoidance function according to any one of claims 1 to 6, wherein the obstacle recognizing means sets an obstacle position where there is a change in height above a predetermined level from a predetermined horizontal plane. vehicle.   A vehicle movement speed detector; and the collision prediction means predicts a collision from the obstacle position recognized by the obstacle recognition means and the vehicle movement direction obtained by the vehicle movement speed detector. The vehicle with an obstacle avoidance function according to any one of claims 1 to 7.   The collision prediction means obtains a speed approaching the obstacle from the obstacle position recognized by the obstacle recognition means and the vehicle movement speed obtained by the vehicle movement speed detector, and calculates the obstacle approach speed and the obstacle and vehicle. The vehicle with an obstacle avoidance function according to claim 8, wherein a collision is predicted from a distance between them.   The obstacle avoidance function according to any one of claims 1 to 9, wherein the travel correction unit adds a turning force for turning in a safe direction as a difference in driving force with respect to the left and right driving wheels. Vehicle with.   The vehicle with an obstacle avoidance function according to any one of claims 1 to 10, wherein the travel correction means limits a speed of propulsion, turning, or lateral movement so as not to collide.

Images