A method and device for measuring the articulation angle of a semi-trailer based on multi-sensor fusion

Abstract

This invention provides a method and apparatus for measuring the articulation angle of a semi-trailer based on multi-sensor fusion, comprising: taking the absolute value of the difference between a second distance and a third distance to obtain the compensated distance difference between two ultrasonic sensors; determining the distance residual based on the compensated distance difference between the two ultrasonic sensors and the articulation angle formula; adding the distance between the projection of a 3D feature point in the world coordinate system onto the plane of the left image and its corresponding feature point in the left image, and the distance between the projection of the feature point onto the plane of the right image and its corresponding feature point in the right image, to obtain the reprojection residual; determining the overall residual based on the distance residual and the reprojection residual; constructing a least-squares optimization objective function based on the overall residual, and solving iteratively using the Levenberg-Marquardt algorithm. This invention provides a method and apparatus for measuring the articulation angle of a semi-trailer based on multi-sensor fusion to solve the problems of environmental sensitivity, poor stability, and difficult installation of contact sensors in single-sensor solutions.

Description

Technical Field

[0001] This invention relates to the field of vehicle control, and in particular to a method and apparatus for measuring the articulation angle of a semi-trailer based on multi-sensor fusion. Background Technology

[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 single-sensor-based measurement methods, commonly used sensors include angle sensors, LiDAR, cameras (vision sensors), and ultrasonic sensors. Contact sensors (such as angle sensors) are more difficult to install; while non-contact sensors are easily affected by environmental factors. For example, ultrasonic sensors may be affected by noise, cameras perform unpredictably under changing lighting conditions, and LiDAR sensors experience accuracy degradation in harsh weather conditions (such as rain and snow). Furthermore, single-sensor systems lack redundancy and cannot continue to provide valid data in the event of a failure, resulting in poor system stability.

[0004] Measurement methods based on kinematic models rely on the kinematic model to estimate the hinge angle. The accuracy depends on the parameters of the model. Therefore, errors are prone to occur when the model parameters are not accurate enough or the external environment is complex, which affects the measurement results. Summary of the Invention

[0005] This invention provides a method and device for measuring the articulation angle of a semi-trailer based on multi-sensor fusion, in order to solve the problems of environmental sensitivity, poor stability and difficulty in installing contact sensors in single-sensor solutions.

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

[0007] The left and right images acquired by the binocular camera are preprocessed, and the first distance is calculated based on 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 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 the second distance and the third distance. The second distance is the compensated distance from the first ultrasonic sensor to the front end of the semi-trailer, and the third distance is the compensated 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 on the same straight line as the binocular camera.

[0009] The absolute value of the difference between the second distance and the third distance is taken to obtain the compensation distance difference between the two ultrasonic sensors. The distance residual is determined based on the compensation distance difference between the two ultrasonic sensors and the hinge angle formula.

[0010] The reprojection residual is obtained by adding the distance between the projection of the 3D feature point in the world coordinate system onto the plane of the left image and its corresponding feature point in the left image and the distance between the projection of the 3D feature point onto the plane of the right image and its corresponding feature point in the right image.

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

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

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

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

[0015] Based on the mean filtering algorithm, the preset number of fourth distances are smoothed to obtain the preset number of fifth distances;

[0016] The preprocessing is performed on the preset number of left and right images, and the preset number of first distances are calculated based on the preset number of preprocessed image information;

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

[0018]

[0019] Where k is the average proportional error, N is the preset quantity, and d v，i For the i-th first distance, d u，i The distance to the i-th fifth term;

[0020] Based on the average proportional error, the following distance compensation formula is determined:

[0021] d′ u =kd v

[0022] Where, d′ u d is the compensation distance from the ultrasonic sensor to the front of the semi-trailer. vThis is the first distance.

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

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

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

[0026] Where, r ultra (θ) represents the distance residual, θ is the hinge angle, and Δd compensated Let Δd be the compensation distance difference between the two ultrasonic sensors, and Δd be the theoretical distance difference between the two ultrasonic sensors. The theoretical distance difference Δd between the two ultrasonic sensors is determined according to the hinge angle formula, which is as follows:

[0027]

[0028] Where L is the distance between the two ultrasonic sensors.

[0029] Optionally, the step of adding the distance between the projection of the 3D feature point in the world coordinate system onto the plane of the left image and its corresponding feature point in the left image, and the distance between the projection of the feature point onto the plane of the right image and its corresponding feature point in the right image, to obtain the reprojection residual includes:

[0030] The coordinates of the 3D feature points in the world coordinate system are transformed to the left camera coordinate system according to the following formula:

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

[0032] Among them, P cL Let R be the coordinates of the 3D feature point in the left camera coordinate system, R be the rotation matrix of the stereo camera, P be the coordinates of the 3D feature point in the world coordinate system, and T be the coordinates of the 3D feature point in the world coordinate system. L Let R be the translation vector of the left camera. θ Rotation matrix introduced for hinge angle:

[0033]

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

[0035] P 2DL =K L P cL

[0036] Among them, P 2DLK represents the projected coordinates of the 3D feature points in the world coordinate system onto the left image plane. L Here is the intrinsic and extrinsic parameter matrix for the left camera:

[0037]

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

[0039] The coordinates of the 3D feature points in the world coordinate system are transformed to the right camera coordinate system according to the following formula:

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

[0041] Among them, P cR T represents the coordinates of the 3D feature points in the right camera coordinate system. R Let be the translation vector of the right camera;

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

[0043] P 2DR =K R P cR

[0044] Among them, P 2DR K represents the projected coordinates of the 3D feature points in the world coordinate system onto the right image plane. R Here is the intrinsic and extrinsic parameter matrix for the right camera:

[0045]

[0046] Among them, f xR and f yR c is the focal length of the right camera. xR and c yR The coordinates of the principal point 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] Where, r reproj (θ) represents the reprojection residual, P LP represents the coordinates of the corresponding feature point in the left image for the 3D feature point in the world coordinate system. R The coordinates of the three-dimensional feature points in the world coordinate system are the coordinates of the corresponding feature points in the right image.

[0050] Optionally, determining the overall residual based on the distance residual and the reprojection residual includes:

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

[0052]

[0053] Where r(θ) is the total residual.

[0054] Optionally, the step of constructing a least-squares optimization objective function based on the overall residuals and iteratively solving it using the Levenberg-Marquardt algorithm to obtain the hinge angle measurement includes:

[0055] Based on the overall residual, the following least-squares optimization objective function is constructed:

[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 total residuals, μ is the adjustment parameter, and I is the identity matrix;

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

[0061] The iteration is performed based on the given formula and the initial value of the hinge angle. The iteration stops when the termination condition is met, and the measured value of the hinge angle is obtained. The termination condition is:

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

[0063] Where ∈ is the convergence threshold.

[0064] Secondly, 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 and right images acquired by the binocular camera and calculate the first distance based on 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 of the semi-trailer.

[0066] The distance compensation module is used to compensate 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 compensated distance from the first ultrasonic sensor to the front end of the semi-trailer, and the third distance is the compensated 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 on the same straight line as the binocular camera.

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

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

[0069] The fusion optimization module is used to determine the overall residual based on 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 the problem to obtain the hinge angle measurement value.

[0070] Thirdly, the present invention provides a computing device, comprising:

[0071] Memory, used to store program instructions;

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

[0073] Fourthly, the present invention provides a computer-readable non-volatile storage medium including computer-readable instructions that, when read and executed by a computer, cause the computer to perform any of the methods described above.

[0074] The above scheme achieves data compensation and reduces observation errors by integrating ultrasonic sensors and binocular cameras. Combined with least squares optimization, it significantly improves measurement accuracy. The combination of ultrasonic sensors and binocular cameras can better adapt to complex environments such as noise, light changes and harsh weather, and has a wider range of applications. The Levenberg-Marquardt least squares optimization method is used to optimize algorithm efficiency, ensuring computational efficiency in the multi-sensor data processing process and providing stronger real-time performance. Attached Figure Description

[0075] To more clearly illustrate the technical solutions in the embodiments of the present invention, the accompanying drawings used in the description of the embodiments will be briefly introduced below. Obviously, the accompanying drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0076] Figure 1 A flowchart illustrating a method for measuring the articulation angle of a semi-trailer based on multi-sensor fusion, provided in an embodiment of the present invention;

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

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

[0079] Figure 4 This is a schematic diagram of a semi-trailer articulation angle measuring device based on multi-sensor fusion, provided in an embodiment of the present invention. Detailed Implementation

[0080] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings. Obviously, the described embodiments are only a part of the embodiments of this invention, and not all of them. Based on the embodiments of this invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this invention.

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

[0082] S101, preprocess the left and right images acquired by the binocular camera, and calculate the first distance based on the preprocessed image information.

[0083] Specifically, the binocular camera is mounted on the tractor unit.

[0084] The optimal mounting position for the binocular camera is where its baseline is perpendicularly bisected by the centerline of the semi-trailer, such as... Figure 2 As shown, this simplifies the calculation and improves accuracy.

[0085] In one example, preprocessing includes binocular alignment, denoising, grayscale conversion, 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 conversion is used to convert a color image into a single-channel grayscale image to reduce computational complexity.

[0089] ORB feature extraction involves using the FAST algorithm to detect feature points (corners / keypoints) on the image and applying the BRIEF descriptor generation method to each feature point to generate a binary descriptor that describes the local features of that feature point. After ORB feature extraction, the coordinates and descriptors of the feature points are obtained.

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

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

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

[0093] Based on the preprocessed image information, further stereo matching and triangulation are performed to obtain the first distance. Specifically, stereo matching is used to calculate the disparity of feature points and to recover the depth of 3D feature points; triangulation is used to calculate the depth of 3D feature points and the coordinates of 3D feature points in the world coordinate system.

[0094] S102, the first distance is compensated 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.

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

[0096] The optimal installation position for the two ultrasonic sensors is where the line connecting them is perpendicularly bisected by the centerline of the semi-trailer. Figure 2 As shown.

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

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

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

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

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

[0102] S202, based on the mean filtering algorithm, smooth the preset number of fourth distances to obtain the preset number of fifth distances.

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

[0104] Assume the distance to the i-th fourth node is d. i (i = 1, 2, ..., N), with a window size of W, then the number of sampling points within the window is d. i-W / 2 , ..., d i+W / 2 After smoothing, the distance of the i-th fifth element is:

[0105]

[0106] Where, d u，i This is the distance to the i-th fifth distance.

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

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

[0109]

[0110] Where k is the average proportional error, N is the preset quantity, and d v，i Let be the first distance of the i-th element.

[0111] S205, the following distance compensation formula is determined based on the average proportional error:

[0112] d′ u =kd v

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

[0114] It should be noted that the distance compensation formula for the first and second ultrasonic sensors is the same, only the k value is different.

[0115] S103, take the absolute value of the difference between the second distance and the third distance to obtain the 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 hinge angle formula.

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

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

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

[0119] Where, r ultra (θ) represents the distance residual, which reflects the error in the compensation distance difference between the two ultrasonic sensors. θ is the hinge angle, and Δd compensated Let Δd be the compensation distance difference between the two ultrasonic sensors, and Δd be the theoretical distance difference between the two ultrasonic sensors. The theoretical distance difference Δd between the two ultrasonic sensors is determined according to the hinge angle formula, which is as follows:

[0120]

[0121] Where L is the distance between the two ultrasonic sensors.

[0122] It should be noted that the hinge angle formula is derived from... Figure 3 The geometric relationships shown are obtained.

[0123] S104. Add the distance between the projection of the 3D feature point in the world coordinate system onto the plane of the left image and its corresponding feature point in the left image, and the distance between the projection of the feature point onto the plane of the right image and its corresponding feature point in the right image, to obtain the reprojection residual.

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

[0125] Three-dimensional feature points can be created by manually placing markers (such as reflective strips, QR code signs, colored stickers, etc.) on the semi-trailer. These markers have high saliency and traceability, making them suitable for long-term observation and tracking. If it is inconvenient to place manual markers, natural feature points (such as bolt holes, structural edges, etc.) can be extracted from the surface of the semi-trailer as three-dimensional feature points.

[0126] In one example, the reprojection residual is obtained by adding the distance between the projection of the 3D feature point in the world coordinate system onto the plane of the left image and its corresponding feature point in the left image, and the distance between the projection of the feature point onto the plane of the right image and its corresponding feature point in the right image. This reprojection residual includes:

[0127] The coordinates of the 3D feature points in the world coordinate system are transformed to the left camera coordinate system using the following formula:

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

[0129] Among them, P cL Let R be the coordinates of the 3D feature point in the left camera coordinate system, R be the rotation matrix of the stereo camera, P be the coordinates of the 3D feature point in the world coordinate system, and T be the coordinates of the 3D feature point in the world coordinate system. L Let R be the translation vector of the left camera. θ Rotation matrix introduced for hinge angle:

[0130]

[0131] The following formula is used to project the 3D feature points in the left camera coordinate system onto the plane containing the left image:

[0132] P 2DL =K L P cL

[0133] Among them, P 2DL K represents the projected coordinates of the 3D feature points in the world coordinate system onto the left image plane. L Here is the intrinsic and extrinsic parameter matrix for the left camera:

[0134]

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

[0136] Transform the coordinates of the 3D feature points in the world coordinate system to the right camera coordinate system using the following formula:

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

[0138] Among them, P cR T represents the coordinates of the 3D feature points in the right camera coordinate system. R Let be the translation vector of the right camera;

[0139] The following formula is used to project the 3D feature points in the right camera coordinate system onto the plane containing the right image:

[0140] P 2DR =K R P cR

[0141] Among them, P 2DR K represents the projected coordinates of the 3D feature points in the world coordinate system onto the right image plane. R Here is the intrinsic and extrinsic parameter matrix for the right camera:

[0142]

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

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

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

[0146] Where, r reproj (θ) represents the reprojection residual, which reflects the error between the projection of a 3D feature point in the world coordinate system onto the plane containing the left and right images and its corresponding feature point in the left and right images, respectively. L Let P be the coordinates of the corresponding feature point in the left image for the 3D feature point in the world coordinate system. R The coordinates of the three-dimensional feature point in the world coordinate system are the coordinates of the corresponding feature point in the right image.

[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. Pose estimation is to calculate the rotation and translation information of the camera by combining 3D feature points and image feature points.

[0148] The rotation matrix introduced by the hinge angle is obtained by calculating the hinge angle from the preprocessed image information. The hinge angle calculation is to estimate the camera rotation (hinge angle) by the changes in feature points (such as position or parallax).

[0149] S105. Determine the overall residual based on the distance residual and the reprojection residual. Construct a least-squares optimization objective function based on the overall residual and solve iteratively using the Levenberg-Marquardt algorithm to obtain the hinge angle measurement value.

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

[0151] The total residual is determined using the following formula:

[0152]

[0153] Where r(θ) is the total residual.

[0154] In one example, a least-squares optimization objective function is constructed based on the overall residuals, and the Levenberg-Marquardt algorithm is used iteratively to solve the problem, yielding the hinge angle measurements, including:

[0155] Based on the total residuals, the following least-squares optimization objective function is constructed, which is to minimize the sum of squares of the total residuals:

[0156]

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

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

[0159] Where J is the Jacobian matrix of the total residuals, which is the partial derivative of the total residuals with respect to the hinge angle θ. μ is an adjustment parameter, a non-negative value, used to control the step size of the optimization. When the overall residual decreases, μ decreases (increasing the dependence on second-order information), and when the overall residual increases, μ increases (becoming more dependent on first-order information). I is the identity matrix.

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

[0161] The iterative solution is performed based on the iterative formula and the initial value of the hinge angle. The iteration stops when the termination condition is met, and the measured value of the hinge angle is obtained. The termination condition is:

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

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

[0164] The above scheme achieves data compensation and reduces observation errors by integrating ultrasonic sensors and binocular cameras. Combined with least squares optimization, it significantly improves measurement accuracy. The combination of ultrasonic sensors and binocular cameras can better adapt to complex environments such as noise, light changes and harsh weather, and has a wider range of applications. The Levenberg-Marquardt least squares optimization method is used to optimize algorithm efficiency, ensuring computational efficiency in the multi-sensor data processing process and providing stronger real-time performance.

[0165] Based on the same inventive concept Figure 4 This invention illustrates the structure of 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:

[0166] The preprocessing module is used to preprocess the left and right images acquired by the binocular camera and calculate the first distance based on 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 of the semi-trailer.

[0167] The distance compensation module is used to compensate for the first distance according to the distance compensation formula 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 compensated distance from the first ultrasonic sensor to the front of the semi-trailer, and the third distance is the compensated distance from the second ultrasonic sensor to the front of the semi-trailer. The two ultrasonic sensors are respectively installed on both sides of the binocular camera and are on the same straight line as the binocular camera.

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

[0169] The reprojection residual determination module is used to add the distance between the projection of the 3D feature point in the world coordinate system onto the plane of the left image and its corresponding feature point in the left image and the distance between the projection of the feature point onto the plane of the right image and its corresponding feature point in the right image to obtain the reprojection residual.

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

[0171] Based on the same inventive concept, embodiments of the present invention provide a computing device, including:

[0172] Memory, used to store program instructions;

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

[0174] Based on the same inventive concept, embodiments of the present invention provide a computer-readable non-volatile storage medium, including computer-readable instructions, which, when read and executed by a computer, cause the computer to perform the above-described method.

[0175] Those skilled in the art will understand that embodiments of the present invention can be provided as methods or computer program products. Therefore, the present invention can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present invention can take the form of a computer program product embodied 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] This invention is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the invention. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart illustrations and / or block diagrams. Figure 1 One or more processes and / or boxes Figure 1A device that provides the functions specified in one or more boxes.

[0177] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1 The function specified in one or more boxes.

[0178] These computer program instructions may also be loaded onto a computer or other programmable data processing equipment to cause a series of operational steps to be performed on the computer or other programmable equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable equipment for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.

[0179] Although preferred embodiments of the invention have been described, those skilled in the art, upon learning the basic inventive concept, can make other changes and modifications to these embodiments. Therefore, the appended claims are intended to be interpreted as including the preferred embodiments as well as all changes and modifications falling within the scope of the invention.

[0180] Obviously, those skilled in the art can make various modifications and variations to this application without departing from the spirit and scope of this application. Therefore, if such modifications and variations fall within the scope of the claims of this application and their equivalents, this application also intends to include such modifications and variations.

Claims

1. A method for measuring the articulation angle of a semi-trailer based on multi-sensor fusion, characterized in that, include: The left and right images acquired by the binocular camera are preprocessed, and the first distance is calculated based on 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 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 the second distance and the third distance. The second distance is the compensated distance from the first ultrasonic sensor to the front end of the semi-trailer, and the third distance is the compensated 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 on the same straight line as the binocular camera. The absolute value of the difference between the second distance and the third distance is taken to obtain the compensation distance difference between the two ultrasonic sensors. The distance residual is determined based on the compensation distance difference between the two ultrasonic sensors and the hinge angle formula. The reprojection residual is obtained by adding the distance between the projection of the 3D feature point in the world coordinate system onto the plane of the left image and its corresponding feature point in the left image and the distance between the projection of the 3D feature point onto the plane of the right image and its corresponding feature point in the right image. The overall residual is determined based on the distance residual and the reprojection residual. A least-squares optimization objective function is constructed based on the overall residual, and the Levenberg-Marquardt algorithm is used to iteratively solve the problem to obtain the hinge angle measurement value.

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

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

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

5. The method according to claim 4, characterized in that, The reprojection residual is obtained by adding the distance between the projection of the 3D feature point in the world coordinate system onto the plane of the left image and its corresponding feature point in the left image, and the distance between the projection of the 3D feature point onto the plane of the right image and its corresponding feature point in the right image. The coordinates of the 3D feature points in the world coordinate system are transformed to the left camera coordinate system according to the following formula: P cL =RR θ P+T L Among them, P cL Let R be the coordinates of the 3D feature point in the left camera coordinate system, R be the rotation matrix of the stereo camera, P be the coordinates of the 3D feature point in the world coordinate system, and T be the coordinates of the 3D feature point in the world coordinate system. L Let R be the translation vector of the left camera. θ Rotation matrix introduced for hinge angle: The three-dimensional feature points in the left camera coordinate system are projected onto the plane containing the left image according to the following formula: P 2DL =K L P cL Among them, P 2DL K represents the projected coordinates of the 3D feature points in the world coordinate system onto the left image plane. L Here is the intrinsic and extrinsic parameter matrix for the left camera: Among them, f xL and f yL c is the focal length of the left camera. xL and c yL The coordinates of the principal point of the left camera; The coordinates of the 3D feature points in the world coordinate system are transformed to the right camera coordinate system according to the following formula: P cR =RR θ P+T R Among them, P cR T represents the coordinates of the 3D feature points in the right camera coordinate system. R Let be the translation vector of the right camera; The three-dimensional feature points in the right camera coordinate system are projected onto the plane containing the right image according to the following formula: P 2DR =K R P cR Among them, P 2DR K represents the projected coordinates of the 3D feature points in the world coordinate system onto the right image plane. R Here is the intrinsic and extrinsic parameter matrix for the right camera: Among them, f xR and f yR c is the focal length of the right camera. xR and c yR The coordinates of the principal point 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 || Where, r reproj (θ) represents the reprojection residual, P L P represents the coordinates of the corresponding feature point in the left image for the 3D feature point in the world coordinate system. R The coordinates of the three-dimensional feature points in the world coordinate system are the coordinates of the corresponding feature points in the right image.

6. The method according to claim 5, characterized in that, The step of determining the overall residual based on the distance residual and the reprojection residual includes: The total residual is determined according to the following formula: Where r(θ) is the total residual.

7. The method according to claim 6, characterized in that, The least-squares optimization objective function is constructed based on the overall residual, and the Levenberg-Marquardt algorithm is used to iteratively solve it to obtain the hinge angle measurement values, including: Based on the overall residual, the following least-squares optimization objective function is constructed: 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 total residuals, μ is the adjustment parameter, and I is the identity matrix; The initial value of the hinge angle is calculated based on the compensation distance difference between the two ultrasonic sensors and the hinge angle formula. The iteration is performed based on the given formula and the initial value of the hinge angle. The iteration stops when the termination condition is met, and the measured value of the hinge angle is obtained. The termination condition is: ||r(θ k+1 )||<∈ Where ∈ is the convergence threshold.

8. A semi-trailer articulation angle measuring 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 and right images acquired by the binocular camera and calculate the first distance based on 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 of the semi-trailer. The distance compensation module is used to compensate 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 compensated distance from the first ultrasonic sensor to the front end of the semi-trailer, and the third distance is the compensated 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 on the same straight line as the binocular camera. The distance residual determination module is used to take the absolute value of the difference between the second distance and the third distance to obtain the compensation distance difference between the two ultrasonic sensors, and to determine the distance residual based on the compensation distance difference between the two ultrasonic sensors and the hinge angle formula. The reprojection residual determination module is used to add the distance between the projection of the three-dimensional feature point in the world coordinate system onto the plane of the left image and its corresponding feature point in the left image and the distance between the projection of the feature point onto the plane of the right image and its corresponding feature point in the right image to obtain the reprojection residual. The fusion optimization module is used to determine the overall residual based on 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 the problem to obtain the hinge angle measurement value.

9. A computing device, characterized in that, include: Memory, used to store program instructions; A processor is configured to invoke program instructions stored in the memory and execute the method as described in any one of claims 1-7 according to the obtained program.

10. A computer-readable non-volatile storage medium, characterized in that, It includes computer-readable instructions that, when read and executed by a computer, cause the computer to perform the method as described in any one of claims 1-7.

Images