A trailer pose detection method and device, a terminal device, and a storage medium

Abstract

The present application relates to the technical field of unmanned flow, and discloses a trailer pose detection method and device, terminal equipment and storage medium, wherein the method comprises: acquiring the real-time pose of a towing vehicle; acquiring the pose estimation value of a trailer, calculating the detection range of the trailer, and setting a stationary reference point; dynamically selecting a target radar, extracting point clouds and performing rasterization processing, generating trailer side point clouds and placing them in corresponding grids; traversing all point clouds to obtain a rasterized candidate point cloud set; scanning all rows to output a fitted grid set; screening out effective grids; after straight line fitting processing and multiple iterations, obtaining one side of the trailer, combining the physical size of the trailer and generating the overall pose of the trailer through operation conversion. The present application only relies on the radar installed on the towing vehicle, and does not need to install a radar on the trailer to realize the detection of the trailer pose, realizes the trailer compartment pose estimation through fine processing, and effectively realizes the safety obstacle avoidance function.

Description

Technical Field

[0001] This invention relates to the field of unmanned logistics technology, specifically a method, device, terminal equipment, and storage medium for detecting trailer position and posture. Background Technology

[0002] To improve the freight efficiency of unmanned delivery vehicles, a relatively effective method is to attach a trailer to the back of the vehicle, with the two connected by a pivot pin, thus combining the tractor and trailer into a trailer-trailer system. To accommodate the diversity of goods of different sizes and the variability of operations, in actual operation, only the tractor is typically equipped with relevant sensors, and the tractor provides traction power; the trailer retains only a simple mechanical transmission structure without any sensors. However, once in autonomous driving mode, the planning system needs to constantly monitor the trailer's real-time status to determine whether safe turns are possible and to make decisions regarding key parameters such as the planned turning radius. Since the sensors are only located on the tractor, the safe operation of unmanned delivery vehicles with trailer functionality relies on the tractor's sensors to effectively estimate the trailer's position and orientation.

[0003] The patent with publication number CN114661039A, entitled "Method for Determining the Position of a Logistics Vehicle and its Trailer, Pre-collision Detection, and Autonomous Driving," discloses a method for determining the position of a trailer applied in the logistics field. The main inventive point is that it uses a kinematic model of the logistics vehicle to estimate the trailer's position and trailer angle, then compares the actual position of the trailer with the estimated position, and uses the position error to correct the estimated trailer angle. Finally, it combines the actual position of the trailer, the trailer angle, and the geometric parameters of the trailer and each trailer to obtain the position of each trailer. However, the position obtained through kinematic estimation is only a result under ideal conditions. In actual operation, the movement of the trailer is affected by various physical factors, such as the trailer's tendency to lateralize its position during bumpy rides, which can easily lead to position estimation failure.

[0004] Patent CN116091606A, entitled "Truck Position Detection Method, Device, Electronic Equipment, and Storage Medium," discloses a scheme for estimating the position of a trailer using a detection method. In terms of scheme design, this invention first determines a feasible trailer detection range based on the trailer's position and the trailer's physical dimensions. Within this range, the trailer's angle is obtained by fitting a straight line along its side. Since the trailer's physical dimensions and the trailer's dimensions can be pre-measured manually, the starting points of the line segments can be extracted from the point cloud of successfully fitted lines, obtaining information on two corner points of the trailer. Then, based on the trailer's actual length and width, the other two corner points can be reconstructed. These two sets of corner points constitute the edge points used to describe the square trailer. The advantage of this scheme is that it can fully utilize the information collected by sensors to correct the actual trailer position. However, the success of this scheme heavily depends on the design of the detection bounding box (ROI) and the information of preset reference points during the line fitting process. These two parts are difficult to pre-calibrate during implementation. Therefore, this scheme suffers from insufficient versatility or difficulty in implementation. Summary of the Invention

[0005] To address the shortcomings of the existing technologies, this invention provides a trailer position detection method, device, terminal equipment, and storage medium, offering a versatile, easy-to-implement, and accurate trailer position detection function for unmanned trailer logistics delivery systems.

[0006] In a first aspect, embodiments of this application provide a trailer position and orientation detection method, wherein the trailer is articulated and mounted behind a tractor to form a trailer-trailer system, and the tractor is equipped with multiple radars; the trailer position and orientation detection method includes the following steps: Step 1: Obtain the real-time pose of the tractor, which includes at least position coordinates, velocity, acceleration, heading angle, angular velocity, front wheel steering angle, and angular velocity of the front wheel steering angle. Step 2: Obtain the pose estimate of the trailer, calculate the detection range of the trailer based on the physical dimensions and kinematic equations of the trailer, and set a stationary reference point with the trailer as the reference object based on the estimated center point position and estimated heading angle of the trailer. Step 3: Based on the detection range and the stationary reference point, dynamically select the target radar installed on the corresponding side of the tractor, extract the point cloud of the corresponding target radar and project it into the detection range for rasterization processing, generate the trailer side point cloud and put it into the corresponding grid. Step 4: Traverse all point clouds of the target radar to obtain a rasterized candidate point cloud set; scan all rows within the detection range, and after all scans are completed, extract the raster with the shortest distance to the stationary reference point from all rows and output the fitted raster as the fitted raster set for the trailer side. Step 5: Perform directional control on the candidate point cloud set and the fitted grid set in the two dimensions of point cloud sampling and grid sampling, respectively, and filter out the effective grids that make up the side of the trailer. Step 6: Perform straight line fitting on the selected effective grid, and obtain the starting point position and length of the line segment on one side of the trailer after multiple iterations; Step 7: Based on the physical dimensions of the trailer, restore the starting point position, actual center point position, and actual heading angle of the line segment on the other side of the trailer in sequence, and generate the trailer pose through calculation and conversion.

[0007] Secondly, embodiments of this application also provide a trailer position detection device, the device comprising: The tractor real-time pose acquisition unit is used to acquire the real-time pose of the tractor, which includes at least position coordinates, velocity, acceleration, heading angle, angular velocity, front wheel steering angle and angular velocity of the front wheel steering angle. The trailer pose estimation value acquisition unit is used to acquire the pose estimation value of the trailer, calculate the detection range of the trailer based on the physical dimensions and kinematic equations of the trailer, and set a stationary reference point with the trailer as the reference object based on the estimated center point position and estimated heading angle of the trailer. The rasterization processing unit is used to dynamically select the target radar installed on the corresponding side of the tractor according to the detection range and the stationary reference point, extract the point cloud of the corresponding target radar and project it into the detection range for rasterization processing, generate the trailer side point cloud and put it into the corresponding grid. The trailer side point cloud extraction unit is used to traverse all point clouds of the target radar to obtain a rasterized candidate point cloud set; scan all rows within the detection range, and after all scans are completed, extract the raster with the shortest distance to the stationary reference point from all rows and output the fitted raster as the fitted raster set for the trailer side. The sampling and filtering unit is used to control the direction of the candidate point cloud set and the fitted grid set in the two dimensions of point cloud sampling and grid sampling, respectively, and to filter out the effective grids that make up the side of the trailer. A straight line fitting unit is used to perform straight line fitting on the selected effective grid, and after multiple iterations, obtain the starting point position and length of a line segment on one side of the trailer. The trailer pose generation unit is used to combine the physical dimensions of the trailer to sequentially restore the starting point position, actual center point position, and actual heading angle of the line segment on the other side of the trailer, and generate the trailer pose through calculation and conversion.

[0008] Thirdly, embodiments of this application also provide a terminal device, the terminal device being equipped with: Memory, used to store programs; A processor is configured to execute the program stored in the memory, and when the program is executed, the processor is configured to perform some or all of the steps of the trailer pose detection method as described above.

[0009] Fourthly, embodiments of this application also provide a computer-readable storage medium storing computer-executable instructions; when executed by a processor, the computer-executable instructions are used to implement some or all of the steps of the trailer pose detection method described above.

[0010] The beneficial technical effects of this invention are as follows: The trailer pose detection method provided in this application relies solely on the radar installed on the tractor unit, eliminating the need to install radar on the trailer itself. Through refined processing, the detection range of the trailer is automatically calculated, and a stationary reference point is automatically set with the trailer as the reference object. The corresponding radar is dynamically selected, and the corresponding point cloud is extracted and rasterized. The side equations of the rasterized point cloud are well-processed, significantly improving robustness. Simultaneously, the detection results are kinematically controllable, meeting the navigation requirements for real-time pose and other basic information of the vehicle's components. This provides accurate trailer pose estimation for unmanned trailer logistics delivery systems, effectively achieving obstacle avoidance. Furthermore, the entire solution integrates the estimation and detection methods, exhibiting strong versatility, ease of implementation, and significant practical value.

[0011] It is understandable that the trailer position detection device, terminal equipment, and computer-readable storage medium provided in the second to fourth aspects also have the aforementioned beneficial effects, which will not be elaborated here. Attached Figure Description

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

[0013] Figure 1 This is a flowchart of the trailer position detection method described in this embodiment of the invention; Figure 2 This is a flowchart illustrating the process of obtaining the trailer's pose estimation value, detection range, and stationary reference point in an embodiment of the present invention. Figure 3 A schematic diagram of the kinematic model of a trailer; Figure 4 Figure a shows the setup of the stationary reference point when the trailer's heading angle points to the left of the tractor; Figure 4 b is a diagram showing the setting of the stationary reference point when the trailer's heading angle points to the right of the tractor; Figure 5 This is a flowchart of the point cloud extraction and rasterization process in an embodiment of the present invention; Figure 6 This is a flowchart of trailer side point cloud and grid extraction in an embodiment of the present invention; Figure 7 This is a flowchart illustrating the sequential recovery of the actual position and orientation of the trailer at each location in an embodiment of the present invention; Figure 8 This is a structural block diagram of the trailer position detection device described in an embodiment of the present invention; Figure 9 This is a structural block diagram of the terminal device described in this embodiment of the invention.

[0014] The realization of the objective, functional features and advantages of the present invention will be further explained in conjunction with the embodiments and with reference to the accompanying drawings. Detailed Implementation

[0015] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative effort are within the scope of protection of the present invention.

[0016] It should be noted that all directional indications (such as up, down, left, right, front, back, etc.) in the embodiments of the present invention are only used to explain the relative positional relationship and movement of each component in a certain specific posture (as shown in the figure). If the specific posture changes, the directional indication will also change accordingly.

[0017] Furthermore, the technical solutions of the various embodiments of the present invention can be combined with each other, but only if they are feasible for those skilled in the art. If the combination of technical solutions is contradictory or cannot be implemented, it should be considered that such combination of technical solutions does not exist and is not within the scope of protection claimed by the present invention.

[0018] To address the challenges posed by the unpredictable nature of vehicle movement and the complex lane connections at uncontrolled intersections, this embodiment develops a trailer pose detection method. This method is primarily used to predict the future driving trajectory of motor vehicles in different scenarios, assisting autonomous driving equipment in making comprehensive decisions. Unlike other solutions that use lane lines and trajectories on a line-by-line basis, this embodiment refines the topological structure of vehicle trajectories and lane line nodes, fully mining effective information for future trajectory prediction. This improves the accuracy of future trajectory prediction, enabling the entire prediction process to be effectively applied to the trajectory prediction of motor vehicles at uncontrolled intersections.

[0019] like Figure 1 As shown, a trailer pose detection method is provided, mainly applied to unmanned logistics trailer systems that include a tractor and a trailer. In this system, the tractor is primarily responsible for all environmental perception and task decision-making, and provides traction power support, while the trailer only requires a simple transmission structure. For this type of automated delivery device, to achieve safe driving, in addition to collision avoidance detection of itself, it is also necessary to simultaneously utilize the trailer's pose for collision avoidance detection, while providing technical support for the tractor's perception and decision-making. Therefore, it is particularly important to achieve safe, stable, and efficient pose detection of the trailer without changing the sensor layout of the tractor.

[0020] The trailer position detection method specifically includes the following steps 1 to 7: Step 1: Obtain the real-time pose of the tractor vehicle. Specifically, this is extracted from the tractor vehicle's onboard inertial navigation system. The real-time pose includes at least position coordinates, velocity, acceleration, heading angle, angular velocity, front wheel steering angle, and angular velocity of the front wheel steering angle.

[0021] Because the cargo loaded on trailers varies in shape, the trailer portion changes significantly in radar point cloud imaging, making it difficult to find a uniform shape for description. In addition, a trailer is a standard elongated quadrilateral structure, and when turning at intersections or experiencing side bumps, one side of the trailer will appear in the radar's lateral field of view. Therefore, how to effectively utilize the radar's lateral point cloud for trailer pose detection is the key to the formulation of this application's solution.

[0022] Step 2: Obtain the pose estimate of the trailer, calculate the detection range of the trailer based on the physical dimensions and kinematic equations of the trailer, and set a stationary reference point with the trailer as the reference object based on the estimated center point position and estimated heading angle of the trailer.

[0023] In unmanned logistics trailer systems, both the tractor and the trailer are standard rigid bodies, and their motion still follows the vehicle kinematics model. The difference lies in that the trailer's power originates from the tractor, thus requiring a breakdown of their motion analysis. For the tractor, its motion is constrained by the following kinematic equations: (1) In the formula, and for Position coordinates at that moment For speed, For acceleration, For heading angle, The size of the front wheel steering angle. Angular velocity, Let be the angular velocity of the front wheel's steering angle, and This refers to the wheelbase between the front and rear wheels.

[0024] The trailer, powered by the tractor unit, is a rigid body, and its motion is constrained by the following kinematic equations: (2) In the formula, The heading angle of the tractor unit. For the estimated heading angle of the trailer, Estimate the distance between the center point and the articulation point of the trailer. This is the distance between the center point of the tractor and the articulation point.

[0025] For analyzing probabilities, such as Figure 3 The diagram shows the kinematic model of the trailer, along with the physical meaning of the relevant symbols.

[0026] To estimate the trailer's state, the relevant states of the tractor are input into kinematic equations (1) and (2) to obtain the ideal trailer position and state. However, in real-world scenarios, due to various external uncertainties such as bumps and tire friction, the estimated trailer position obtained through kinematic estimation deviates significantly from the actual position. To eliminate estimation errors, this embodiment employs a kinematic scheme to obtain the trailer's detection range, such as... Figure 2 As shown, the details are as follows: Step 21: Estimate the estimated center point position of the trailer using kinematic equations (1) and (2) to obtain the estimated value; Step 22, Calculate the detection range of the trailer body: Based on the prior parameters such as the trailer's physical dimensions and kinematic equations, set slack variables. and ,in and These represent the search ranges added based on the estimated center point of the trailer, with the origin as the origin, and the trailer length and width, respectively, to obtain the search position of the trailer. A quadrilateral frame is used to describe the search position to obtain the detection range of the trailer. Since the scheme adopts the mode of estimation before setting, the designed quadrilateral detection frame is reasonable as a whole, and its shape is controllable and fits the actual shape of the trailer.

[0027] Step 23, calculate the stationary reference point with the trailer as the reference system: In the coordinate system of the center point of the tractor, if the estimated center point position of the trailer is in... Components less than This indicates that the estimated heading angle of the trailer points to the right of the tractor. In this case, the estimated center point of the trailer is used as the reference, and the stationary reference point is set two vehicle widths to the left of the estimated center point. If the estimated center point position of the trailer is... Component greater than This indicates that the trailer's heading angle is pointing to the left of the tractor. In this case, the estimated center point of the trailer is used as the reference, and the stationary reference point is set two vehicle widths to the right of the estimated center point. If the estimated center point of the trailer is set in the estimated result... The component is not greater than This indicates that the direction of the trailer's cab is basically the same as that of the tractor's cab. Therefore, there is no need to set a fixed reference point; the actual center point of the trailer and the positions of the four corner points of the quadrilateral frame can be directly reconstructed from the estimation results. Preset values ​​that are manually set in advance.

[0028] For the setting of a stationary reference point, the calculation process for its specific location is as follows: Assume the pose estimate of the center point position of the trailer is... When the trailer's heading angle points to the left of the tractor, such as Figure 4 As shown in a, the position of the stationary reference point is selected as follows:

[0029] When the estimated heading angle of the trailer points to the right of the tractor, such as Figure 4 As shown in b, the position of the stationary reference point is selected as follows:

[0030] in, , The width of a trailer.

[0031] Step 3: Based on the detection range and the stationary reference point, dynamically select the target radar installed on the corresponding side of the tractor, extract the point cloud of the corresponding target radar and project it into the detection range for rasterization processing, generate the trailer side point cloud and put it into the corresponding grid.

[0032] In one embodiment, the target radar installed on the corresponding side of the tractor is dynamically selected based on the detection range and the stationary reference point. Specifically, the approximate position and orientation of the trailer can be estimated from the kinematic equations. In the tractor's perception system, the lower left radar can only detect obstacles to the left rear, while the lower right radar can only detect obstacles to the right rear. Therefore, the radar measurement equipment is dynamically selected based on the actual situation to minimize the consumption of computing resources.

[0033] After identifying the target radar, the point cloud of the corresponding target radar is extracted and projected onto the pre-calculated detection range for rasterization processing to generate the trailer side point cloud, which is then placed into the corresponding grid. Figure 5 As shown, the specific method is as follows: Step 31: Set the raster resolution and minimum point cloud height. and the highest point cloud height In this embodiment, the grid resolution is set to 10cm*10cm. Step 32, when the point cloud height is between the lowest point cloud height and the highest point cloud height When the location is within the detection range of the trailer, it is highly likely to be a side point cloud of the trailer. In this case, it is identified as a side point cloud of the trailer and placed into the corresponding grid.

[0034] Step 4: Traverse all point clouds of the target radar to obtain a rasterized candidate point cloud set; scan all rows within the detection range, and after all scans are completed, extract the raster with the shortest distance to the stationary reference point from all rows and output the fitted raster as the fitted raster set for the trailer side.

[0035] The candidate point cloud could be the edge points extending from the cargo or the actual side of the trailer, and it contains many components, so point cloud selection is necessary. Based on the stationary reference point given in step 2, a point is randomly selected from the rasterized candidate point cloud, and its distance to the stationary reference point is calculated. This distance is then used as the simplified distance between the raster and the stationary reference point. For example... Figure 6 As shown, the specific calculation steps are as follows: Step 41: Scan all rows within the detection range. When the stationary reference point is located on the left side of the trailer, extract all point clouds in the first grid containing valid point clouds as the fitted grid output after scanning that row. When the stationary reference point is located on the right side of the trailer, extract all point clouds in the last grid containing valid point clouds as the fitted grid output after scanning that row.

[0036] Step 42: After all scans are completed, extract the raster that is closest to the stationary reference point from all row raster cells. Step 43: Output the fitted grid as the fitted grid set for the trailer side.

[0037] The advantage of using a grid instead of a single point cloud for computation is that it can effectively prevent the occurrence of singular points from causing great disturbance to the fitted edges.

[0038] The grid containing effective point clouds refers to the grid that is measured by the point cloud density index. Specifically, there must be at least 3 point clouds in a 10cm*10cm grid, and the number of point clouds is unlimited but not more than 10. If it exceeds 10, it may be a false alarm grid caused by water or car window glass.

[0039] Step 5: Perform directional control on the candidate point cloud set and the fitted raster set in the two dimensions of point cloud sampling and raster sampling, respectively, and filter out the effective raster that makes up the side of the trailer.

[0040] Step 6: Perform straight line fitting on the selected valid grid, and obtain the starting point position and length of a line segment on one side of the trailer after multiple iterations.

[0041] Specifically, the RANSAC algorithm is used to perform linear fitting on the selected valid grid cells. To achieve random sampling and avoid overfitting, the sampling process in this embodiment is as follows: first, a candidate point cloud is randomly determined to participate in this round of sampling; then, a point is randomly selected from this candidate point cloud to participate in the sampling. The remaining sample numbers are then randomly selected from all remaining candidate point clouds.

[0042] The advantage of adopting this principle is that it can effectively avoid fitting errors in terms of direction, and at the same time greatly reduce the number of point cloud fittings.

[0043] Step 7: Based on the physical dimensions of the trailer, restore the starting point position, actual center point position, and actual heading angle of the line segment on the other side of the trailer in sequence, and generate the trailer pose through calculation and conversion.

[0044] like Figure 7 As shown, the specific steps are as follows: Step 71: Based on the starting point and length of the line segment on one side of the trailer obtained in Step 6, determine two corner points of the quadrilateral frame of the trailer. The other two corner points are obtained by calculation and conversion based on the physical dimensions of the trailer (including length and width). Step 72: After restoring the information of the four corner points, the actual center point position and actual heading angle of the trailer are further restored through calculation and transformation, thereby generating the trailer pose, thus completing the trailer pose detection. Finally, based on the position of the trailer's center point and the positions of its four corner points, the results are output to the task planning system for collision avoidance detection or for high-level decision-making.

[0045] As a preferred technical solution, the position coordinates and heading angle are measured through the positioning function, and the speed, acceleration, angular velocity, front wheel steering angle and angular velocity of the front wheel steering angle are measured and extracted through an on-board six-degree-of-freedom IMU.

[0046] The trailer pose detection method provided in this embodiment relies solely on the radar installed on the tractor unit, eliminating the need to install radar on the trailer itself. Through refined processing, the detection range of the trailer is automatically calculated, and a stationary reference point is automatically set with the trailer as the reference object. The corresponding radar is dynamically selected, and the corresponding point cloud is extracted and rasterized. The side equations of the rasterized point cloud are well-processed, significantly improving robustness. Simultaneously, the detection results are kinematically controllable, meeting the navigation requirements for real-time pose and other basic information of the vehicle's components. This provides accurate trailer pose estimation for unmanned trailer logistics delivery systems, effectively achieving obstacle avoidance. Furthermore, the entire solution integrates the estimation and detection methods, demonstrating strong practical significance.

[0047] like Figure 8 As shown, corresponding to the trailer pose detection method described in the above embodiments, this application also provides a trailer pose detection device. The device includes a tractor real-time pose acquisition unit 201, a trailer pose estimation acquisition unit 202, a rasterization processing unit 203, a trailer side point cloud extraction unit 204, a sampling and filtering unit 205, a straight line fitting unit 206, and a trailer pose generation unit 207; wherein, The real-time pose acquisition unit 201 of the tractor is used to acquire the real-time pose and motion state description of the tractor. Specifically, it is extracted from the on-board inertial navigation system of the tractor. The real-time pose includes at least basic motion information such as position coordinates, velocity, acceleration, heading angle, angular velocity, front wheel steering angle, and angular velocity of the front wheel steering angle.

[0048] The trailer pose estimation unit 202 is used to acquire the pose estimation value of the trailer, calculate the detection range of the trailer based on the physical dimensions and kinematic equations of the trailer, set a stationary reference point with the trailer as the reference object based on the estimated center point position and estimated heading angle of the trailer; estimate the estimated center point position of the trailer from the kinematic equations of the tractor and the trailer respectively, generate the detection range of the trailer using information such as the physical dimensions of the trailer; and generate a stationary reference point with the trailer as the reference object for fitting the side based on the estimated center point position and estimated heading angle of the trailer.

[0049] The rasterization processing unit 203 is used to dynamically select the target radar installed on the corresponding side of the tractor according to the detection range and the stationary reference point, extract the point cloud of the corresponding target radar and project it into the detection range for rasterization processing, generate a stable trailer side point cloud for fitting the side and put it into the corresponding grid.

[0050] The trailer side point cloud extraction unit 204 is used to traverse all point clouds of the target radar to obtain a rasterized candidate point cloud set; scan all rows within the detection range, and after all scans are completed, the fitted raster output by all rows is used as the fitted raster set of the trailer side. The sampling and filtering unit 205 is used to perform directional control on the candidate point cloud set and the fitting grid set entering the fitting process in the two dimensions of point cloud sampling and grid sampling, respectively, and to filter out the effective grids that make up the side of the trailer. The line fitting unit 206 is used to perform line fitting on the selected effective grid using the RANSAC method, and after multiple iterations, obtain the starting point position and length of a line segment on one side of the trailer. The trailer pose generation unit 207 is used to combine the physical dimensions of the trailer, including the length, width and height of the trailer, and sequentially restore the starting point position of the line segment on the other side of the trailer, the actual center point position and the actual heading angle, and generate the trailer pose through calculation and conversion.

[0051] It should be noted that the information interaction and execution process between the above-mentioned units / modules are based on the same concept as the method embodiments of this application. For details on their specific functions and technical effects, please refer to the method embodiments section, and they will not be repeated here.

[0052] like Figure 9As shown, this embodiment discloses a terminal device, which includes a transmitter 302, a receiver 301, a memory 304, and a processor 303. The transmitter 302 transmits instructions and data, the receiver 301 receives instructions and data, the memory 304 stores computer execution instructions, and the processor 303 executes the computer execution instructions stored in the memory 304 to implement some or all of the steps of the aforementioned trailer pose detection method. Its specific implementation process is the same as the aforementioned trailer pose detection method.

[0053] It should be noted that the aforementioned memory 304 can be either independent or integrated with the processor 303. When the memory is configured independently, the terminal device also includes a bus for connecting the memory 304 and the processor 303.

[0054] This embodiment also discloses a computer-readable storage medium storing computer-executable instructions. When the processor executes the computer-executable instructions, it implements some or all of the steps of the above-described trailer pose detection method.

[0055] The above description is merely a preferred embodiment of the present invention and does not limit the patent scope of the present invention. Any equivalent structural transformations made using the contents of the present invention's specification and drawings under the inventive concept of the present invention, or direct / indirect applications in other related technical fields, are included within the patent protection scope of the present invention.

Claims

1. A method for detecting the position and orientation of a trailer, characterized in that, The trailer is articulated and mounted behind the tractor to form a trailer system. The tractor is equipped with multiple radars. The trailer position and attitude detection method includes the following steps: Step 1: Obtain the real-time pose of the tractor, which includes at least position coordinates, velocity, acceleration, heading angle, angular velocity, front wheel steering angle, and angular velocity of the front wheel steering angle. Step 2: Obtain the pose estimate of the trailer, calculate the detection range of the trailer based on the physical dimensions and kinematic equations of the trailer, and set a stationary reference point with the trailer as the reference object based on the estimated center point position and estimated heading angle of the trailer. Step 3: Based on the detection range and the stationary reference point, dynamically select the target radar installed on the corresponding side of the tractor, extract the point cloud of the corresponding target radar and project it into the detection range for rasterization processing, generate the trailer side point cloud and put it into the corresponding grid. Step 4: Traverse all point clouds of the target radar to obtain a rasterized candidate point cloud set; scan all rows within the detection range, and after all scans are completed, extract the raster with the shortest distance to the stationary reference point from all rows and output the fitted raster as the fitted raster set for the trailer side. Step 5: Perform directional control on the candidate point cloud set and the fitted grid set in the two dimensions of point cloud sampling and grid sampling, respectively, and filter out the effective grids that make up the side of the trailer. Step 6: Perform straight line fitting on the selected effective grid, and obtain the starting point position and length of a line segment on one side of the trailer after multiple iterations. Step 7: Based on the physical dimensions of the trailer, restore the starting point position, actual center point position, and actual heading angle of the line segment on the other side of the trailer in sequence, and generate the trailer pose through calculation and conversion.

2. The trailer position detection method according to claim 1, characterized in that, The motion of the tractor is constrained by the following kinematic equations: （1） In the formula, and for Position coordinates at that moment For speed, For acceleration, For heading angle, The size of the front wheel steering angle. Angular velocity, Let be the angular velocity of the front wheel's steering angle, and This refers to the wheelbase between the front and rear wheels; The motion of the trailer is constrained by the following kinematic equations: （2） In the formula, The heading angle of the tractor unit. For the estimated heading angle of the trailer, Estimate the distance between the center point and the articulation point of the trailer. This is the distance between the center point of the tractor and the articulation point.

3. The trailer position detection method according to claim 2, characterized in that, In step 2, obtaining the pose estimate of the trailer, calculating the detection range of the trailer based on its physical dimensions and kinematic equations, and setting a stationary reference point with the trailer as the reference frame based on the estimated center point position and estimated heading angle of the trailer, includes: Step 21: Estimate the estimated center point position of the trailer using kinematic equations (1) and (2) to obtain the estimated value; Step 22: Set slack variables based on the trailer's physical dimensions and kinematic equations. and ,in and These represent the search ranges added to the trailer's length and width, respectively, with the estimated center point of the trailer as the origin, to obtain the trailer's search position. A quadrilateral frame is used to describe the search position to obtain the trailer's detection range. Step 23, in the coordinate system of the center point position of the tractor, if the estimated center point position of the trailer is in... Components less than This indicates that the estimated heading angle of the trailer points to the right of the tractor. In this case, the estimated center point of the trailer is used as the reference point, and the stationary reference point is set two vehicle widths to the left of the estimated center point of the trailer. If the estimated center point position of the trailer is... Component greater than This indicates that the estimated heading angle of the trailer points to the left of the tractor. In this case, the estimated center point of the trailer is used as the reference, and the fixed reference point is set two vehicle widths to the right of the estimated center point. If the estimated center point of the trailer is set in the estimated result... The component is not greater than This indicates that the direction of the trailer's cab is basically the same as the direction of the tractor's cab, and there is no need to set a stationary reference point; among which, This is the default value.

4. The trailer position detection method according to claim 1, characterized in that, In step 3, the step of extracting the point cloud of the corresponding target radar and projecting it onto the detection range for rasterization processing to generate the trailer side point cloud and placing it into the corresponding grid includes: Step 31: Set the raster resolution and minimum point cloud height. and the highest point cloud height ; Step 32, when the point cloud height is between the lowest point cloud height and the highest point cloud height When the location is within the detection range of the trailer, it is determined to be a side point cloud of the trailer and placed in the corresponding grid.

5. The trailer position detection method according to claim 1, characterized in that, In step 4, scanning all rows within the detection range, and extracting the grid with the shortest distance to the stationary reference point from all rows after the scan is complete, and outputting the fitted grid as the fitted grid set for the trailer side, includes: Step 41: Scan all rows within the detection range. When the stationary reference point is located on the left side of the trailer, extract all point clouds from the first grid containing valid point clouds as the fitted grid output after scanning that row. When the stationary reference point is located on the right side of the trailer, extract all point clouds from the last grid containing valid point clouds as the fitted grid output after scanning that row. The grid containing valid point clouds refers to the grids that are measured by point cloud density. Step 42: After all scans are completed, extract the raster that is closest to the stationary reference point from all row raster cells. Step 43: Output the fitted grid as the fitted grid set for the trailer side.

6. The trailer position detection method according to claim 3, characterized in that, In step 7, combining the physical dimensions of the trailer, the starting point position of the line segment on the other side of the trailer, the actual center point position, and the actual heading angle are sequentially restored, and the trailer pose is generated through calculation and conversion, including: Step 71: Based on the starting point and length of the line segment on one side of the trailer obtained in Step 6, determine two corner points of the quadrilateral frame of the trailer. The other two corner points are obtained by calculation and conversion based on the physical dimensions of the trailer. Step 72: After restoring the information of the four corner points, the actual center point position and actual heading angle of the trailer are further restored through calculation and transformation, thereby generating the trailer pose, thus completing the trailer pose detection.

7. The trailer position detection method according to claim 1, characterized in that, The position coordinates and heading angle are measured through the positioning function, and the speed, acceleration, angular velocity, front wheel steering angle and angular velocity of the front wheel steering angle are measured and extracted through an on-board six-degree-of-freedom IMU.

8. A trailer position and posture detection device, characterized in that, The device includes: The tractor real-time pose acquisition unit is used to acquire the real-time pose of the tractor, which includes at least position coordinates, velocity, acceleration, heading angle, angular velocity, front wheel steering angle and angular velocity of the front wheel steering angle. The trailer pose estimation value acquisition unit is used to acquire the pose estimation value of the trailer, calculate the detection range of the trailer based on the physical dimensions and kinematic equations of the trailer, and set a stationary reference point with the trailer as the reference object based on the estimated center point position and estimated heading angle of the trailer. The rasterization processing unit is used to dynamically select the target radar installed on the corresponding side of the tractor according to the detection range and the stationary reference point, extract the point cloud of the corresponding target radar and project it into the detection range for rasterization processing, generate the trailer side point cloud and put it into the corresponding grid. The trailer side point cloud extraction unit is used to traverse all point clouds of the target radar to obtain a rasterized candidate point cloud set; scan all rows within the detection range, and after all scans are completed, extract the raster with the shortest distance to the stationary reference point from all rows and output the fitted raster as the fitted raster set for the trailer side. The sampling and filtering unit is used to control the direction of the candidate point cloud set and the fitted grid set in the two dimensions of point cloud sampling and grid sampling, respectively, and to filter out the effective grids that make up the side of the trailer. The line fitting unit is used to perform line fitting processing on the selected effective grid, and after multiple iterations, obtain the starting point position and length of the line segment on one side of the trailer. The trailer pose generation unit is used to combine the physical dimensions of the trailer to sequentially restore the starting point position, actual center point position, and actual heading angle of the line segment on the other side of the trailer, and generate the trailer pose through calculation and conversion.

9. A terminal device, characterized in that, The terminal device is equipped with: Memory, used to store programs; A processor is configured to execute the program stored in the memory, wherein when the program is executed, the processor is configured to perform the trailer pose detection method as described in any one of claims 1 to 7.

10. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores computer-executable instructions; when executed by a processor, the computer-executable instructions are used to implement the trailer pose detection method as described in any one of claims 1 to 7.

Images