Industrial vehicle early warning and obstacle avoidance method and system based on binocular camera

Abstract

The present application relates to an industrial vehicle early warning and obstacle avoidance method based on a binocular camera, wherein the method comprises: obtaining a binocular image of a weak field of view of an industrial vehicle through a binocular camera, obtaining the internal reference information and relative external reference information of the first camera and the second camera in the binocular camera, and correcting the binocular image according to the internal reference information and the relative external reference information to obtain a corrected binocular image; obtaining a pedestrian detection frame in the corrected binocular image through a target detection algorithm, projecting the pedestrian detection frame to the calibrated target ground, and calculating the projection distance data from the pedestrian to the industrial vehicle based on the projection result; calculating and outputting early warning information according to the projection distance data and the real-time braking distance and the real-time deceleration distance, and performing real-time obstacle avoidance control according to the early warning information. Through this application, the problems of poor early warning effect and obstacle avoidance accuracy of industrial vehicles in related technologies are solved, the early warning accuracy and obstacle avoidance accuracy are improved, and the safety factor of personnel is improved.

Description

Technical Field

[0001] The present application relates to the field of computer vision technology, and in particular to an industrial vehicle early warning and obstacle avoidance method and system based on a binocular camera. Background Art

[0002] Due to the special characteristics of industrial vehicles such as large weak field of vision, high frequency of reversing, heavy load weight and long braking distance, serious industrial vehicle accidents such as scratches, collisions, crushing and crashes often occur during operation, causing great personal injury and economic losses. The safety of industrial vehicles has become an important issue that the global industrial vehicle industry must face.

[0003] In the related technologies, binocular cameras are mainly used in combination with traditional machine vision, infrared TOF and video analysis to provide early warning and obstacle avoidance based on the relationship between obstacle distance and braking distance and deceleration distance. However, in the above technologies, due to the poor accuracy of obstacle distance, braking distance and deceleration distance, when the vehicle's driving state changes greatly or the vehicle is in a turning state, its early warning and obstacle avoidance effects are not ideal.

[0004] Currently, no effective solution has been proposed to address the problems of poor warning effect and obstacle avoidance accuracy of industrial vehicles in related technologies. Summary of the invention

[0005] The embodiments of the present application provide a binocular camera-based industrial vehicle early warning and obstacle avoidance method and system to at least solve the problem of poor early warning and obstacle avoidance effects of industrial vehicles in related technologies.

[0006] In a first aspect, an embodiment of the present application provides an industrial vehicle early warning and obstacle avoidance method based on a binocular camera, the method comprising:

[0007] Acquire a binocular image of a weak field of view of an industrial vehicle through a binocular camera and perform correction to obtain a corrected binocular image, perform binocular stereo matching on the binocular corrected image to obtain a disparity map, and convert the disparity map from an image coordinate system to a world coordinate system to obtain a 3D point cloud image;

[0008] Based on the 3D point cloud map, the target ground on which the industrial vehicle is traveling is marked, and reserved distance information, as well as driving information and vehicle parameters of the industrial vehicle are obtained, wherein the driving information includes vehicle speed, braking time, deceleration time, reduction speed and steering angle, the vehicle parameters include braking coefficient, deceleration coefficient and vehicle wheelbase, and the reserved distance information includes reserved braking distance and reserved deceleration distance;

[0009] Calculating a real-time braking distance and a real-time deceleration distance according to the driving information, the vehicle parameters and the reserved distance information, and determining a warning area frame according to the preset range information;

[0010] Acquire a pedestrian detection frame in the corrected binocular image by using a target detection algorithm, separate 3D points representing a human body in the pedestrian detection frame to obtain separation points, and project the separation points to the target ground surface, and calculate projection distance data from the pedestrian to the industrial vehicle in a straight-ahead area or a turning area based on the projection result;

[0011] According to the projection distance data, the real-time braking distance and the real-time deceleration distance, early warning information is calculated and output according to preset rules, and real-time obstacle avoidance control is performed according to the early warning information.

[0012] In some embodiments, performing binocular stereo matching on the corrected binocular image to obtain a disparity map includes:

[0013] The disparity map is obtained by performing stereo matching on the corrected binocular image through a cost aggregation algorithm, including:

[0014] Performing edge detection on the corrected binocular image by using a Sobel operator, mapping the data after the edge detection into a preprocessed image, and obtaining gradient information of the preprocessed image;

[0015] Calculating a gradient cost according to the gradient information by a sampling method, and calculating a SAD cost based on the corrected binocular image;

[0016] Based on the real-time planning strategy, energy is accumulated in each direction on the corrected binocular image, and the matching cost in each direction is added to obtain a combined matching cost;

[0017] The disparity map is obtained by executing a post-processing step, wherein the post-processing step includes uniqueness detection, sub-pixel interpolation, left-right consistency detection and connected area detection.

[0018] In some embodiments, converting the disparity map from the image coordinate system to the world coordinate system to obtain a 3D point cloud map comprises:

[0019] The binocular camera is subjected to stereo correction to obtain a reprojection matrix. According to the reprojection matrix, the disparity map is converted from the image coordinate system to the world coordinate system to obtain a 3D point cloud map by the following formula:

[0020]

[0021] in, is the three-dimensional coordinate in the world coordinate system, d is the disparity information in the disparity map, c′ x Indicates the principal point of the right image taken by the binocular camera. x and y are the values ​​in the image coordinates, respectively. c x 、c y ,t xand f are the intrinsic parameters of the reprojection matrix respectively.

[0022] In some embodiments, locating the target ground for the industrial vehicle to travel on based on the 3D point cloud image includes:

[0023] Mapping the ground plane points on the corrected binocular image to the 3D point cloud image to obtain the spatial coordinates of the ground plane points;

[0024] Based on the spatial coordinates of the ground plane point, the projection parameter equation of the target ground is fitted by the least squares method, wherein the projection parameter equation can be expressed as follows:

[0025]

[0026] Among them, x, y, z are the spatial coordinates of the ground plane point, and A, B and C are equation parameters.

[0027] In some embodiments, calculating the real-time braking distance and the real-time deceleration distance according to the driving information, the vehicle parameters and the reserved distance information includes:

[0028] The real-time braking distance is calculated by the following formula:

[0029]

[0030] Among them, l′ stop is the real-time braking distance, l stop is the braking distance, v is the vehicle speed, t stop is the braking time, k stop is the braking coefficient;

[0031] The real-time deceleration distance is calculated by the following formula:

[0032]

[0033] Among them, l′ dec is the real-time deceleration distance, l dec is the deceleration reserved distance, v is the vehicle speed, v stop is the reduction speed, t dec is the deceleration time, k dec is the deceleration coefficient.

[0034] In some embodiments, separating the 3D points representing the human body in the pedestrian detection frame to obtain separation points, and projecting the separation points to the target ground includes:

[0035] Calculating the pixel disparity of the pixel points in the pedestrian detection frame in the corrected binocular image, and obtaining the pixel points whose pixel disparity is less than or equal to a first preset threshold value as target pixel points;

[0036] Calculating the spatial position coordinates of the target pixel points based on the 3D point cloud image, and obtaining pixel points whose spatial position coordinates are greater than or equal to a second preset threshold value from among the target pixel points as the separation points;

[0037] If the coordinates of the point not on the target ground are P(x i ,y i , z i ), according to the projection parameter equation, the separation point is projected onto the target ground to obtain the projection point and calculate the projection coordinates by the following formula:

[0038]

[0039] Among them, x p ,y p , z p The values ​​of the projection coordinates on each coordinate axis, A, B, C and D are equation parameters.

[0040] In some embodiments, the projection distance data from the pedestrian to the industrial vehicle is calculated in a straight-ahead area based on the projection result, and includes:

[0041] Obtain the length of the vector formed by the projection point and the camera base point in the z-axis direction, use the vector length as the straight-line relative distance, and combine the straight-line relative distances corresponding to all projection points to obtain a straight-line relative distance list;

[0042] According to the plane vector relationship between the coordinates of the projection point in the x and z directions and the vertex coordinates of the warning area frame in the x and z directions, a straight-line boundary projection point is determined among the projection points;

[0043] When the number of the straight-through boundary projection points is greater than a preset threshold, the relative distances of the straight-through boundary projection points are determined in the straight-through relative distance list, and the maximum distance and the minimum distance of the relative distances of the straight-through boundary projection points are obtained;

[0044] Based on the maximum distance and the minimum distance, the projection distance data from the pedestrian to the industrial vehicle in the straight travel area is calculated by a histogram distance measurement algorithm.

[0045] In some embodiments, the projection distance data from the pedestrian to the industrial vehicle in the turning area calculated based on the projection result includes:

[0046] Obtaining a steering angle of the industrial vehicle during steering, calculating a steering arc radius according to the steering angle and a wheelbase of the industrial vehicle, and determining a steering center point according to the steering arc radius and a camera base point;

[0047] Calculate the projection radius of each projection point to the center point of the turning circle according to the projection coordinates of the projection point, and obtain the projection point that falls within the width of the turning area from each projection point according to the projection radius and the arc radius as the first projection point;

[0048] Calculate the center angle of the turning area according to the preset range information and the turning arc radius;

[0049] Determine a first vector formed by the camera base point and the center point of the circle, determine a second vector formed by the projection point and the center point of the circle, and calculate a vector angle between the first vector and the second vector;

[0050] Obtain a second projection point in the first projection point where the vector angle is smaller than the center angle of the circle, and use the second projection point as the turning into boundary projection point;

[0051] When the number of the turning entry projection points is greater than a preset point number threshold, the projection distance data from the pedestrian to the industrial vehicle in the turning area is calculated by a histogram distance measurement algorithm.

[0052] In some embodiments, the calculating and outputting of warning information according to preset rules based on the projection distance data, the real-time braking distance and the real-time deceleration distance, and performing real-time obstacle avoidance control according to the warning information include:

[0053] Obtaining the minimum value of the projection distance data;

[0054] Based on the minimum value, an alarm signal is output according to a preset rule, wherein the preset rule includes:

[0055] In d Min >l′ stoP +l′ dec Output safety signal in case of

[0056] In l′ stop +l′ dec >d Min >l′ stop Output deceleration signal in the case of

[0057] In l′ stop >d Min >0, output brake signal;

[0058] The industrial vehicle performs obstacle avoidance control based on the safety signal, the deceleration signal and the brake signal.

[0059] In the second aspect, an embodiment of the present application also provides an industrial vehicle early warning and obstacle avoidance system based on a binocular camera, comprising: a vehicle-mounted controller, a CAN communication module, an industrial computer, a binocular depth camera, a vehicle-mounted power supply and a vehicle-mounted display, for executing the industrial vehicle early warning and obstacle avoidance method based on a binocular camera as provided in the first aspect.

[0060] Compared with the related art, the industrial vehicle early warning and obstacle avoidance method based on binocular camera provided in the embodiment of the present application has the following beneficial effects:

[0061] 1. According to the vehicle driving information, vehicle parameters and reserved distance information, the real-time braking distance and real-time deceleration distance that match the specific vehicle with higher accuracy are calculated, and then early warning and obstacle avoidance are performed based on the distance, which can improve the accuracy of early warning and obstacle avoidance and improve the safety factor of personnel.

[0062] 2. For different driving states of the vehicle (including straight driving and turning), a targeted target distance calculation method is formulated. Based on this target distance calculation method, the accurate obstacle distance can be calculated under different vehicle states, and then the obstacle distance can be used for early warning, thereby further improving the accuracy of early warning and obstacle avoidance.

[0063] 3. The warning analysis results can be displayed in front of the driver's field of vision via the on-board display, providing the driver with accurate information support. BRIEF DESCRIPTION OF THE DRAWINGS

[0064] The drawings described herein are used to provide a further understanding of the present application and constitute a part of the present application. The illustrative embodiments of the present application and their descriptions are used to explain the present application and do not constitute an improper limitation on the present application. In the drawings:

[0065] Figure 1 It is a schematic diagram of an application scenario of an industrial vehicle early warning and obstacle avoidance method based on a binocular camera according to an embodiment of the present application;

[0066] Figure 2 is a flow chart of an industrial vehicle early warning and obstacle avoidance method based on a binocular camera according to an embodiment of the present application;

[0067] Figure 3 is a schematic diagram of an early warning range frame according to an embodiment of the present application;

[0068] Figure 4 is a schematic diagram of a steering area according to an embodiment of the present application;

[0069] Figure 5It is a structural block diagram of an industrial vehicle early warning and obstacle avoidance system based on a binocular camera in an embodiment of the present application. DETAILED DESCRIPTION

[0070] In order to make the purpose, technical solutions and advantages of the present application clearer, the present application is described and illustrated below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present application and are not intended to limit the present application. Based on the embodiments provided in the present application, all other embodiments obtained by ordinary technicians in the field without making creative work are within the scope of protection of the present application.

[0071] Obviously, the drawings described below are only some examples or embodiments of the present application. For ordinary technicians in this field, the present application can also be applied to other similar scenarios based on these drawings without creative work. In addition, it can also be understood that although the efforts made in this development process may be complicated and lengthy, for ordinary technicians in this field related to the content disclosed in this application, some changes in design, manufacturing or production based on the technical content disclosed in this application are just conventional technical means, and should not be understood as insufficient content disclosed in this application.

[0072] Reference to "embodiments" in this application means that a particular feature, structure, or characteristic described in conjunction with the embodiments may be included in at least one embodiment of the present application. The appearance of the phrase in various locations in the specification does not necessarily refer to the same embodiment, nor is it an independent or alternative embodiment that is mutually exclusive with other embodiments. It is explicitly and implicitly understood by those of ordinary skill in the art that the embodiments described in this application may be combined with other embodiments without conflict.

[0073] Unless otherwise defined, the technical terms or scientific terms involved in this application should be understood by people with ordinary skills in the technical field to which this application belongs. The words "one", "a", "a", "the" and the like involved in this application do not indicate a quantitative limitation, and may represent the singular or plural. The terms "include", "comprise", "have" and any of their variations involved in this application are intended to cover non-exclusive inclusions; for example, a process, method, system, product or device that includes a series of steps or modules (units) is not limited to the listed steps or units, but may also include steps or units that are not listed, or may also include other steps or units inherent to these processes, methods, products or devices. The words "connect", "connected", "coupled" and the like involved in this application are not limited to physical or mechanical connections, but may include electrical connections, whether direct or indirect. The "multiple" involved in this application refers to two or more. "And / or" describes the association relationship of associated objects, indicating that there may be three relationships, for example, "A and / or B" can represent: A exists alone, A and B exist at the same time, and B exists alone. The character " / " generally indicates that the objects before and after are in an "or" relationship. The terms "first", "second", "third", etc. involved in this application are only used to distinguish similar objects and do not represent a specific ordering of the objects.

[0074] The present application provides an industrial vehicle early warning and obstacle avoidance method based on a binocular camera, which can be applied in Figure 1 In the application scenario shown, Figure 1 is a schematic diagram of an application scenario of an industrial vehicle early warning and obstacle avoidance method based on a binocular camera according to an embodiment of the present application, such as Figure 1 As shown, when people drive industrial vehicles (such as forklifts), they need to look back frequently when reversing due to the weak field of vision, which causes great inconvenience to the driver. In this application, based on binocular image acquisition, image recognition and other technologies, combined with CAN communication equipment, industrial computers and display screens and other hardware equipment, accurate warning information can be calculated and output according to the driving conditions of industrial vehicles, and reasonable obstacle avoidance control can be performed. This greatly improves the safety factor of industrial site personnel and the comfort of vehicle drivers.

[0075] Figure 2 is a flow chart of an industrial vehicle early warning and obstacle avoidance method based on a binocular camera according to an embodiment of the present application, such as Figure 2 As shown, the process includes the following steps:

[0076] S201, acquiring a binocular image of a weak field of view of an industrial vehicle through a binocular camera and performing correction to obtain a corrected binocular image, performing binocular stereo matching on the corrected binocular image to obtain a disparity map, and converting the disparity map from an image coordinate system to a world coordinate system to obtain a 3D point cloud image;

[0077] S202, based on the 3D point cloud map, locate the target ground on which the industrial vehicle is traveling, and obtain reserved distance information, as well as driving information and vehicle parameters of the industrial vehicle, wherein the driving information includes vehicle speed, braking time, deceleration time, reduction speed and steering angle, the vehicle parameters include braking coefficient, deceleration coefficient and vehicle wheelbase, and the reserved distance information includes reserved braking distance and reserved deceleration distance;

[0078] S203, calculating the real-time braking distance and the real-time deceleration distance according to the driving information, the vehicle parameters and the reserved distance information, and determining the warning area frame according to the preset range information;

[0079] S204, obtaining a pedestrian detection frame in the corrected binocular image through a target detection algorithm, separating 3D points representing a human body in the pedestrian detection frame to obtain separation points, and projecting the separation points to the target ground, and calculating projection distance data from the pedestrian to the industrial vehicle in a straight-ahead area or a turning area based on the projection result;

[0080] S205, based on the projection distance data, the real-time braking distance and the real-time deceleration distance, the warning information is calculated and output according to the preset rules, and the real-time obstacle avoidance control is performed according to the warning information.

[0081] Through the above steps S201 to S205, compared with the industrial vehicle early warning and obstacle avoidance method in the related art, the embodiment of the present application uses the binocular image of the weak field of view of the industrial vehicle collected in real time by the binocular camera, and performs stereo correction, depth estimation, ground calibration, determination of the early warning range, and the distance and direction of pedestrians in the detection range and combines the vehicle driving information to make a safety early warning judgment. Finally, different signals within the braking and deceleration early warning range are obtained, and obstacles are avoided in real time within the safety early warning range based on the signals. Through this application, the problem of poor early warning effect and obstacle avoidance accuracy of industrial vehicles in the related art is solved, and the control accuracy and personnel safety factor are improved.

[0082] Embodiment 1

[0083] The present application embodiment provides an industrial vehicle early warning and obstacle avoidance method based on a binocular camera, comprising:

[0084] Step 1, camera calibration:

[0085] Due to the characteristics of the camera's optical lens, the image has radial distortion; at the same time, due to unavoidable errors in assembly, the camera sensor and the optical lens are not completely parallel, resulting in tangential distortion in the image. Therefore, the binocular camera needs to be calibrated to obtain the intrinsic parameter information of the first camera and the second camera, as well as the relative extrinsic parameter information between the two.

[0086] It should be noted that the intrinsic parameters of a single camera include the intrinsic parameter matrix and coefficients of the camera, and the relative extrinsic parameter information between the two is the rotation matrix and translation vector of the first camera relative to the second camera.

[0087] Among them, the distortion coefficient, intrinsic parameter matrix, rotation matrix and translation vector of the binocular camera can be expressed as:

[0088] Distortion coefficient: (k1 k2 p1 p2 k3);

[0089] Internal parameter matrix:

[0090] Rotation matrix:

[0091] Translation vector: T = (t x , t y , t z ) T .

[0092] In the above formula, k1, k2, k3 are radial distortion coefficients; p1, p2 are tangential distortion coefficients; c x , cy is the coordinate of the principal point, usually located near the center of the image; f x and f y is the focal length of the binocular camera in the horizontal and vertical directions, in pixels; α, β, γ are the rotation angles of the right camera around the three axes of the left camera coordinate system; R and T are called camera extrinsics.

[0093] Step 2, Image Acquisition

[0094] The binocular camera captures the light signal of the working environment in the weak field of vision of the industrial vehicle, converts it into an electrical signal, and finally converts it into a digital image signal to obtain a binocular image, wherein the binocular image is composed of a first image taken by a first camera and a second image taken by a second camera.

[0095] Step 3: Image Correction

[0096] Binocular correction is to eliminate distortion and align the left and right views of the acquired image based on the intrinsic parameter data (focal length, imaging origin, distortion coefficient) obtained after camera calibration and the binocular relative extrinsic parameter data (rotation matrix and translation vector), so that the imaging origin coordinates of the left and right views are consistent, the optical axes of the two cameras are parallel, the left and right imaging planes are coplanar, and the epipolar lines are aligned, thereby completing the image correction process to obtain a corrected binocular image.

[0097] Step 4: Depth Estimation

[0098] The corrected binocular image is matched with binocular stereo vision to obtain a disparity map, wherein the corrected binocular image is matched with stereo vision by using a SGBM (Semi Global Block Matching) cost aggregation algorithm to obtain a disparity map, which optionally includes:

[0099] Preprocessing: processing the input image by using a Sobel operator, performing edge detection on the corrected binocular image, mapping the data after edge detection into a preprocessed image, and obtaining gradient information of the preprocessed image;

[0100] Cost calculation, using sampling method to calculate the gradient cost based on the gradient information, and calculating the SAD cost based on the corrected binocular image;

[0101] Real-time planning, based on the real-time planning strategy, accumulates energy in each direction on the corrected binocular image, and adds the matching cost in each direction to obtain the combined matching cost;

[0102] Post-processing, performing post-processing steps to obtain a disparity map, wherein the post-processing steps include uniqueness detection, sub-pixel interpolation, left-right consistency detection, and connected area detection.

[0103] Step 5: Calculate 3D point cloud

[0104] The stereo camera is rectified to obtain the reprojection matrix, which can be represented as follows:

[0105]

[0106] According to the reprojection matrix, the disparity map is converted from the image coordinate system to the world coordinate system to obtain a 3D point cloud map through the following formula 2. The specific steps include:

[0107] Formula 1: Q(x,y,d,1) T =(X, Y, Z, W) T

[0108] Where d represents the disparity and the three-dimensional coordinates are c′ x represents the principal point of the right image. When the correction is correct, cx =c′ x ; Further, under the premise of correct stereo correction, formula 1 is expanded to obtain the following formula 2:

[0109] Formula 2:

[0110] Step 6: Ground calibration

[0111] Locate the target ground for industrial vehicles based on 3D point cloud images, including:

[0112] Select ground plane points on the calibrated binocular image, map the ground plane points to the 3D point cloud image, and obtain the spatial coordinates of the ground plane points;

[0113] The parametric equation from the ground plane point to the space coordinate is set by the following formula 3:

[0114] Formula 3: Ax+By+C=z

[0115] Based on the spatial coordinates of multiple ground planes, the projection parameter equation of the target ground is fitted by the least squares method, where the projection parameter equation can be expressed in the form of the following formula 4:

[0116] Formula 4:

[0117] Furthermore, according to Formula 4, Z = Xε, where ε=(A,B,C) T ,

[0118] Step 7: Calculate the real-time braking distance and the real-time deceleration distance

[0119] Obtain the reserved distance information, as well as the driving information and vehicle parameters of the industrial vehicle, wherein the driving information includes: vehicle speed, braking time, deceleration time, reduction speed and steering angle when the vehicle turns; the vehicle parameters include: braking coefficient, deceleration coefficient and vehicle wheelbase; the reserved distance information includes reserved braking distance and reserved deceleration distance. It should be noted that, in the above information, except for the vehicle speed, other information parameters should be consistent with the actual situation of the vehicle, and real-time testing and determination of the optimal value are required.

[0120] The real-time braking distance is calculated by the following formula 5:

[0121] Formula 5:

[0122] Among them, l′ stop is the real-time braking distance, l stop is the braking distance, v is the vehicle speed, t stop is the braking time, k stop is the braking coefficient;

[0123] The real-time deceleration distance is calculated by the following formula 6:

[0124] Formula 6:

[0125] Among them, l′ dec is the real-time deceleration distance, l dec is the deceleration distance, v is the vehicle speed, and v stop is the cutting speed, t dec is the deceleration time, k dec is the deceleration factor.

[0126] Step 8: Determine the warning area frame

[0127] Figure 3 is a schematic diagram of a straight warning range frame according to an embodiment of the present application, such as Figure 2 As shown, the warning area frame is determined according to the preset range information, the preset range information includes the preset length d and the preset width s, the braking area is determined by the reserved braking distance lstop, and the deceleration area is determined by the reserved deceleration distance l dec Sure.

[0128] Step 9: Detect pedestrians and obtain pedestrian detection frames

[0129] A lightweight target detection algorithm is used to detect and identify the corresponding positions and categories of obstacles such as people in real time, and frame the positions of pedestrians in the video screen.

[0130] Step 10: Project pedestrian detection frame

[0131] Calculate the pixel disparity of the pixel points in the pedestrian detection frame in the corrected binocular image, and obtain target pixel points whose pixel disparity is less than or equal to a first preset threshold value from the pixel points in the pedestrian detection frame;

[0132] Calculate the spatial position coordinates of the target pixel points based on the 3D point cloud image, and obtain the pixel points whose spatial position coordinates are greater than or equal to the second preset threshold value as separation points among the target pixel points;

[0133] It should be noted that the above steps are to traverse the points in the human body detection frame pixel by pixel at a certain interval. When the disparity of the pixel point in the left and right views is greater than the first preset threshold, it is considered that the point is too close or fails to match successfully and needs to be excluded. For the points that meet the requirements, the spatial position coordinates of the point are further obtained from the point cloud map, and the height from the point to the ground is calculated. If the height is less than the second fixed threshold, it needs to be excluded. In this way, the pixel points in the pedestrian orientation frame that meet the projection requirements and can represent the human body are obtained, namely the separation points;

[0134] Assume that the coordinates of any point not on the target ground are P(x i,y i , z i ), whose projection coordinates on the target ground are P′(x p ,y p , z p ). Because the vector from the projection point to the current point is perpendicular to the ground plane, according to the vertical constraint and the projection parameter equation, it is easy to know that y p , z p The following conditions must be met:

[0135]

[0136] The simultaneous equations can be obtained as follows:

[0137]

[0138] Among them, x p ,y p , z p are the values ​​of the projection coordinates on each coordinate axis, and A, B, C and D are equation parameters.

[0139] According to the above formula 7, the projection coordinates of the projection point obtained after the separation point is projected onto the ground are calculated.

[0140] Step 11, calculate the projection distance

[0141] 1) Calculation of projection distance of straight area:

[0142] The camera projection point is taken as the camera base point (x0, y0, z0), and the length of the vector formed by the projection point and the camera base point in the z-axis direction is taken as the straight-line relative distance d between the projection point and the vehicle. P′ ,in, z y_line It is the value of the direction vector perpendicular to the camera plane on the z-axis.

[0143] Combine the straight relative distances of all projection points to get the straight distance list d PLlist , where d PLlist It can be characterized by the following form:

[0144] d PLlist ={d′ P1 , d′ P2 , ..., d′ Pi}(1≤i≤len(d PLlist )

[0145] According to the projection point P′(x p ,y p , z p ) in the x, z direction coordinate P″(x p , zp ), and the x and z coordinates V of the four vertices in the straight area tl (x tl , z tl ), V tr (x tr , z tr )V bl (x bl , z bl ), V br (x br , z br ) to determine whether the projection point is in the bounds, where the determination can be made according to the following formula 8:

[0146] Formula 8:

[0147] For any projection point, if f(P″)=1, it is determined that the projection point falls into the warning area frame. Thus, the straight-line boundary projection point is determined among all the projection points according to the plane vector relationship;

[0148] When the number of the projection points entering the boundary of the straight line is higher than the corresponding preset threshold, the relative distances of the projection points entering the boundary are determined in the relative distance list, and the maximum distance and the minimum distance are obtained from the relative distances of the projection points entering the boundary.

[0149] The maximum distance and the minimum distance are divided by the fixed histogram interval width to obtain the number of histogram intervals; the difference between the distance of each point in the relative distance list and the minimum distance is divided by the interval width to obtain the histogram interval number to which each distance belongs, and the number of distances in the interval is counted plus 1, so as to obtain the histogram of the number of distances in each interval;

[0150] A sliding window with a fixed width is slid along the histogram from left to right with a specific step length. The sum of the distances of each histogram interval in the sliding window after each sliding step and the distance corresponding to the center point of the current sliding window are counted. The distance of the sliding window center point corresponding to the sliding window with the largest sum of distances is obtained as the projection distance data of the current pedestrian judgment box.

[0151] 2) Calculation of the projection distance of the steering area

[0152] Figure 4 is a schematic diagram of the steering area according to an embodiment of the present application, referring to Figure 4 , the calculation process of the projection distance of the steering area is as follows:

[0153] The steering angle of the vehicle during driving and the wheelbase of the industrial vehicle are obtained, and the steering arc radius is calculated by the following formula 9:

[0154] Formula 9: R = L / sin(θ)

[0155] Where R is the turning circle radius, L is the vehicle wheelbase, and θ is the steering angle.

[0156] Take the camera projection point as the camera base point (x0, y0, z0), the distance R from the camera base point to the center of the circle as the hypotenuse of the right triangle, calculate l1 (the vertical distance from the camera projection base point of the two right-angled sides to the front of the vehicle), and l2 (the horizontal distance from the right-angled vertex of the right triangle to the center of the circle), where l1 and l2 are calculated using the following formulas 9 and 10:

[0157] Formula 9: l1 = Rsin(θ)

[0158] Formula 10: l2=Rcos(θ)

[0159] According to the plane projection equation and the z-axis component vector of the camera plane, calculate the direction vector x_line (x x line ,y x line , z x line ) and the direction vector y_line(x y_line ,y y_line , z y_line ).

[0160] Move the base point coordinates (x0, y0, z0) in the opposite direction of y_line by l1 distances, and then move it in the direction of x_line (turn left) or in the opposite direction of x_line (turn right) by l2 distances to obtain the center point C (x′0, y′0, z′0).

[0161] It should be noted that when calculating the distance of the projection point during the turning process, there is a certain distance error between the position along the line of the starting point of the arc area and the position along the line of the camera base point due to the influence of the steering angle, so it is necessary to first calculate the distance correction term d′ of the entry point P′ , where d′ is calculated by the following formula 11 P′ , R′ is the projection radius from the projection point to the center of the turning circle:

[0162] Formula 11: d′ P′ =(RR′)×tanθ

[0163] Furthermore, the length of the arc along the line from the projection point to the camera base point is taken as the relative turning distance d of the point during the turning process. P′ , turn to the relative distance set to get the distance list d pLlist , where d is calculated by the following formula 12 P′ :

[0164] Formula 12: d P′ =θ p R′-d′ p′

[0165] Determining whether the projection point is a turning boundary point includes: determining whether the projection point is within the width range of the turning area width according to the following formula 13:

[0166] Formula 13:

[0167] If f(P′)=1, it is determined that the projection point is within the width of the turning area;

[0168] For the projection point within the width of the turning area, further determine whether it is within the length of the turning area;

[0169] According to the corresponding relationship between the length d and the arc radius R in the preset range information, the central angle θ corresponding to the length range of the turning area is calculated. d , assuming that the vector formed by the camera base point (x0, y0, z0) and the steering circle center C (x′0, y′0, z′0) is the first vector, and the projection point P′(x p ,y p , z p ) and the turning center point C (x′0, y′0, z′0) is the second vector, and the vector angle between the first vector and the second vector is calculated according to the following formula 14:

[0170] Formula 14:

[0171] Among them, p c is the first vector, p′ c is the second vector, θ p is the vector angle.

[0172] Determine whether each projection point is within the range of the central angle of the arc length: If θ p <θ d , it is proved that the projection point is within the arc region. Thus, according to the above determination relationship, the second projection points whose vector angle is smaller than the center angle are obtained from the first projection points, and these second projection points are used as the turning into boundary projection points.

[0173] When the number of projection points of the turning boundary is greater than the preset point threshold, the projection distance data from the pedestrian to the industrial vehicle in the turning area is calculated based on the histogram distance measurement algorithm. The specific method of calculating the projection distance data in this step is the same as the method of calculating the projection distance data based on the histogram in the straight area, so it will not be repeated.

[0174] Step 12: Output warning information and real-time obstacle avoidance control

[0175] 1) Output warning information:

[0176] Get the minimum value in the projection distance data;

[0177] Based on the minimum value, an alarm signal is output according to preset rules, wherein the preset rules include:

[0178] In d Min >l′ stop +l′ dec Output safety signal in case of

[0179] In l′ stop +l′ dec >d Min >l′ stop Output deceleration signal in the case of

[0180] In l′ stop >d Min >0, the brake signal is output.

[0181] 2) Obstacle avoidance control

[0182] Receive warning information, and instruct the braking device of the industrial vehicle to brake the vehicle when a brake signal is received;

[0183] In case of receiving a deceleration signal, instructing a deceleration device of the industrial vehicle to decelerate the vehicle;

[0184] Embodiment 2

[0185] The present application also provides an industrial vehicle early warning and obstacle avoidance system based on a binocular camera. Figure 5 is a structural block diagram of an industrial vehicle warning and obstacle avoidance system based on a binocular camera according to an embodiment of the present application, such as Figure 5 As shown, the system includes: a vehicle controller, a CAN communication module, an industrial computer, a binocular camera, a vehicle power supply and a vehicle display, wherein;

[0186] The on-board power supply is used to power the power unit of industrial vehicles and other computing devices and communication devices;

[0187] The industrial computer is installed with a software program based on a binocular camera-based industrial vehicle early warning and obstacle avoidance method, which is used to execute the information processing link and output control instructions in the early warning and obstacle avoidance method in the first embodiment;

[0188] The binocular camera is used to obtain binocular images of the weak field of vision of industrial vehicles. The baseline of the binocular depth camera is 120mm, which is used to collect environmental images within a range of more than 120° and between 0.2m and 8m behind the vehicle.

[0189] The on-board controller is used to calculate the real-time distance between pedestrians and vehicles;

[0190] The on-board display is used to provide video information of the poorly visible area in front of the driver's field of vision, as well as to send brake signals or deceleration signals.

[0191] The CAN module is used for communication between the vehicle controller, the industrial computer and the binocular depth camera. The CAN communication device is connected to the industrial computer and the vehicle controller in a two-way communication manner. The industrial computer is connected to the binocular depth camera and the vehicle display through signal lines and power lines.

[0192] In addition, in combination with the industrial vehicle early warning and obstacle avoidance method based on binocular camera in the above embodiment, the embodiment of the present application can provide a computer-readable storage medium for implementation. The computer-readable storage medium stores computer program instructions; when the computer program instructions are executed by the processor, any one of the industrial vehicle early warning and obstacle avoidance methods based on binocular camera in the above embodiment is implemented.

[0193] The technical features of the above embodiments may be combined arbitrarily. To make the description concise, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0194] The above embodiments only express several implementation methods of the present application, and the descriptions thereof are relatively specific and detailed, but they cannot be understood as limiting the scope of the invention patent. It should be pointed out that, for a person of ordinary skill in the art, several variations and improvements can be made without departing from the concept of the present application, and these all belong to the protection scope of the present application. Therefore, the protection scope of the patent of the present application shall be subject to the attached claims.

Claims

1. A binocular camera-based industrial vehicle warning and obstacle avoidance method, characterized in that: The method comprises: Acquire a binocular image of a weak field of view of an industrial vehicle through a binocular camera and perform correction to obtain a corrected binocular image, perform binocular stereo matching on the corrected binocular image to obtain a disparity map, and convert the disparity map from an image coordinate system to a world coordinate system to obtain a 3D point cloud image; Based on the 3D point cloud map, the target ground for the industrial vehicle to travel is marked, and reserved distance information, as well as driving information and vehicle parameters of the industrial vehicle are obtained, wherein the driving information includes vehicle speed, braking time, deceleration time, reduction speed and steering angle, and the vehicle parameters include braking coefficient, deceleration coefficient and vehicle wheelbase, and the reserved distance information includes reserved braking distance and reserved deceleration distance, wherein the marking of the target ground for the industrial vehicle to travel based on the 3D point cloud map includes: Mapping the ground plane points on the corrected binocular image to the 3D point cloud image to obtain the spatial coordinates of the ground plane points; Based on the spatial coordinates of the ground plane point, the projection parameter equation of the target ground is fitted by the least squares method, wherein the projection parameter equation is expressed in the following form: Wherein, x, y, z are the spatial coordinates of the ground plane point, and A, B and C are equation parameters; Calculating a real-time braking distance and a real-time deceleration distance according to the driving information, the vehicle parameters and the reserved distance information, and determining a warning area frame according to the preset range information; A pedestrian detection frame in the corrected binocular image is obtained by a target detection algorithm, 3D points representing a human body in the pedestrian detection frame are separated to obtain separation points, and the separation points are projected to the target ground surface, and projection distance data from the pedestrian to the industrial vehicle in a straight-ahead area or a turning area is calculated based on the projection result, wherein the separation of the 3D points representing a human body in the pedestrian detection frame to obtain separation points, and the projecting of the separation points to the target ground surface includes: Calculating the pixel disparity of the pixel points in the pedestrian detection frame in the corrected binocular image, and obtaining the pixel points whose pixel disparity is less than or equal to a first preset threshold value as target pixel points; Calculating the spatial position coordinates of the target pixel points based on the 3D point cloud image, and obtaining pixel points whose spatial position coordinates are greater than or equal to a second preset threshold value from among the target pixel points as the separation points; If the coordinates of the point not on the target ground are P(x i ,y i ,z i ), according to the projection parameter equation, the separation point is projected onto the target ground to obtain the projection point and calculate the projection coordinates through the following formula: Among them, x p ,y p , z p The projection coordinates are the values ​​on each coordinate axis, and D is the equation parameter; According to the projection distance data, the real-time braking distance and the real-time deceleration distance, early warning information is calculated and output according to preset rules, and real-time obstacle avoidance control is performed according to the early warning information.

2. The method according to claim 1, characterized in that The performing binocular stereo matching on the corrected binocular image to obtain a disparity map comprises: The disparity map is obtained by performing stereo matching on the corrected binocular image through a cost aggregation algorithm, including: Performing edge detection on the corrected binocular image by using a Sobel operator, mapping the data after the edge detection into a preprocessed image, and obtaining gradient information of the preprocessed image; Calculating a gradient cost according to the gradient information by a sampling method, and calculating a SAD cost based on the corrected binocular image; Based on the real-time planning strategy, energy is accumulated in each direction on the corrected binocular image, and the matching cost in each direction is added to obtain a combined matching cost; The disparity map is obtained by executing a post-processing step, wherein the post-processing step includes uniqueness detection, sub-pixel interpolation, left-right consistency detection and connected area detection.

3. The method according to claim 1, characterized in that The calculating of the real-time braking distance and the real-time deceleration distance according to the driving information, the vehicle parameters and the reserved distance information comprises: The real-time braking distance is calculated by the following formula: Among them, l stop is the real-time braking distance, l stop is the reserved braking distance, v is the vehicle speed, t stop is the braking time, k stop is the braking coefficient; The real-time deceleration distance is calculated by the following formula: Among them, l′ dec is the real-time deceleration distance, l dec is the reserved deceleration distance, v is the vehicle speed, v stop is the reduction speed, t dec is the deceleration time, k dec is the deceleration coefficient.

4. The method according to claim 1, characterized in that: The projection distance data from the pedestrian to the industrial vehicle calculated based on the projection result in the straight-ahead area includes: Obtain the length of the vector formed by the projection point and the camera base point in the z-axis direction, use the vector length as the straight-line relative distance, and combine the straight-line relative distances corresponding to all projection points to obtain a straight-line relative distance list; According to the plane vector relationship between the coordinates of the projection point in the x and z directions and the vertex coordinates of the warning area frame in the x and z directions, a straight-line boundary projection point is determined among the projection points; When the number of the straight-through boundary projection points is greater than a preset threshold, the relative distances of the straight-through boundary projection points are determined in the straight-through relative distance list, and the maximum distance and the minimum distance of the relative distances of the straight-through boundary projection points are obtained; Based on the maximum distance and the minimum distance, the projection distance data from the pedestrian to the industrial vehicle in the straight travel area is calculated by a histogram distance measurement algorithm.

5. The method according to claim 1, characterized in that The projection distance data from the pedestrian to the industrial vehicle calculated in the turning area based on the projection result includes: Obtaining a steering angle of the industrial vehicle during steering, calculating a steering arc radius according to the steering angle and a wheelbase of the industrial vehicle, and determining a steering center point according to the steering arc radius and a camera base point; Calculate the projection radius of each projection point to the center point of the turning circle according to the projection coordinates of the projection point, and obtain the projection point that falls within the width of the turning area from each projection point according to the projection radius and the arc radius as the first projection point; Calculate the center angle corresponding to the length range of the turning area according to the preset range information and the turning arc radius; Determine a first vector formed by the camera base point and the center point of the circle, determine a second vector formed by the projection point and the center point of the circle, and calculate a vector angle between the first vector and the second vector; Obtain a second projection point in the first projection point where the vector angle is smaller than the center angle of the circle, and use the second projection point as the turning into boundary projection point; When the number of the turning entry projection points is greater than a preset point number threshold, the projection distance data from the pedestrian to the industrial vehicle in the turning area is calculated by a histogram distance measurement algorithm.

6. The method according to claim 1, characterized in that The calculating and outputting of warning information according to the projection distance data, the real-time braking distance and the real-time deceleration distance according to a preset rule, and performing real-time obstacle avoidance control according to the warning information comprises: Obtaining the minimum value of the projection distance data; Based on the minimum value, an alarm signal is output according to a preset rule, wherein the preset rule includes: In d Min >l′ stop +l′ dec Output safety signal in case of In l′ stop +l′ dec >d Min >l′ stop Output deceleration signal in the case of In l′ stop >d Min >0, output brake signal; Among them, l′ stop is the real-time braking distance, l′ dec is the real-time deceleration distance; The industrial vehicle performs obstacle avoidance control based on the safety signal, the deceleration signal and the brake signal.

7. An industrial vehicle warning and obstacle avoidance system based on a binocular camera, characterized in that: include: An on-board controller, a CAN communication module, an industrial computer, a binocular depth camera, an on-board power supply and an on-board display are used to execute the industrial vehicle early warning and obstacle avoidance method based on a binocular camera as described in any one of claims 1 to 6.

Images