Semitrailer hinge angle measuring method and device based on multi-sensor fusion

Abstract

The invention provides a semitrailer hinge angle measuring method and device based on multi-sensor fusion, and the method comprises the steps: obtaining an absolute value of a difference value between a second distance and a third distance, obtaining a compensation distance difference of two ultrasonic sensors, and determining a distance residual error based on the compensation distance difference of the two ultrasonic sensors and a hinge angle formula; adding the distance between the projection of the three-dimensional feature points in the world coordinate system on the plane where the left image is located and the corresponding feature points in the left image and the distance between the projection of the three-dimensional feature points in the world coordinate system on the plane where the right image is located and the corresponding feature points in the right image to obtain a re-projection residual error; and determining a total residual error according to the distance residual error and the reprojection residual error, constructing a least square optimization objective function based on the total residual error, and carrying out iterative solution by adopting a Levenberg-Marquardt algorithm. The invention provides a semitrailer hinge angle measuring method and device based on multi-sensor fusion. The semitrailer hinge angle measuring method and device are used for solving the problems that a single sensor scheme is sensitive to the environment and poor in stability, and a contact sensor is large in installation difficulty.

Description

Technical Field

[0001] The present invention relates to the field of vehicle control, and in particular to a method and device for measuring the articulation angle of a semitrailer based on multi-sensor fusion. Background Art

[0002] Currently, in the field of semi-trailer articulation angle measurement, two main measurement methods are used: one based on a single sensor and the other based on a kinematic model.

[0003] In the measurement method based on a single sensor, commonly used sensors include angle sensors, lidar, cameras (visual sensors) and ultrasonic sensors. Contact sensors (such as angle sensors) are difficult to install, while non-contact sensors are easily interfered by environmental factors. For example, ultrasonic sensors may be affected by noise, cameras are unstable when lighting conditions change, and lidar sensors have reduced accuracy in harsh climates (such as rain and snow). In addition, due to the lack of redundant design, single sensor systems cannot continue to provide valid data when failures occur, resulting in poor system stability.

[0004] The measurement method based on the kinematic model relies on the kinematic model to estimate the articulation angle, and its accuracy depends on the parameters of the model. Therefore, when the model parameters are not accurate enough or the external environment is complex, errors are prone to occur, affecting the measurement effect. Summary of the invention

[0005] The present invention provides a method and device for measuring the articulation angle of a semitrailer based on multi-sensor fusion, which are used to solve the problems of environmental sensitivity, poor stability and difficulty in installing contact sensors in a single sensor solution.

[0006] In a first aspect, the present invention provides a method for measuring the articulation angle of a semitrailer based on multi-sensor fusion, comprising:

[0007] Preprocessing the left image and the right image captured by the binocular camera, and calculating the first distance according to the preprocessed image information, the binocular camera is installed on the tractor, and the first distance is the measurement distance from the binocular camera to the front end of the semi-trailer;

[0008] The first distance is compensated according to the distance compensation formulas of the first ultrasonic sensor and the second ultrasonic sensor respectively to obtain a second distance and a third distance, the second distance is the compensation distance from the first ultrasonic sensor to the front end of the semi-trailer, and the third distance is the compensation distance from the second ultrasonic sensor to the front end of the semi-trailer, and the two ultrasonic sensors are respectively installed on both sides of the binocular camera and are in the same straight line with the binocular camera;

[0009] Taking an absolute value of a difference between the second distance and the third distance to obtain a compensation distance difference between the two ultrasonic sensors, and determining a distance residual based on the compensation distance difference between the two ultrasonic sensors and an articulation angle formula;

[0010] Add the distance between the projection of the three-dimensional feature point on the plane where the left image is located and the corresponding feature point in the left image and the distance between the projection of the three-dimensional feature point on the plane where the right image is located and the corresponding feature point in the right image in the world coordinate system to obtain a reprojection residual;

[0011] The overall residual is determined according to the distance residual and the reprojection residual, a least squares optimization objective function is constructed based on the overall residual, and a Levenberg-Marquardt algorithm is used to iteratively solve the problem to obtain a measured value of the articulation angle.

[0012] Optionally, the preprocessing includes binocular alignment, denoising, graying, ORB feature extraction and feature matching.

[0013] Optionally, the distance compensation formula is obtained by the following steps:

[0014] When the articulation angle is 0°, a preset number of fourth distances are collected by the ultrasonic sensor, and the preset number of left images and right images are collected by the binocular camera at the same time, the fourth distance being the measurement distance from the ultrasonic sensor to the front end of the semi-trailer;

[0015] Based on a mean filtering algorithm, smoothing is performed on the preset number of fourth distances to obtain a preset number of fifth distances;

[0016] Performing the preprocessing on the preset number of left images and right images, and calculating a preset number of first distances based on the preset number of preprocessed image information;

[0017] The average proportional error is obtained according to the following formula:

[0018]

[0019] Wherein, k is the average proportional error, N is the preset number, and d v，i is the i-th first distance, d u，i is the ith fifth distance;

[0020] The following distance compensation formula is determined based on the average proportional error:

[0021] d′ u =kd v

[0022] Among them, d′ u is the compensation distance from the ultrasonic sensor to the front end of the semi-trailer, d vis the first distance.

[0023] Optionally, the determining of the distance residual based on the compensation distance difference of the two ultrasonic sensors and the articulation angle formula includes:

[0024] The distance residual is determined according to the following formula:

[0025] r ultra (θ) = Δd compensated -Δd

[0026] Among them, r ultra (θ) is the distance residual, θ is the articulation angle, Δd compensated is the compensation distance difference of the two ultrasonic sensors, Δd is the theoretical distance difference of the two ultrasonic sensors, and the theoretical distance difference Δd of the two ultrasonic sensors is determined according to the articulation angle formula, and the articulation angle formula is as follows:

[0027]

[0028] Wherein, L is the placement distance between the two ultrasonic sensors.

[0029] Optionally, the adding of the distance between the projection of the three-dimensional feature point in the world coordinate system on the plane where the left image is located and the corresponding feature point in the left image and the distance between the projection of the three-dimensional feature point on the plane where the right image is located and the corresponding feature point in the right image to obtain the reprojection residual includes:

[0030] The coordinates of the three-dimensional feature points in the world coordinate system are transformed into the left camera coordinate system according to the following formula:

[0031] P cL =RR θ P+T L

[0032] Among them, P cL is the coordinate of the 3D feature point in the left camera coordinate system, R is the rotation matrix of the binocular camera, P is the coordinate of the 3D feature point in the world coordinate system, T L is the translation vector of the left camera, R θ The rotation matrix introduced for the articulation angle is:

[0033]

[0034] The three-dimensional feature points in the left camera coordinate system are projected onto the plane where the left image is located according to the following formula:

[0035] P 2DL =K L P cL

[0036] Among them, P 2DLis the projection coordinate of the three-dimensional feature point on the left image plane in the world coordinate system, K L is the intrinsic and extrinsic parameter matrix of the left camera:

[0037]

[0038] Among them, f xL and f yL is the focal length of the left camera, c xL and c yL is the principal point coordinate of the left camera;

[0039] The coordinates of the three-dimensional feature points in the world coordinate system are transformed into the right camera coordinate system according to the following formula:

[0040] P cR =RR θ P+T R

[0041] Among them, P cR is the coordinate of the 3D feature point in the right camera coordinate system, T R is the translation vector of the right camera;

[0042] The three-dimensional feature points in the right camera coordinate system are projected onto the plane where the right image is located according to the following formula:

[0043] P 2DR =K R P cR

[0044] Among them, P 2DR is the projection coordinate of the three-dimensional feature point on the right image plane in the world coordinate system, K R is the intrinsic and extrinsic parameter matrix of the right camera:

[0045]

[0046] Among them, f xR and f yR is the focal length of the right camera, c xR and c yR is the principal point coordinate of the right camera;

[0047] The reprojection residual is determined according to the following formula:

[0048] r reproj (θ)=||P 2DL -P L ||+||P 2DR -P R ||

[0049] Among them, r reproj (θ) is the reprojection residual, P Lis the coordinate of the corresponding feature point of the three-dimensional feature point in the left image in the world coordinate system, P R is the coordinate of the feature point corresponding to the three-dimensional feature point in the right image in the world coordinate system.

[0050] Optionally, determining the overall residual according to the distance residual and the reprojection residual includes:

[0051] The population residual is determined according to the following formula:

[0052]

[0053] Among them, r(θ) is the overall residual.

[0054] Optionally, the least squares optimization objective function is constructed based on the overall residual, and the Levenberg-Marquardt algorithm is used to iteratively solve the obtained articulation angle measurement value, which includes:

[0055] The following least squares optimization objective function is constructed based on the overall residual:

[0056]

[0057] The Levenberg-Marquardt algorithm is used to determine the following iterative formula:

[0058] θ k+1 =θ k -(J T J+μI) -1 J T r(θ k )

[0059] Where J is the Jacobian matrix of the overall residual, μ is the adjustment parameter, and I is the identity matrix;

[0060] The initial value of the articulation angle is calculated based on the compensation distance difference of the two ultrasonic sensors and the articulation angle formula;

[0061] An iterative solution is performed based on the iterative formula and the initial value of the articulation angle, and the iteration is stopped when a termination condition is met to obtain the articulation angle measurement value, and the termination condition is:

[0062] ||r(θ k+1 )||<∈

[0063] Among them, ∈ is the convergence threshold.

[0064] In a second aspect, the present invention provides a semi-trailer articulation angle measurement device based on multi-sensor fusion, comprising a preprocessing module, a distance compensation module, a distance residual determination module, a reprojection residual determination module and a fusion optimization module, wherein:

[0065] The preprocessing module is used to preprocess the left image and the right image collected by the binocular camera, and calculate the first distance according to the preprocessed image information. The binocular camera is installed on the tractor, and the first distance is the measurement distance from the binocular camera to the front end of the semi-trailer;

[0066] The distance compensation module is used to perform distance compensation on the first distance according to the distance compensation formula of the first ultrasonic sensor and the second ultrasonic sensor, respectively, to obtain a second distance and a third distance, the second distance is the compensation distance from the first ultrasonic sensor to the front end of the semi-trailer, and the third distance is the compensation distance from the second ultrasonic sensor to the front end of the semi-trailer, and the two ultrasonic sensors are respectively installed on both sides of the binocular camera and are in the same straight line with the binocular camera;

[0067] The distance residual determination module is used to take an absolute value of a difference between the second distance and the third distance to obtain a compensation distance difference between two ultrasonic sensors, and determine a distance residual based on the compensation distance difference between the two ultrasonic sensors and an articulation angle formula;

[0068] The reprojection residual determination module is used to add the distance between the projection of the three-dimensional feature point on the plane where the left image is located and the corresponding feature point in the left image in the world coordinate system and the distance between the projection of the three-dimensional feature point on the plane where the right image is located and the corresponding feature point in the right image to obtain the reprojection residual;

[0069] The fusion optimization module is used to determine the overall residual according to the distance residual and the reprojection residual, construct a least squares optimization objective function based on the overall residual, and use the Levenberg-Marquardt algorithm to iteratively solve to obtain the articulation angle measurement value.

[0070] In a third aspect, the present invention provides a computing device, comprising:

[0071] A memory for storing program instructions;

[0072] The processor is used to call the program instructions stored in the memory and execute any of the above methods according to the obtained program.

[0073] In a fourth aspect, the present invention provides a computer-readable non-volatile storage medium, comprising computer-readable instructions, which enable a computer to execute any of the above methods when the computer reads and executes the computer-readable instructions.

[0074] The above scheme realizes data compensation and reduces observation errors by fusing two sensors, ultrasonic sensors and binocular cameras. It also significantly improves measurement accuracy by combining least squares optimization. The combination of ultrasonic sensors and binocular cameras can better adapt to complex environments such as noise, lighting changes and harsh climates, and has a wider range of applications. The Levenberg-Marquardt least squares optimization method is used to optimize algorithm efficiency, ensure computational efficiency in the multi-sensor data processing process, and make it more real-time. BRIEF DESCRIPTION OF THE DRAWINGS

[0075] In order to more clearly illustrate the technical solutions in the embodiments of the present invention, the drawings required for use in the description of the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0076] Figure 1 A schematic flow chart of a method for measuring the articulation angle of a semitrailer based on multi-sensor fusion provided in an embodiment of the present invention;

[0077] Figure 2 A schematic diagram of the optimal installation positions of the binocular camera and the ultrasonic sensor provided in an embodiment of the present invention;

[0078] Figure 3 A schematic diagram of geometric relationships provided by an embodiment of the present invention;

[0079] Figure 4 A schematic structural diagram of a semitrailer articulation angle measurement device based on multi-sensor fusion provided in an embodiment of the present invention. DETAILED DESCRIPTION

[0080] In order to make the purpose, technical scheme and advantages of the present invention clearer, the present invention will be further described in detail below with reference to the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, rather than all the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0081] Figure 1 The process of a method for measuring the articulation angle of a semitrailer based on multi-sensor fusion provided by an embodiment of the present invention is shown in detail, including:

[0082] S101, preprocessing a left image and a right image captured by a binocular camera, and calculating a first distance according to the preprocessed image information.

[0083] Specifically, a binocular camera is installed on the tractor.

[0084] The best position for installing the binocular camera is where its baseline is perpendicularly bisected by the center line of the semi-trailer, such as Figure 2 As shown, at this time, the calculation can be simplified and the accuracy can be improved.

[0085] In one example, preprocessing includes binocular alignment, denoising, grayscale, ORB feature extraction, and feature matching.

[0086] Binocular alignment is used to ensure that the same feature points in the left and right images are aligned in the row direction for subsequent processing.

[0087] Denoising is used to remove noise from images, improve image quality, and enhance the accuracy and robustness of feature extraction.

[0088] Grayscale is used to convert a color image into a single-channel grayscale image to reduce computational complexity.

[0089] ORB feature extraction includes using the FAST algorithm to detect feature points (corner points / key points) on the image, and applying the BRIEF descriptor generation method to each feature point to generate a binary descriptor to describe the local features of the feature point. After ORB feature extraction, the feature point coordinates and descriptors can be obtained.

[0090] Feature matching is used to compare the descriptors extracted from the left image and the right image, find matching feature points, and obtain the matching feature points and their coordinates in the left image and the right image.

[0091] Finally, the preprocessed image information includes the coordinates of the feature points detected and the matched feature points in the left and right images.

[0092] Specifically, the first distance is the measured distance from the binocular camera to the front end of the semi-trailer. The first distance is the distance between the point where the perpendicular line intersects the front end of the semi-trailer and the foot of the perpendicular line when the midpoint of the binocular camera baseline is used as the foot of the perpendicular line, and the front end of the semi-trailer is the end of the semi-trailer close to the articulation point.

[0093] According to the preprocessed image information, stereo matching and triangulation are further performed to obtain the first distance, wherein: stereo matching is used to calculate the disparity of the feature points to restore the depth of the three-dimensional feature points; triangulation is used to calculate the depth of the three-dimensional feature points and the coordinates of the three-dimensional feature points in the world coordinate system.

[0094] S102, performing distance compensation on the first distance according to distance compensation formulas of the first ultrasonic sensor and the second ultrasonic sensor respectively to obtain a second distance and a third distance.

[0095] Specifically, the second distance is the compensation distance from the first ultrasonic sensor to the front end of the semi-trailer, and the third distance is the compensation distance from the second ultrasonic sensor to the front end of the semi-trailer. The two ultrasonic sensors are respectively installed on both sides of the binocular camera and are in the same line with the binocular camera.

[0096] The best location for installing two ultrasonic sensors is where their connecting line is bisected vertically by the center line of the semi-trailer. Figure 2 shown.

[0097] In one example, the distance compensation formula is obtained by the following steps:

[0098] S201, when the articulation angle is 0°, a preset number of fourth distances are collected by the ultrasonic sensor, and a preset number of left images and right images are collected by the binocular camera at the same time, the fourth distance being the measured distance from the ultrasonic sensor to the front end of the semi-trailer.

[0099] In one example, acquiring a preset number of fourth distances and acquiring a preset number of left images and right images are performed based on a preset frequency.

[0100] The articulation angle is the angle formed by the center line of the tractor and the center line of the semi-trailer with the articulation point as the vertex.

[0101] When a perpendicular line is drawn to the line connecting the two ultrasonic sensors with one of the ultrasonic sensors as the foot of the perpendicular, the distance between the point where the perpendicular line intersects the front end of the semitrailer and the foot of the perpendicular is the measured distance from the ultrasonic sensor to the front end of the semitrailer.

[0102] S202, performing smoothing processing on a preset number of fourth distances based on a mean filtering algorithm to obtain a preset number of fifth distances.

[0103] Since the fourth distance may be deviated due to noise jitter, a preset number of fourth distances need to be smoothed based on a mean filtering algorithm.

[0104] Assume that the i-th fourth distance is d i (i=1,2,...,N), the window size is W, then the number of sampling points in the window is d i-W / 2 , ..., d i+W / 2 , after smoothing, the i-th fifth distance is:

[0105]

[0106] Among them, d u，i is the i-th fifth distance.

[0107] S203, preprocessing a preset number of left images and right images, and calculating a preset number of first distances based on a preset number of preprocessed image information.

[0108] S204, the average proportional error is obtained according to the following formula:

[0109]

[0110] Among them, k is the average proportional error, N is the preset number, and d v，i is the i-th first distance.

[0111] S205, determining the following distance compensation formula according to the average proportional error:

[0112] d′ u =kd v

[0113] Among them, d′ u is the compensation distance from the ultrasonic sensor to the front end of the semi-trailer, d v is the first distance.

[0114] It should be noted that the distance compensation formulas of the first ultrasonic sensor and the second ultrasonic sensor are universal, except that the k value is different.

[0115] S103, taking an absolute value of a difference between the second distance and the third distance to obtain a compensation distance difference between the two ultrasonic sensors, and determining a distance residual based on the compensation distance difference between the two ultrasonic sensors and an articulation angle formula.

[0116] In one example, determining the distance residual based on the compensated distance difference of two ultrasonic sensors and the articulation angle formula includes:

[0117] The distance residual is determined according to the following formula:

[0118] r ultra (θ) = Δd compensated -Δd

[0119] Among them, r ultra (θ) is the distance residual, which reflects the error of the compensation distance difference between the two ultrasonic sensors, θ is the articulation angle, Δd compensated is the compensation distance difference between the two ultrasonic sensors, Δd is the theoretical distance difference between the two ultrasonic sensors, and the theoretical distance difference Δd between the two ultrasonic sensors is determined according to the articulation angle formula. The articulation angle formula is as follows:

[0120]

[0121] Wherein, L is the placement distance between the two ultrasonic sensors.

[0122] It should be noted that the formula for the hinge angle is given by Figure 3 The geometric relationship shown is obtained.

[0123] S104, adding the distance between the projection of the three-dimensional feature point on the plane where the left image is located and the corresponding feature point in the left image and the distance between the projection of the three-dimensional feature point on the plane where the right image is located and the corresponding feature point in the right image in the world coordinate system to obtain a reprojection residual.

[0124] A 3D feature point is any significant 3D feature point, such as a corner point or an edge point.

[0125] Artificial signs (such as reflective strips, QR code signs, colored stickers, etc.) can be placed on the semi-trailer as 3D feature points. These sign points have high prominence and traceability and are suitable for long-term observation and tracking. If it is inconvenient to place artificial signs, natural feature points (such as bolt holes, structural edges, etc.) can also be extracted from the surface of the semi-trailer as 3D feature points.

[0126] In one example, the distance between the projection of the three-dimensional feature point in the plane where the left image is located and the corresponding feature point in the left image is added, and the distance between the projection of the three-dimensional feature point in the plane where the right image is located and the corresponding feature point in the right image is added, and the reprojection residual is obtained, including:

[0127] The coordinates of the three-dimensional feature points in the world coordinate system are transformed to the left camera coordinate system according to the following formula:

[0128] P cL =RR θ P+T L

[0129] Among them, P cL is the coordinate of the 3D feature point in the left camera coordinate system, R is the rotation matrix of the binocular camera, P is the coordinate of the 3D feature point in the world coordinate system, T L is the translation vector of the left camera, R θ The rotation matrix introduced for the articulation angle is:

[0130]

[0131] Project the 3D feature points in the left camera coordinate system to the plane where the left image is located according to the following formula:

[0132] P 2DL =K L P cL

[0133] Among them, P 2DL is the projection coordinate of the 3D feature point on the left image plane in the world coordinate system, K L is the intrinsic and extrinsic parameter matrix of the left camera:

[0134]

[0135] Among them, fxL and f yL is the focal length of the left camera, c xL and c yL is the principal point coordinate of the left camera;

[0136] The coordinates of the three-dimensional feature points in the world coordinate system are transformed to the right camera coordinate system according to the following formula:

[0137] P cR =RR θ P+T R

[0138] Among them, P cR is the coordinate of the 3D feature point in the right camera coordinate system, T R is the translation vector of the right camera;

[0139] Project the 3D feature points in the right camera coordinate system to the plane where the right image is located according to the following formula:

[0140] P 2DR =K R P cR

[0141] Among them, P 2DR is the projection coordinate of the 3D feature point on the right image plane in the world coordinate system, K R is the intrinsic and extrinsic parameter matrix of the right camera:

[0142]

[0143] Among them, f xR and f yR is the focal length of the right camera, c xR and c yR is the principal point coordinate of the right camera;

[0144] The reprojection residual is determined according to the following formula:

[0145] r reproj (θ)=||P 2DL -P L ||+||P 2DR -P R ||

[0146] Among them, r reproj (θ) is the reprojection residual, which reflects the error between the projection of the 3D feature point on the plane of the left image and the right image in the world coordinate system and the distance between the corresponding feature point in the left image and the right image. L is the coordinate of the corresponding feature point of the three-dimensional feature point in the left image in the world coordinate system, P R is the coordinate of the corresponding feature point of the three-dimensional feature point in the right image in the world coordinate system.

[0147] The rotation matrix of the binocular camera, the translation vector of the left camera and the translation vector of the right camera are obtained by stereo matching and pose estimation of the preprocessed image information, where pose estimation is to calculate the rotation and translation information of the camera by combining the three-dimensional feature points and the image feature points.

[0148] The rotation matrix introduced by the articulation angle is obtained by performing articulation angle estimation on the preprocessed image information. The articulation angle estimation is to estimate the camera rotation (articulation angle) through the change of feature points (such as position or parallax).

[0149] S105, determining the overall residual according to the distance residual and the reprojection residual, constructing a least squares optimization objective function based on the overall residual, and using the Levenberg-Marquardt algorithm to iteratively solve to obtain the articulation angle measurement value.

[0150] In one example, determining an overall residual based on the distance residual and the reprojection residual includes:

[0151] The population residual is determined according to the following formula:

[0152]

[0153] Among them, r(θ) is the overall residual.

[0154] In one example, a least square optimization objective function is constructed based on the overall residual, and the Levenberg-Marquardt algorithm is used to iteratively solve the obtained articulation angle measurement values, including:

[0155] The following least squares optimization objective function is constructed based on the overall residual, that is, minimizing the sum of squares of the overall residual is used as the objective function:

[0156]

[0157] The Levenberg-Marquardt algorithm is used to determine the following iterative formula:

[0158] θ k+1 =θ k -(J T J+μI) -1 J T r(θ k )

[0159] Among them, J is the Jacobian matrix of the overall residual, that is, the partial derivative of the overall residual with respect to the hinge angle θ, μ is a tuning parameter, which is a non-negative value used to control the optimization step size. When the overall residual decreases, μ is reduced (increasing the dependence on the second-order information). When the overall residual increases, μ is increased (more dependent on the first-order information). I is the unit matrix;

[0160] The initial value of the articulation angle is calculated based on the compensation distance difference of the two ultrasonic sensors and the articulation angle formula;

[0161] Iteration is performed based on the iterative formula and the initial value of the articulation angle. When the termination condition is met, the iteration is stopped to obtain the measured value of the articulation angle. The termination condition is:

[0162] ||r(θ k+1 )||<∈

[0163] Among them, ∈ is the convergence threshold, which can be set to ∈=1*10 -6 .

[0164] The above scheme realizes data compensation and reduces observation errors by fusing two sensors, ultrasonic sensors and binocular cameras. It also significantly improves measurement accuracy by combining least squares optimization. The combination of ultrasonic sensors and binocular cameras can better adapt to complex environments such as noise, lighting changes and harsh climates, and has a wider range of applications. The Levenberg-Marquardt least squares optimization method is used to optimize algorithm efficiency, ensure computational efficiency in the multi-sensor data processing process, and make it more real-time.

[0165] Based on the same inventive concept, Figure 4 The structure of a semitrailer articulation angle measurement device based on multi-sensor fusion provided by an embodiment of the present invention is shown, including a preprocessing module, a distance compensation module, a distance residual determination module, a reprojection residual determination module and a fusion optimization module, wherein:

[0166] The preprocessing module is used to preprocess the left image and the right image collected by the binocular camera, and calculate the first distance according to the preprocessed image information. The binocular camera is installed on the tractor, and the first distance is the measured distance from the binocular camera to the front end of the semi-trailer;

[0167] The distance compensation module is used to perform distance compensation on the first distance according to the distance compensation formulas of the first ultrasonic sensor and the second ultrasonic sensor, respectively, to obtain the second distance and the third distance, the second distance is the compensation distance from the first ultrasonic sensor to the front end of the semi-trailer, the third distance is the compensation distance from the second ultrasonic sensor to the front end of the semi-trailer, and the two ultrasonic sensors are respectively installed on both sides of the binocular camera and are in the same straight line with the binocular camera;

[0168] The distance residual determination module is used to take an absolute value of the difference between the second distance and the third distance to obtain a compensation distance difference between the two ultrasonic sensors, and determine the distance residual based on the compensation distance difference between the two ultrasonic sensors and the articulation angle formula;

[0169] The reprojection residual determination module is used to add the distance between the projection of the three-dimensional feature point on the plane where the left image is located and the corresponding feature point in the left image and the distance between the projection of the three-dimensional feature point on the plane where the right image is located and the corresponding feature point in the right image in the world coordinate system to obtain the reprojection residual;

[0170] The fusion optimization module is used to determine the overall residual according to the distance residual and the reprojection residual, and to construct the least squares optimization objective function based on the overall residual. The Levenberg-Marquardt algorithm is used to iteratively solve the problem and obtain the articulation angle measurement value.

[0171] Based on the same inventive concept, an embodiment of the present invention provides a computing device, including:

[0172] A memory for storing program instructions;

[0173] The processor is used to call the program instructions stored in the memory and execute the above method according to the obtained program.

[0174] Based on the same inventive concept, an embodiment of the present invention provides a computer-readable non-volatile storage medium, including computer-readable instructions. When a computer reads and executes the computer-readable instructions, the computer executes the above method.

[0175] It will be appreciated by those skilled in the art that embodiments of the present invention may be provided as methods or computer program products. Therefore, the present invention may take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware. Moreover, the present invention may take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0176] The present invention is described with reference to flowcharts and / or block diagrams of methods, devices (systems), and computer program products according to embodiments of the present invention. It should be understood that each process and / or block in the flowchart and / or block diagram, as well as the combination of processes and / or blocks in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 A process or multiple processes and / or boxes Figure 1A device that provides the functions specified in a block or multiple blocks.

[0177] These computer program instructions may also be stored in a computer readable memory capable of directing a computer or other programmable data processing device to operate in a specific manner, so that the instructions stored in the computer readable memory produce an article of manufacture including an instruction device, which implements the process Figure 1 A process or multiple processes and / or boxes Figure 1 A function specified in one or more boxes.

[0178] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operating steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing instructions for implementing the process. Figure 1 A process or multiple processes and / or boxes Figure 1 The steps for the functions specified in one or more boxes.

[0179] Although the preferred embodiments of the present invention have been described, those skilled in the art may make additional changes and modifications to these embodiments once they have learned the basic creative concept. Therefore, the appended claims are intended to be interpreted as including the preferred embodiments and all changes and modifications that fall within the scope of the present invention.

[0180] Obviously, those skilled in the art can make various changes and modifications to the present application without departing from the spirit and scope of the present application. Thus, if these modifications and variations of the present application fall within the scope of the claims of the present application and their equivalents, the present application is also intended to include these modifications and variations.

Claims

1. A method for measuring the articulation angle of a semitrailer based on multi-sensor fusion, characterized in that: include: Preprocessing the left image and the right image captured by the binocular camera, and calculating the first distance according to the preprocessed image information, the binocular camera is installed on the tractor, and the first distance is the measurement distance from the binocular camera to the front end of the semi-trailer; The first distance is compensated according to the distance compensation formulas of the first ultrasonic sensor and the second ultrasonic sensor respectively to obtain a second distance and a third distance, the second distance is the compensation distance from the first ultrasonic sensor to the front end of the semi-trailer, and the third distance is the compensation distance from the second ultrasonic sensor to the front end of the semi-trailer, and the two ultrasonic sensors are respectively installed on both sides of the binocular camera and are in the same straight line with the binocular camera; Taking an absolute value of a difference between the second distance and the third distance to obtain a compensation distance difference between the two ultrasonic sensors, and determining a distance residual based on the compensation distance difference between the two ultrasonic sensors and an articulation angle formula; Add the distance between the projection of the three-dimensional feature point on the plane where the left image is located and the corresponding feature point in the left image and the distance between the projection of the three-dimensional feature point on the plane where the right image is located and the corresponding feature point in the right image in the world coordinate system to obtain a reprojection residual; The overall residual is determined according to the distance residual and the reprojection residual, a least squares optimization objective function is constructed based on the overall residual, and a Levenberg-Marquardt algorithm is used to iteratively solve the problem to obtain a measured value of the articulation angle.

2. The method according to claim 1, characterized in that: The preprocessing includes binocular alignment, denoising, graying, ORB feature extraction and feature matching.

3. The method according to claim 1, characterized in that: The distance compensation formula is obtained by the following steps: When the articulation angle is 0°, a preset number of fourth distances are collected by the ultrasonic sensor, and the preset number of left images and right images are collected by the binocular camera at the same time, the fourth distance being the measurement distance from the ultrasonic sensor to the front end of the semi-trailer; Based on a mean filtering algorithm, smoothing is performed on the preset number of fourth distances to obtain a preset number of fifth distances; Performing the preprocessing on the preset number of left images and right images, and calculating a preset number of first distances based on the preset number of preprocessed image information; The average proportional error is obtained according to the following formula: Wherein, k is the average proportional error, N is the preset number, and d v,i is the i-th first distance, d u,i is the ith fifth distance; The following distance compensation formula is determined based on the average proportional error: d′ u =kd v Among them, d′ u is the compensation distance from the ultrasonic sensor to the front end of the semi-trailer, d v is the first distance.

4. The method according to claim 1, characterized in that: The method for determining the distance residual based on the compensation distance difference of the two ultrasonic sensors and the articulation angle formula includes: The distance residual is determined according to the following formula: r ultra (θ)=Δd compensatred -Δd Among them, r ultra (θ) is the distance residual, θ is the articulation angle, Δd compensated is the compensation distance difference of the two ultrasonic sensors, Δd is the theoretical distance difference of the two ultrasonic sensors, and the theoretical distance difference Δd of the two ultrasonic sensors is determined according to the articulation angle formula, and the articulation angle formula is as follows: Wherein, L is the placement distance between the two ultrasonic sensors.

5. The method according to claim 4, characterized in that: The step of adding the distance between the projection of the three-dimensional feature point in the world coordinate system on the plane where the left image is located and the corresponding feature point in the left image and the distance between the projection of the three-dimensional feature point on the plane where the right image is located and the corresponding feature point in the right image to obtain the reprojection residual comprises: The coordinates of the three-dimensional feature points in the world coordinate system are transformed into the left camera coordinate system according to the following formula: P cL =RR θ P+T L Among them, P cL is the coordinate of the 3D feature point in the left camera coordinate system, R is the rotation matrix of the binocular camera, P is the coordinate of the 3D feature point in the world coordinate system, T L is the translation vector of the left camera, R θ The rotation matrix introduced for the articulation angle is: The three-dimensional feature points in the left camera coordinate system are projected onto the plane where the left image is located according to the following formula: P 2DL =K L P cL Among them, P 2DL is the projection coordinate of the three-dimensional feature point on the left image plane in the world coordinate system, K L is the intrinsic and extrinsic parameter matrix of the left camera: Among them, f xL and f yL is the focal length of the left camera, c xL and c yL is the principal point coordinate of the left camera; The coordinates of the three-dimensional feature points in the world coordinate system are transformed into the right camera coordinate system according to the following formula: P cR =RR θ P+T R Among them, P cR is the coordinate of the 3D feature point in the right camera coordinate system, T R is the translation vector of the right camera; The three-dimensional feature points in the right camera coordinate system are projected onto the plane where the right image is located according to the following formula: P 2DR =K R P cR Among them, P 2DR is the projection coordinate of the three-dimensional feature point on the right image plane in the world coordinate system, K R is the internal and external parameter matrix of the right camera: Among them, f xR and f yR is the focal length of the right camera, c xR and c yR is the principal point coordinate of the right camera; The reprojection residual is determined according to the following formula: r reproj (θ)6||P 2DL -R L ||+||P 2DR -P R || Among them, r reproj (θ) is the reprojection residual, P L is the coordinate of the corresponding feature point of the three-dimensional feature point in the left image in the world coordinate system, P R is the coordinate of the feature point corresponding to the three-dimensional feature point in the right image in the world coordinate system.

6. The method according to claim 5, characterized in that: Determining the overall residual according to the distance residual and the reprojection residual comprises: The population residual is determined according to the following formula: Among them, r(θ) is the overall residual.

7. The method according to claim 6, characterized in that: The least square optimization objective function is constructed based on the overall residual, and the Levenberg-Marquardt algorithm is used to iteratively solve the obtained articulation angle measurement value, including: The following least squares optimization objective function is constructed based on the overall residual: The Levenberg-Marquardt algorithm is used to determine the following iterative formula: i k+1 =θ k -(J T J+μI) -1 J T r(θ k ) Where J is the Jacobian matrix of the overall residual, μ is the adjustment parameter, and I is the identity matrix; The initial value of the articulation angle is calculated based on the compensation distance difference of the two ultrasonic sensors and the articulation angle formula; An iterative solution is performed based on the iterative formula and the initial value of the articulation angle, and the iteration is stopped when a termination condition is met to obtain the articulation angle measurement value, and the termination condition is: ||r(θ k+1 )||<∈ Among them, ∈ is the convergence threshold.

8. A semi-trailer articulation angle measurement device based on multi-sensor fusion, characterized in that: It includes a preprocessing module, a distance compensation module, a distance residual determination module, a reprojection residual determination module and a fusion optimization module, wherein: The preprocessing module is used to preprocess the left image and the right image collected by the binocular camera, and calculate the first distance according to the preprocessed image information. The binocular camera is installed on the tractor, and the first distance is the measurement distance from the binocular camera to the front end of the semi-trailer; The distance compensation module is used to perform distance compensation on the first distance according to the distance compensation formula of the first ultrasonic sensor and the second ultrasonic sensor, respectively, to obtain a second distance and a third distance, the second distance is the compensation distance from the first ultrasonic sensor to the front end of the semi-trailer, and the third distance is the compensation distance from the second ultrasonic sensor to the front end of the semi-trailer, and the two ultrasonic sensors are respectively installed on both sides of the binocular camera and are in the same straight line with the binocular camera; The distance residual determination module is used to take an absolute value of a difference between the second distance and the third distance to obtain a compensation distance difference between two ultrasonic sensors, and determine a distance residual based on the compensation distance difference between the two ultrasonic sensors and an articulation angle formula; The reprojection residual determination module is used to add the distance between the projection of the three-dimensional feature point on the plane where the left image is located and the corresponding feature point in the left image in the world coordinate system and the distance between the projection of the three-dimensional feature point on the plane where the right image is located and the corresponding feature point in the right image to obtain the reprojection residual; The fusion optimization module is used to determine the overall residual according to the distance residual and the reprojection residual, construct a least squares optimization objective function based on the overall residual, and use the Levenberg-Marquardt algorithm to iteratively solve to obtain the articulation angle measurement value.

9. A computing device, characterized in that include: A memory for storing program instructions; A processor is used to call the program instructions stored in the memory and execute the method according to any one of claims 1 to 7 according to the obtained program.

10. A computer-readable non-volatile storage medium, characterized in that: The method comprises computer-readable instructions, and when a computer reads and executes the computer-readable instructions, the computer executes the method according to any one of claims 1 to 7.

Images