A method and system for intelligent sensing and control of a traction robot

Abstract

This invention belongs to the field of traction robot control technology, and relates to an intelligent perception and control method and system for traction robots. The method includes: generating trailer motion constraints; verifying the consistency of satellite positioning, ultra-wideband positioning, and odometer information and calculating their reliability, fusing them to obtain a fused pose and pose confidence level, discarding low-reliability information and re-fusing when the confidence level is below a threshold; constructing an environmental representation based on laser point cloud and image semantics and updating dynamic obstacles; generating and updating a target trajectory under the environmental representation and motion constraints according to environmental changes or confidence level changes; generating control commands based on the target trajectory, fused pose, and traction connection angle, and parking the vehicle when safety conditions are triggered. The technical solution of this application achieves pose reliability maintenance and trajectory-control closed-loop stability through the coordinated use of trailer motion constraints, multi-source positioning consistency verification, dynamic obstacle environmental representation, and trajectory adaptive updating, meeting the needs of trailer / RV relocation and parking in campsites and parking lots.

Description

Technical Field

[0001] This invention belongs to the field of traction robot control technology, and specifically relates to an intelligent sensing and control method and system for traction robots. Background Technology

[0002] In existing technologies, the movement and parking of towed objects such as trailers or RVs by towing robots typically rely on satellite positioning, onboard sensor perception, and preset parking strategies. This is achieved by acquiring the attitude relationship between the towing vehicle and the towed object and planning a driving path within the site to meet the needs of trailer positioning, alignment, and parking in confined spaces such as campsites and parking lots. However, existing towing and parking control methods have some significant shortcomings in terms of pose reliability management and trajectory update coordination in complex environments.

[0003] In practical applications, relevant systems often possess a certain degree of path planning and attitude control capabilities. However, under conditions of environmental occlusion such as trees, vehicles, and buildings, as well as changes in ground adhesion conditions, multi-source pose information exhibits consistent fluctuations within the time window, resulting in generally weak reliability and stability of the positioning results. Furthermore, when obstacle states are updated dynamically with changes in the human and vehicle, there is still room for improvement in the level of collaborative updates between trajectory generation and control execution. This can easily lead to fluctuations in the risk margin during the control process, affecting the consistency and repeatability of the operation.

[0004] Therefore, it is evident that existing technologies often suffer from weaknesses in maintaining pose reliability and in achieving coordinated stability between trajectory updates and control execution when performing trailer / RV towing and parking operations within confined spaces. These are the shortcomings of existing technologies.

[0005] In view of this, it is very necessary to provide an intelligent perception and control method and system for traction robots to solve the above-mentioned defects in the prior art. Summary of the Invention

[0006] The purpose of this invention is to address the shortcomings of existing technologies, such as weak position and posture reliability and weak coordination stability of trajectory updates and control execution when performing trailer / RV towing and parking operations in confined spaces, by providing a method and system for intelligent perception and control of a towing robot to solve the aforementioned technical problems.

[0007] To achieve the above objectives, the present invention provides the following technical solution: In a first aspect, embodiments of this application provide an intelligent sensing and control method for a traction robot, comprising: Acquire the parameters of the traction robot, the trailer parameters, and the traction connection angle information, and generate the trailer motion constraints; Satellite positioning information and ultra-wideband positioning information are collected, as well as inertial measurement information and wheel speed information, and odometer information is generated. Consistency checks are performed on the satellite positioning information, ultra-wideband positioning information, and odometer information. The reliability of satellite positioning, ultra-wideband positioning, and odometer information are calculated, and fused to obtain fused pose information and pose confidence. When the pose confidence is lower than the first preset threshold, downgraded fusion is performed. Downgraded fusion includes removing positioning information with confidence lower than the second preset threshold and re-fusing. Obstacle information is generated based on laser point cloud information and image semantic information, and an environment representation is constructed. Dynamic obstacle information is determined and updated based on obstacle information at adjacent acquisition times, and the dynamic obstacle information is written into the environment representation. Based on fused pose information, a target trajectory is generated under environmental representation and trailer motion constraints, and the target trajectory is updated when the environmental representation changes or the pose confidence changes. Based on the target trajectory, fused pose information and traction connection angle information, traction control commands are generated and the movement of the traction robot is controlled. When safety conditions are triggered, parking commands are generated and the traction robot is controlled to park.

[0008] Preferably, the step of generating trailer motion constraints includes: Determine the geometric relationships of the traction connection point and the trailer geometry based on the trailer parameters; Based on the traction connection angle information, a geometric consistency check is performed on the geometric relationship of the traction connection point and the trailer geometric relationship. If the geometric consistency check fails, the geometric relationship of the traction connection point or the trailer geometric relationship is corrected to a geometric relationship that meets the geometric consistency check requirements according to the preset fault tolerance rules. Based on the geometric relationship of the traction connection point after the geometric consistency check is passed, the geometric relationship of the trailer and the parameters of the traction robot are used to calculate the permissible range of the traction connection angle and the permissible range of the rate of change of the traction connection angle, and to generate the motion constraints of the trailer.

[0009] Preferably, the steps of performing consistency checks on satellite positioning information, ultra-wideband positioning information, and odometer information, calculating satellite positioning confidence, ultra-wideband positioning confidence, and odometer confidence, and fusing them to obtain fused pose information and pose confidence include: Within a preset time window, the odometer displacement increment and odometer heading increment are calculated based on odometer information, the satellite displacement increment and satellite heading increment are calculated based on satellite positioning information, and the ultra-wideband displacement increment and ultra-wideband heading increment are calculated based on ultra-wideband positioning information. The difference between the odometer displacement increment and the satellite displacement increment is calculated and denoted as the first displacement residual. The difference between the odometer heading increment and the satellite heading increment is calculated and denoted as the first heading residual. Based on the first displacement residual and the first heading residual, the satellite consistency test result is generated and the satellite positioning reliability is determined. The difference between the odometer displacement increment and the ultra-wideband displacement increment is calculated and denoted as the second displacement residual. The difference between the odometer heading increment and the ultra-wideband heading increment is calculated and denoted as the second heading residual. Based on the second displacement residual and the second heading residual, the ultra-wideband consistency test result is generated and the reliability of ultra-wideband positioning is determined. Based on the first displacement residual, the second displacement residual, the first heading residual, and the second heading residual, the odometer consistency test results are generated and the odometer reliability is determined. The satellite positioning reliability, ultra-wideband positioning reliability, and odometer reliability are normalized to obtain the satellite fusion weight, ultra-wideband fusion weight, and odometer fusion weight. The satellite displacement increment, ultra-wideband displacement increment, and odometer displacement increment are weighted and fused to obtain the fused displacement increment. The satellite heading increment, ultra-wideband heading increment, and odometer heading increment are weighted and fused to obtain the fused heading increment. The fused pose information is updated based on the fused displacement increment and fused heading increment. The pose confidence is generated based on the first displacement residual, the second displacement residual, the first heading residual, the second heading residual, the satellite positioning confidence, the ultra-wideband positioning confidence, and the odometer confidence.

[0010] Preferably, the step of determining and updating dynamic obstacle information based on obstacle information at adjacent acquisition times, and writing the dynamic obstacle information into the environment representation, includes: Perform data association on obstacle information from adjacent acquisition times and generate obstacle association results; Based on the obstacle association results, the obstacle motion state is generated, and dynamic obstacle information is determined and updated. Motion prediction information is generated based on dynamic obstacle information. The dynamic obstacle information and motion prediction information are written into the environmental representation and participate in the triggering determination of environmental representation changes.

[0011] Preferably, the steps of generating a target trajectory based on fused pose information under environmental representation and trailer motion constraints, and updating the target trajectory when the environmental representation changes or the pose confidence changes, include: The current pose of the traction robot is determined based on the fused pose information, and the target pose is determined based on the target parking information. Generate accessible area information and obstacle constraint information based on environmental representation; Under the constraints of passable area information, obstacle constraint information, and trailer motion constraint, a set of candidate trajectories from the current pose to the target pose is generated, and the feasibility of the candidate trajectory set is checked. The trajectory cost is calculated for the candidate trajectories that pass the feasibility check and the target trajectory is selected. The trajectory cost includes the deviation cost between the candidate trajectory termination pose and the target pose, and the risk cost of the candidate trajectory in the environmental representation. When changes in environmental representation or pose confidence meet preset update conditions, the latest laser point cloud information and image semantic information are collected and the environmental representation is updated. The latest satellite positioning information, ultra-wideband positioning information and odometer information are collected and the fused pose information and pose confidence are updated. The current pose of the traction robot is re-determined, and a new set of candidate trajectories from the current pose to the target pose is generated and the target trajectory is updated.

[0012] Preferably, safety conditions include slippage and freewheeling, and the determination and triggering of safety conditions include: Based on wheel speed information and motor speed information, combined with traction robot parameters, the desired speed and desired displacement are generated. Based on the fused pose information, the actual speed and actual displacement are generated. The speed deviation between the desired speed and the actual speed, as well as the displacement deviation between the desired displacement and the actual displacement, are calculated. When the speed deviation or displacement deviation meets the preset judgment conditions, the slippage state or the freewheeling state is determined. Lateral deviation and heading deviation are generated based on the target trajectory and fused pose information. Connection angle deviation is generated based on the traction connection angle information. Control strategy identifiers are generated based on lateral deviation, heading deviation and connection angle deviation. Control strategy identifiers include conventional tracking strategy identifiers and angle suppression strategy identifiers. In the slipping or idling state, the control strategy identifier is determined as the included angle suppression strategy identifier and the upper limit of speed and acceleration of the traction control command is updated. The upper limit of the steering rate of change of the traction control command is constrained and the allowable range of the traction connection angle of the trailer motion constraint is tightened. The safety condition is triggered when the preset continuous judgment condition is met continuously in the slipping or idling state.

[0013] Preferably, the steps for generating parking instructions include: Generate a set of control states and perform state transitions. The set of control states includes automatic control state, degraded control state, and safe stop state. In automatic control mode, trajectory control commands are generated based on the target trajectory, and safety control commands are generated based on safety conditions; The trajectory control command and safety control command are subjected to consistency verification and verification results are generated. When the verification results meet the preset failure conditions, the control state is degraded and the upper limit of speed, upper limit of acceleration and permissible range of traction connection angle are tightened. When the preset exit conditions are met in the downgraded control state, the vehicle enters the safe parking state and generates a parking command. The parking command is maintained in the safe parking state until the preset unlocking conditions are met.

[0014] Secondly, embodiments of this application also provide an intelligent perception and control system for a traction robot, comprising: The constraint generation unit is used to acquire the parameters of the traction robot, the parameters of the trailer, and the traction connection angle information, and to generate the motion constraints of the trailer. The pose fusion unit is used to collect satellite positioning information and ultra-wideband positioning information, collect inertial measurement information and wheel speed information and generate odometer information, perform consistency checks on satellite positioning information, ultra-wideband positioning information and odometer information, calculate satellite positioning confidence, ultra-wideband positioning confidence and odometer confidence, and fuse them to obtain fused pose information and pose confidence. When the pose confidence is lower than a first preset threshold, downgraded fusion is performed. Downgraded fusion includes removing positioning information with confidence lower than a second preset threshold and re-fusing. The environment construction unit is used to generate obstacle information and construct an environment representation based on laser point cloud information and image semantic information, determine and update dynamic obstacle information based on obstacle information at adjacent acquisition times, and write the dynamic obstacle information into the environment representation. The trajectory generation unit is used to generate a target trajectory based on fused pose information, under environmental representation and trailer motion constraints, and to update the target trajectory when the environmental representation changes or the pose confidence changes. The control output unit is used to generate traction control commands and control the movement of the traction robot based on the target trajectory, fused pose information and traction connection angle information, and to generate parking commands and control the traction robot to park when safety conditions are triggered.

[0015] Preferably, the system also includes a security determination unit, which is used for: Based on wheel speed information and motor speed information, combined with traction robot parameters, the desired speed and desired displacement are generated. Based on the fused pose information, the actual speed and actual displacement are generated. The speed deviation between the desired speed and the actual speed, as well as the displacement deviation between the desired displacement and the actual displacement, are calculated. When the speed deviation or displacement deviation meets the preset judgment conditions, the slippage state or the freewheeling state is determined. Lateral deviation and heading deviation are generated based on the target trajectory and fused pose information. Connection angle deviation is generated based on the traction connection angle information. Control strategy identifiers are generated based on lateral deviation, heading deviation and connection angle deviation. Control strategy identifiers include conventional tracking strategy identifiers and angle suppression strategy identifiers. In the slipping or idling state, the control strategy identifier is determined as the angle suppression strategy identifier, and the constraint update of the upper limit of speed, upper limit of acceleration and upper limit of steering change rate of the traction control command is triggered. At the same time, the tightening of the allowable range of traction connection angle of trailer motion constraint is triggered.

[0016] Preferably, the trajectory generation unit is also used for: The current pose of the traction robot is determined based on the fused pose information, and the target pose is determined based on the target parking information. Generate accessible area information and obstacle constraint information based on environmental representation; Under the constraints of passable area information, obstacle constraint information, and trailer motion constraint, a set of candidate trajectories from the current pose to the target pose is generated, and the feasibility of the candidate trajectory set is checked. The trajectory cost is calculated for the candidate trajectories that pass the feasibility check and the target trajectory is selected. The trajectory cost includes the deviation cost between the candidate trajectory termination pose and the target pose, and the risk cost of the candidate trajectory in the environmental representation. When changes in environmental representation or pose confidence meet preset update conditions, the latest laser point cloud information and image semantic information are collected and the environmental representation is updated. The latest satellite positioning information, ultra-wideband positioning information and odometer information are collected and the fused pose information and pose confidence are updated. The current pose of the traction robot is re-determined, and a new set of candidate trajectories from the current pose to the target pose is generated and the target trajectory is updated.

[0017] As can be seen from the above technical solutions, the present invention has the following advantages: This application provides an intelligent perception and control method and system for a traction robot. It generates trailer motion constraints based on traction robot parameters, trailer parameters, and traction connection angles. Consistency checks are performed on satellite positioning, ultra-wideband positioning, and odometer information to calculate the reliability of each positioning source. These are then fused to obtain fused pose information and pose confidence. Downgraded fusion is implemented, eliminating low-confidence information sources when the confidence level falls below a preset threshold. Simultaneously, an environmental representation containing dynamic obstacle information is constructed based on laser point cloud information and image semantic information. The target trajectory is updated when the environmental representation changes or the pose confidence level changes. Traction control commands are generated by combining trailer motion constraints, and parking commands are generated when safety conditions are triggered. This achieves suppression of multi-source pose consistency fluctuations and continuous maintenance of pose reliability during traction and parking operations in confined spaces. It improves the collaborative stability and risk margin stability of target trajectory updates and control execution under complex conditions such as occlusion and changes in attachment conditions. It enhances the consistency and repeatability of the operation process, meeting the requirements for positioning reliability, trajectory execution stability, and safety control controllability in trailer / RV relocation and parking operations in campsites, parking lots, and other locations.

[0018] Furthermore, the design principle of this invention is reliable, the structure is simple, and it has a very wide range of application prospects.

[0019] Therefore, it is evident that the present invention has outstanding substantive features and significant progress compared with the prior art, and the beneficial effects of its implementation are also obvious. Attached Figure Description

[0020] To more clearly illustrate the technical solution of the present invention, the accompanying drawings used in the description 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.

[0021] Figure 1 This is a flowchart of an intelligent perception and control method for a traction robot provided by the present invention; Figure 2 This is a schematic diagram of the intelligent perception and control system for a traction robot provided by the present invention.

[0022] The components include: 1. Constraint generation unit, 2. Pose fusion unit, 3. Environment construction unit, 4. Trajectory generation unit, and 5. Control output unit. Detailed Implementation

[0023] Various embodiments of this disclosure are described more fully below with reference to the accompanying drawings. This disclosure may have various embodiments, and adjustments and changes may be made therein. However, it should be understood that there is no intention to limit the various embodiments of this disclosure to the specific embodiments disclosed herein, but rather this disclosure should be understood to cover all adjustments, equivalents, and / or alternatives falling within the spirit and scope of the various embodiments of this disclosure.

[0024] In the following, the terms “comprising” or “may include”, which may be used in various embodiments of this disclosure, indicate the presence of the disclosed functions, operations, or elements, and do not limit the addition of one or more functions, operations, or elements. Furthermore, as used in various embodiments of this disclosure, the terms “comprising,” “having,” and their cognates are intended only to indicate a particular feature, number, step, operation, element, component, or combination of the foregoing, and should not be construed as primarily excluding the presence of one or more other features, numbers, steps, operations, elements, components, or combinations of the foregoing, or the possibility of adding one or more combinations of the foregoing.

[0025] It should be noted that, in various embodiments of this disclosure, the expression "or" or "at least one of A and / or B" includes any combination or all combinations of the words listed simultaneously. For example, the expression "A or B" or "at least one of A and / or B" may include A, may include B, or may include both A and B.

[0026] The terms used in the various embodiments of this disclosure (such as "first," "second," etc.) may modify various components in the various embodiments, but do not limit the corresponding components. For example, the above terms do not limit the order and / or importance of the components. The above terms are only used for the purpose of distinguishing one component from other components. For example, a first user device and a second user device refer to different user devices, although both are user devices. For example, a first component may be referred to as a second component without departing from the scope of the various embodiments of this disclosure, and similarly, a second component may also be referred to as a first component.

[0027] In the description of this embodiment, it should be noted that the terms "upper", "lower", "inner", "bottom", etc., indicate the orientation or positional relationship based on the orientation or positional relationship shown in the accompanying drawings, or the orientation or positional relationship that the product of the invention is usually placed in during use. They are only for the convenience of describing the present invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, or be constructed and operated in a specific orientation. Therefore, they should not be construed as limiting the present invention.

[0028] It should be noted in advance that, in order to facilitate a clear and accurate description of the technical solutions in the embodiments of this application, the following is a brief explanation of some terms and related technologies involved in the embodiments of this application: 1. Ultra-wideband positioning Ultra-wideband (UWB) positioning obtains location through wireless ranging and positioning calculations between the target and the base station. Common mechanisms include Time of Flight (TOF) or Time Difference of Arrival (TDOA), which can provide relatively stable positioning support in environments with satellite signal blockage or multipath propagation.

[0029] 2. Inertial measurement information Inertial measurement information typically comes from an IMU (Inertial Measurement Unit), which consists of linear acceleration and angular velocity measured by accelerometers and gyroscopes. It is used to calculate attitude, angular velocity, and short-term motion state, and is often combined with wheel speed information to generate an odometer to provide high-frequency continuous relative attitude estimation.

[0030] 3. Odometer information Odometer information typically refers to relative motion estimation information such as displacement increment and heading increment calculated by wheel speed encoders, IMUs, or a combination thereof. It is characterized by high update frequency and short-term continuity, but its error may increase under conditions such as slippage, idling, or bias accumulation.

[0031] 4. Laser point cloud information Laser point cloud information typically refers to a set of three-dimensional discrete points measured by lidar. Each point contains spatial coordinates and may contain attributes such as intensity. Through point cloud segmentation, clustering, or geometric fitting, environmental elements such as obstacle outlines and passable areas can be formed, which are used for environmental representation construction and obstacle avoidance planning.

[0032] 5. Data Association Data association typically refers to the process of establishing a "correspondence relationship for the same target" between observation data from multiple time points. It is used to match obstacle observations at adjacent acquisition times to estimate the target's motion state and further generate motion prediction information. Common implementations can be based on nearest neighbor matching, gating, and cost function optimization.

[0033] To address the issues of inconsistent multi-source positioning information during trailer / RV relocation and parking operations in confined spaces, which can lead to weak pose reliability due to occlusion, changes in attachment conditions, and poor stability of trajectory updates and control execution coordination when obstacles dynamically change, resulting in significant fluctuations in risk margin, this application discloses an intelligent perception and control method and system for towing robots. This method establishes trailer motion constraints and combines them with the traction connection angle for constrained control, utilizing satellite positioning, ultra-wideband positioning, and odometer information. A consistency check is performed to obtain credibility and the fused pose and pose confidence are fused. When the confidence decreases, a downgrade fusion is performed to maintain the reliability of the pose output. At the same time, laser point cloud and image semantics are fused to construct an environmental representation containing dynamic obstacle information and generate and update the target trajectory accordingly. This enables a collaborative closed loop of trajectory update and motion control in the dynamic environment. Parking control is implemented when safety conditions are triggered, thereby improving the pose reliability maintenance capability and path execution stability under complex site conditions, enhancing the safety margin consistency and repeatability of the operation process, and meeting the requirements of reliable positioning, stable control and safe controllability for confined space towing and parking operations.

[0034] The technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of this application, and not all embodiments. Based on the embodiments of this application, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this application.

[0035] like Figure 1 As shown in the figure, this embodiment provides an intelligent perception and control method for a traction robot, including: Step S1: Obtain the parameters of the traction robot, the parameters of the trailer, and the traction connection angle information, and generate the motion constraints of the trailer; Step S2: Collect satellite positioning information and ultra-wideband positioning information, collect inertial measurement information and wheel speed information and generate odometer information, perform consistency checks on satellite positioning information, ultra-wideband positioning information and odometer information, calculate satellite positioning confidence, ultra-wideband positioning confidence and odometer confidence, and fuse them to obtain fused pose information and pose confidence. When the pose confidence is lower than the first preset threshold, perform downgrade fusion. Downgrade fusion includes removing positioning information with confidence lower than the second preset threshold and re-fusing. Step S3: Generate obstacle information and construct an environment representation based on laser point cloud information and image semantic information; determine and update dynamic obstacle information based on obstacle information at adjacent acquisition times; and write the dynamic obstacle information into the environment representation. Step S4: Based on the fused pose information, generate the target trajectory under the environmental representation and trailer motion constraints, and update the target trajectory when the environmental representation changes or the pose confidence changes; Step S5: Generate traction control commands based on the target trajectory, fused pose information and traction connection angle information, and control the movement of the traction robot. When safety conditions are triggered, generate parking commands and control the traction robot to park.

[0036] This embodiment generates trailer motion constraints based on traction robot parameters, trailer parameters, and traction connection angles. This ensures consistent constraints on the trailer's motion boundaries during the planning and control phases of the traction parking process, reducing the risk of trajectory deviation caused by abnormal expansion of the connection angle. By performing consistency checks and calculating the reliability of satellite positioning information, ultra-wideband positioning information, and odometer information, fused pose information and pose confidence are output. When the pose confidence decreases, a downgraded fusion process is implemented to remove low-reliability information sources, ensuring that the pose output remains continuous and reliable under conditions such as occlusion and changes in attachment conditions, suppressing the amplification effect of multi-source pose consistency fluctuations on the control link. Furthermore, by fusing laser point cloud information and image semantics… The system constructs an environmental representation that includes dynamic obstacle information and triggers target trajectory updates when the environmental representation changes or the pose confidence level changes. This enables a coordinated response between environmental changes caused by dynamic obstacles and fluctuations in positioning confidence level, ensuring both controllable safety margins and trajectory execution efficiency. Furthermore, by generating traction control commands based on the target trajectory, fused pose information, and traction connection angle, and generating parking commands when safety conditions are triggered, the system creates a controllable closed loop between automatic control, degraded control, and safe stopping for traction parking operations. This enhances the pose reliability maintenance capability and trajectory-control coordination stability of traction parking operations in confined spaces, thereby improving the consistency, repeatability, and safety controllability of the operation process.

[0037] Hereinafter, steps S1 to S5 will be specifically described according to embodiments of this application.

[0038] In step S1, the core task is to obtain key parameters that can characterize the coupling relationship between the traction robot and the trailer, and solidify them into trailer motion constraints that can be directly invoked by trajectory generation and control execution, so as to always satisfy the safety boundary of the traction connection angle and its rate of change when searching for feasible trajectories in a confined space.

[0039] In this embodiment, the key parameters obtained may include traction robot parameters, trailer parameters, and traction connection angle information, thereby generating trailer motion constraints. The traction robot parameters may include the traction robot's geometric dimensions, equivalent track / wheel system radius, equivalent wheelbase or track center distance, maximum traction force, maximum linear velocity, maximum acceleration, maximum deceleration, maximum angular velocity, maximum angular acceleration, and mass distribution. The trailer parameters may include the trailer's geometric dimensions, wheelbase, track width, front and rear overhang lengths, distance from the traction connection point to the trailer's center of mass, distance from the traction connection point to the trailer's axle, trailer mass, and moment of inertia. The traction connection angle information characterizes the relative angle between the traction robot's heading and the trailer's heading, and can be directly provided by a trailer angle sensor or indirectly derived from attitude calculation.

[0040] It should be noted that, to ensure consistent calculation methods, a global coordinate system can be established. , Traction robot body coordinate system coordinate system with trailer body ,in of The axis points east of the site. The axis points to the north of the site. of The axis is aligned with the direction of the traction robot's movement. of The axle is aligned with the longitudinal direction of the trailer; the traction robot is in The pose in the middle can be denoted as The trailer was The pose in the middle can be denoted as ,in For position coordinates, For heading angle (around) of (Axis, counterclockwise is positive); the traction connection angle can be defined as... ,in Used to normalize the angle To ensure continuity.

[0041] In some embodiments of this application, in order to bring the "parameter-geometry-constraint" link to a computable expression, it is necessary to clarify the definition of the traction connection point and the trailer geometric reference in the parameter-to-geometry mapping.

[0042] Specifically, the geometric relationships of the traction connection point and the trailer can be determined based on the trailer parameters. In this process, the geometric relationship of the traction connection point can be defined as: the traction connection point of the traction robot in... Fixed coordinates in and trailer towing connection point at Fixed coordinates in ,in The forward and backward offset is from the traction connection point to the geometric center of the traction robot; the trailer geometry can be defined as: the trailer axle center is at... coordinates in ,in This represents the distance from the towing connection point to the center of the trailer axle, and the trailer envelope polygon can also be defined. Used for collision detection and risk calculation. Therefore, the traction connection point is... The position in the middle can be written as:

[0043] in, , It is a two-dimensional rotation matrix.

[0044] Trailer axle center at The position in the middle can be written as:

[0045] Based on the above geometric definitions, the spatial relationship between the traction robot and the trailer can be uniformly defined as follows: , , and On parameters such as...

[0046] Furthermore, actual assembly and parameter input may lead to inconsistencies in geometric caliber, resulting in a "mismatch between theoretical geometry and sensing angles" in constraint generation. Therefore, a geometric consistency check can be introduced before constraint generation to achieve caliber closure. During this process, a geometric consistency check can be performed on the geometric relationships of the traction connection point and the trailer based on the traction connection angle information. If the geometric consistency check fails, the geometric relationships of the traction connection point or the trailer are corrected to meet the geometric consistency check requirements according to preset fault-tolerance rules.

[0047] Specifically, the geometric consistency check can be constructed using the following formula: at the same time Obtained from pose calculation and At that time, the predicted connection angle can be calculated. The traction connection angle provided by the sensor is Then the angle consistency error ; within the preset time window Internal Statistics mean With variance ,when or The verification was deemed unsuccessful at that time. The threshold value is used.

[0048] Based on this, preset fault tolerance rules may include: prioritizing Forward and backward offset Make fine adjustments to make Converging to near zero; or when there is a significant deviation in the trailer parameter input, Perform amplitude limiting correction; or, when a fixed angular offset exists during installation, introduce a fixed offset angle. And make the corrected connection angle ,in .

[0049] For example, in a campsite or parking lot scenario, parameters such as trailer mass and wheelbase can be obtained by scanning a code, user input, or historical records. For example, the trailer mass is 2.8t and the wheelbase is 2.6m. The towing connection angle can be output by the trailer angle sensor at a frequency of 50Hz or higher and aligned with the pose calculation timestamp.

[0050] In some embodiments of this application, the trailer motion constraints need to specify not only the static geometric boundaries of the connection angles but also the boundaries of the rate of change of the connection angles. This allows the control layer to actively converge to a more conservative controllable region when there are changes in the adhesion coefficient, slope, or sudden turns during local obstacle avoidance. During this process, the permissible range of the traction connection angle and the permissible range of the rate of change of the traction connection angle can be calculated based on the geometric relationships of the traction connection points after geometric consistency verification, the trailer geometry, and the parameters of the traction robot, thereby generating the trailer motion constraints.

[0051] Specifically, the permissible range of the traction connection angle can be expressed as: ,in, It can be determined by both mechanical limits and anti-folding margin; the permissible range of the traction connection angle change rate can be expressed as: .

[0052] Based on this, a simplified traction kinematics can be established: Let the linear velocity of the traction robot be angular velocity is The rate of change of the trailer's heading angle It can be approximated as:

[0053] The rate of change of the connection angle for:

[0054] in, .

[0055] Therefore, Constraints transformed into pairs , and The coupling constraints can be further given at discrete times. Feasible domain:

[0056] in, To control the cycle, trailer motion constraints can be organized as follows: This will serve as a unified input for subsequent steps.

[0057] Thus far, step S1, with parameters, geometric consistency, and the permissible domain of angle / rate of change as the main thread, constitutes a trailer motion constraint expression that can directly participate in trajectory feasibility verification and control amplitude limiting, providing an executable kinematic boundary basis for subsequent pose fusion, environmental representation update, and trajectory generation.

[0058] In step S2, the core task is to perform consistency verification and credibility weighting on the three types of pose sources—satellite positioning, ultra-wideband positioning, and odometry—within the same time window, and automatically perform downgrade fusion when the credibility decreases, so as to continuously output fused pose information and pose confidence, thereby avoiding pose jitter caused by occlusion, multipath, and changes in attachment conditions from being directly amplified to the planning and control layer.

[0059] In this embodiment, satellite positioning information and ultra-wideband positioning information can be collected, along with inertial measurement information and wheel speed information to generate odometer information. Then, a consistency check is performed on the satellite positioning information, ultra-wideband positioning information, and odometer information. The reliability of satellite positioning, ultra-wideband positioning, and odometer information is calculated, and they are fused to obtain fused pose information and pose confidence. If the pose confidence is lower than a first preset threshold, positioning information with a confidence level lower than a second preset threshold is discarded and re-fused. Specifically, satellite positioning information can provide absolute coordinates and heading (or heading obtained via a dual-antenna / inertial navigation system combination). Ultra-wideband positioning information can obtain a relatively stable planar position through ranging calculations between base stations and tags. Odometer information is obtained by fusing inertial measurement information and wheel speed information, providing displacement and heading increments at high frequencies. A unified timestamp can be used. For discrete sequence processing, a preset time window length is defined as... The time window is .

[0060] It should be noted that odometer information is usually used in consistency checks in an incremental form, thereby transforming "absolute positioning drift" into "short-term incremental differences", which makes it easier to evaluate multi-source consistency using the same residual caliber.

[0061] In some embodiments of this application, the odometer displacement increment and odometer heading increment can be calculated based on odometer information within a preset time window, the satellite displacement increment and satellite heading increment can be calculated based on satellite positioning information, and the ultra-wideband displacement increment and ultra-wideband heading increment can be calculated based on ultra-wideband positioning information.

[0062] Specifically, it can be arrive Odometry displacement increment defined on a single cycle Odometer heading increment Satellite displacement increment Satellite heading increment Ultra-wideband displacement increment With ultra-wideband heading increment For example, , ,in, For satellite positioning planar location, This refers to the satellite's heading angle; and The calculation can be performed using the same method from the ultra-wideband positioning output; This can be obtained by integrating the wheel speed. It can be obtained by integrating the gyroscope's angular velocity or derived from the wheel speed difference. Furthermore, to reduce the impact of noise, it can also be calculated within a time window. The increment is then filtered using a moving average or median.

[0063] In some embodiments of this application, the consistency check can use the odometer as a high-frequency continuous reference, subtract the satellite and ultra-wideband increments from the odometer increments to obtain the residuals, and construct a reliability mapping accordingly.

[0064] Specifically, the odometer displacement increment can be calculated. With satellite displacement increment The difference is denoted as the first displacement residual. ; Calculate the odometer heading increment Satellite heading increment The difference is denoted as the first heading residual. Based on this, and based on the first displacement residual and first heading residual Generate satellite consistency verification results and determine the reliability of satellite positioning. The satellite consistency verification results can be obtained using composite residuals. ,in, For displacement residual scale parameters, The heading residual scale parameter; satellite positioning reliability can be mapped to... And further, the average value is obtained within the time window. To resist single-point anomalies.

[0065] In some embodiments of this application, the calculation method for ultra-wideband positioning reliability is isomorphic to that for satellite positioning reliability, so as to achieve a "comparable reliability scale" under different obstruction conditions. Specifically, the odometry displacement increment is calculated. With ultra-wideband displacement increment The difference is denoted as the second displacement residual. ; Calculate the odometer heading increment With ultra-wideband heading increment The difference is denoted as the second heading residual. Based on this, and based on the second displacement residual Second heading residual Generate ultra-wideband (UWB) consistency verification results and determine the reliability of UWB positioning. The UWB consistency verification results can be obtained using composite residuals. The reliability of ultra-wideband positioning can be mapped to And the mean value within the time window can be obtained. .

[0066] Furthermore, since odometers may also become distorted under conditions of slippage, idling, or long-term cumulative bias, the reliability of odometers needs to be constrained by "consistency with the two types of external positioning" and bound to their weight normalization.

[0067] Specifically, it can be based on the first displacement residual Second displacement residual First heading residual Second heading residual The odometry consistency verification results are generated and the odometry reliability is determined. The satellite positioning reliability, ultra-wideband positioning reliability, and odometry reliability are then normalized to obtain the satellite fusion weight, ultra-wideband fusion weight, and odometry fusion weight. The odometry consistency verification results can be expressed as composite residuals, defined as:

[0068] Odometer reliability can be mapped to And take the mean within the time window. .

[0069] Based on this, normalization can be defined as:

[0070]

[0071]

[0072] in, To prevent extremely small positive numbers with a denominator of zero, For satellite fusion weights, For ultra-wideband converged weights, For odometer fusion weights.

[0073] For example, the satellite positioning output frequency can be 1–10 Hz, the ultra-wideband positioning output frequency can be 10–50 Hz, the inertial measurement information and wheel speed information can be 100 Hz or higher, and the odometer information can be generated at 100 Hz and provided to the consistency verification and fusion calculation.

[0074] In some embodiments of this application, the fusion is not performed directly in the absolute position domain, but rather in the incremental domain and updated to the fused pose, thereby maintaining trajectory continuity in the presence of short-term anomalies.

[0075] Specifically, it can be used to measure the satellite displacement increment. Ultra-wideband displacement increment and odometer displacement increment Weighted fusion is performed to obtain the fusion displacement increment. ; Increment of satellite heading Ultra-wideband heading increment and odometer heading increment Weighted fusion is performed to obtain the fusion heading increment. .

[0076] Furthermore, based on the fusion displacement increment and integration of course increments The updated fused pose information is used to generate pose confidence based on the first displacement residual, the second displacement residual, the first heading residual, the second heading residual, satellite positioning confidence, ultra-wideband positioning confidence, and odometry confidence.

[0077] Specifically, if the fused pose was at the previous moment Then it can be updated:

[0078]

[0079]

[0080] At the same time, pose confidence can be used arrive scalar The characterization is generated jointly by the residuals and the confidence level. For example, the residual strength can be defined as follows:

[0081] and the overall credibility of the location source Then the pose confidence level can be defined. ,in, This is a truncation function.

[0082] In the embodiments of this application, when the pose confidence level Below the first preset threshold At this point, a downgraded fusion can be initiated: the credibility level is lowered than the second preset threshold. Location source set Remove them, and recalculate the weights of the remaining location sources after normalization. and This allows for proactive avoidance of low-reliability information sources contaminating the fusion output, regardless of any failure mode such as satellite obstruction, ultra-wideband multipath propagation, or odometer slippage.

[0083] Thus, step S2, with the time window incremental consistency check as its core, forms a closed-loop pose management mechanism of "credible quantification - weighted fusion - confidence evaluation - automatic degradation for low confidence", providing quantifiable basis for subsequent environmental representation update triggering and trajectory update triggering.

[0084] In step S3, the aim is to structurally represent the environmental elements related to the safe movement of the traction robot within the confined space. This representation should reflect both static obstacles (walls, pillars, parking space boundaries, etc.) and continuously record the motion state and prediction results of dynamic obstacles (pedestrians, vehicles, etc.) to support subsequent trajectory cost calculation and update trigger determination. During this process, obstacle information can be generated and an environmental representation constructed based on laser point cloud information and image semantic information. Simultaneously, dynamic obstacle information is determined and updated based on obstacle information acquired at adjacent acquisition times, and this dynamic obstacle information is written into the environmental representation.

[0085] Specifically, laser point cloud information can be acquired by a lidar with a large forward field of view (e.g., 270°), image semantic information can be generated by a surround-view camera (e.g., a 120° wide-angle camera) and a semantic segmentation network, and obstacle information can include obstacle location, shape outline, category label, and confidence level. Based on this, environment representation can employ a grid occupancy map. With risk field The combination of forms, in which Indicates whether a grid cell is occupied. Indicates the level of risk of the grid; simultaneously maintains the obstacle set. As a concrete expression of obstacle information, each obstacle Having a state vector ,in As the central location, For category numbering, For geometric scale parameters, For the orientation angle, Confidence level for obstacles.

[0086] For example, the field of view of the lidar can be large enough, with a horizontal angle of 360° and a vertical angle of 31°; the ranging capability can reach 120 meters; the diagonal of a 30m×40m area is about 50 meters, which is within the effective ranging range of the lidar; at the same time, the point cloud density can be large enough, such as 640,000 points / second in single-echo mode and 1,280,000 points / second in dual-echo mode, using the default 600RPM (10Hz) scanning frequency in a 30×40m area.

[0087] It should be noted that obstacle information generated from laser point clouds can be achieved through ground segmentation and clustering: for point clouds First, perform height thresholding or plane fitting to remove the ground, resulting in a non-ground point cloud. ;right Euclidean clustering yields point clusters. For each cluster of points, calculate the minimum bounding rectangle or convex hull to obtain the geometric contour, and project it onto... Planar updates occupy the grid.

[0088] Meanwhile, image semantic information can be output as pixel-level category maps through a semantic segmentation network. Then, combining the camera's intrinsic and extrinsic parameters, the semantic results are projected onto the ground plane to obtain semantic occupancy and category confidence. The two can be fused at the same grid resolution: when laser occupancy and visual semantics conflict on the same grid, they can be fused according to... The weighted or "security-first" strategy is used to determine if the position is true.

[0089] In some embodiments of this application, the key to dynamic obstacle information lies in cross-time correlation and motion state estimation. Data correlation can be performed on obstacle information from adjacent acquisition times to generate obstacle correlation results.

[0090] Specifically, it can be between two adjacent moments. and obstacle collection and Construct the correlation cost matrix between them ,in,

[0091] in, For location, For scale features, For indicator functions, As weights. For those satisfying the gating condition. The candidate pairs are used to obtain the association relationship using the Hungarian algorithm or greedy matching. This serves as the result of the obstacle association.

[0092] In some embodiments of this application, obstacle motion states can be generated based on obstacle association results, thereby determining and updating dynamic obstacle information. During this process, obstacle speeds can be estimated based on the association relationships, and static and dynamic obstacles can be distinguished accordingly.

[0093] Specifically, for the matched trajectory of the same obstacle, the speed can be calculated. And calculate the velocity norm. .when And continuous If a certain number of cycles are satisfied, it can be determined as a dynamic obstacle; otherwise, it is determined as a static obstacle. For speed threshold, To determine the length of the continuous determination.

[0094] Based on this, dynamic obstacle information can be organized as follows: Each Carrying status and covariance Used for expressing uncertainty.

[0095] Furthermore, in order to enable trajectory generation to avoid the future location of dynamic obstacles in advance, motion prediction of dynamic obstacles needs to be performed, motion prediction information is generated based on dynamic obstacle information, and the dynamic obstacle information and motion prediction information are written into the environmental representation and participate in the triggering determination of environmental representation changes, so that the risk cost increases as the prediction occupies the area.

[0096] Specifically, a constant velocity model can be used:

[0097] in, To predict the time domain, To predict the duration, Kalman filtering can also be used for state transitions. Generate multi-step predictions.

[0098] After discretizing the predicted trajectory into a grid, the risk level of the grid can be assessed. Overlaying predicted risk increments, for example:

[0099] in The coordinates of the grid center are, This serves as a scale for risk diffusion. Simultaneously, environmental changes can be represented by the rate of change. or risk field change rate Characterization, in which The total number of grid cells. or Exceeding the threshold can trigger subsequent trajectory updates.

[0100] For example, environmental perception can be configured with a large forward field of view lidar for SLAM (Simultaneous Localization and Mapping) and obstacle avoidance, and combined with surround view cameras for semantic segmentation to identify parking lines, pillars and pedestrians. Ultrasonic probes can be configured for reversing redundancy at close range; during trajectory optimization, a safe boundary of not less than 0.3m with obstacles can be maintained.

[0101] Thus far, step S3, through the path of "point cloud / semantic obstacle generation - cross-time association - dynamic judgment and prediction - writing occupancy and risk field", forms an environment representation that can be updated over time, enabling trajectory generation to maintain a stable risk margin based on the latest obstacle state.

[0102] In step S4, the core task is to generate an executable target trajectory by combining the current pose given by the fused pose information and the target pose given by the target parking information, along with the environmental representation and trailer motion constraints. This trajectory is updated when the environmental representation changes or the pose confidence changes, ensuring that the target trajectory always satisfies obstacle constraints and traction connection angle related constraints. The target trajectory is defined as a discrete time sequence. The above can be represented as , This represents the pose of the traction robot in the global coordinate system. Indicates linear velocity. The angular velocity is represented; the trailer motion constraint is defined as the allowable interval of the traction connection angle and the allowable interval of the rate of change of the traction connection angle, constraining the feasible region of the trajectory. The sequence of the traction connection angle at discrete times is denoted as... and satisfy as well as .

[0103] In this embodiment, the trajectory boundary conditions need to be determined by fusing pose information and target parking information. During this process, the current pose of the traction robot can be determined based on the fused pose information, and the target pose can be determined based on the target parking information.

[0104] Specifically, the current pose of the traction robot is taken The target parking information includes the target location. Orientation towards the target Target pose Furthermore, to ensure the target pose is feasible for engineering purposes, a target tolerance can be introduced. and And used in trajectory termination determination and .

[0105] Furthermore, the environment representation can be decomposed into traversable region information and obstacle constraint information during the trajectory generation stage, to simultaneously support hard constraint removal and soft constraint cost calculation. The traversable region information can be derived from the occupancy grid map. Defined as Obstacle constraint information is defined as and the expansion set Based on this, define a safe distance. Used to constrain the minimum distance between the traction robot and obstacles; the environmental representation also includes the risk field. A higher risk value indicates a closer proximity to an obstacle or a predicted area occupied by a dynamic obstacle. For example, in trajectory optimization, the distance between the traction robot and the obstacle can be maintained at no less than 0.3m, i.e. .

[0106] Furthermore, under the conditions of passable area information, obstacle constraint information, and trailer motion constraints, a set of candidate trajectories from the current pose to the target pose can be generated, and the feasibility of the candidate trajectory set can be verified.

[0107] In some embodiments of this application, the candidate trajectory set can be generated in two stages: global search and local optimization. First, a global reference path is generated on a discrete grid using the A* algorithm, and then a candidate trajectory set that satisfies dynamic and trailer constraints is generated in continuous space using the TEB (Timed-Elastic-Band) algorithm.

[0108] Specifically, the A* stage discretizes the environment representation into a raster graph. ,exist The above cost function Search for the path from the starting raster to the target raster, where, This represents the cumulative cost from the starting point to the current grid cell. The heuristic cost can be represented by Euclidean distance or eight-neighbor distance; simultaneously, to ensure the A* path has obstacle avoidance margin, inflated obstacle grids can be assigned a higher cost or directly deemed impassable; the global reference path output by A* is denoted as... .

[0109] Based on this, the TEB stage represents the trajectory as a pose sequence with time intervals. ,in For the first Optimize the pose of each node. Let be the time interval between nodes. The optimization objective function of TEB is defined as:

[0110]

[0111]

[0112] in, Let cost function be For constraint functions, This represents the trajectory deviation cost, used to constrain the deviation between the trajectory termination pose and the target pose; Represents the cost of static barriers; Indicates the cost of predicting dynamic obstacles; This represents the smoothing cost, used to constrain changes in velocity, acceleration, and angular velocity. This represents the time cost, used to constrain the time interval and reference speed; and Used to incorporate trailer motion constraints into the optimization process in the form of soft constraints; weights It is a non-negative scalar used to balance the contribution of cost terms.

[0113] It should be further clarified that the static obstacle cost can be directly applied to the 0.3m safety distance requirement. This applies to each trajectory node. Calculate its position in the plane. To the obstacle set minimum distance and define:

[0114] in, When the trajectory approaches the obstacle and makes... hour, Generate a penalty and propel the trajectory away from the obstacle; when This value should be zero to avoid being overly conservative and being unable to park in narrow spaces.

[0115] Furthermore, the dynamic obstacle prediction cost can be incorporated into the motion prediction information of the environment representation using step S3. For the first... Predicted location sequence of a dynamic obstacle in the prediction time domain Discrete sampling is performed, and the minimum distance from the trajectory node to the predicted trajectory of the dynamic obstacle is defined. The cost of dynamic barriers is defined as: ,in Consistent with the cost of static barriers, a predicted safety boundary is used to maintain dynamic barriers.

[0116] In some embodiments of this application, the smoothing cost can be expressed as a finite difference to represent the continuity of velocity, acceleration, and angular velocity. Define the... The linear velocity and angular velocity of the segment are as well as Then the smoothing cost can be defined as:

[0117] in, The angular velocity smoothing weight is used. The time cost can be defined as:

[0118] in, This is a reference time interval used to ensure that the trajectory time allocation is consistent with the target speed level of the traction robot.

[0119] Furthermore, trailer motion constraints are implemented in TEB as soft constraint penalties.

[0120] Specifically, the discrete recursion of the trailer traction connection angle adopts:

[0121] in, This is the distance from the towing connection point to the center of the trailer axle. For the first The traction connection angle corresponding to each node.

[0122] The soft penalty for the connection angle allowable interval is defined as follows:

[0123] The soft penalty for the allowable interval of the rate of change of the connection angle is defined as follows:

[0124] in, .pass and The permissible range of the traction connection angle and the permissible range of the rate of change of the traction connection angle can be directly embedded into the optimization process, so that the trajectory still has the controllability of towing in narrow spaces.

[0125] Furthermore, feasibility verification can be performed after TEB optimization. Verification content may include: collision verification (whether the traction robot envelope and the trailer envelope overlap during trajectory sweep). Intersection), velocity and acceleration verification ( , ), angular velocity and angular acceleration verification ( , ), and verification of the traction connection angle and the rate of change of the traction connection angle ( , ).

[0126] Candidate trajectories that pass the above feasibility verification enter the trajectory cost calculation and target trajectory selection stage. The target trajectory is obtained by optimizing the candidate trajectory cost. The trajectory cost may include the deviation cost between the candidate trajectory termination pose and the target pose, as well as the risk cost of the candidate trajectory in the environmental representation.

[0127] Specifically, the cost of deviation can be defined as:

[0128] Risk cost can be defined as the cumulative risk along the trajectory sampling points:

[0129] The total cost of candidate trajectories can be defined as , and select The smallest candidate trajectory is taken as the target trajectory, where, and The cost weight.

[0130] Furthermore, the target trajectory update is triggered by both changes in environmental representation and pose confidence. When either the environmental representation change or the pose confidence change meets preset update conditions, the latest laser point cloud information and image semantic information need to be collected and the environmental representation updated. Simultaneously, the latest satellite positioning information, ultra-wideband positioning information, and odometry information are collected and the fused pose information and pose confidence are updated to redetermine the current pose of the traction robot. Finally, a candidate trajectory set from the current pose to the target pose is regenerated and the target trajectory is updated. The preset update conditions can be defined by the occupancy change rate, risk change rate, change in the number of dynamic obstacles, or the decrease in pose confidence. During the update process, the environmental representation update and fused pose information update can be completed first, followed by two-level trajectory generation using A* and TEB. For example, the site map area can cover approximately... The target parking space can correspond to approximately Within a rectangular area, the target trajectory may include multiple turning segments to adapt to the trajectory shape of narrow parking space entrances.

[0131] Thus far, step S4 provides global connectivity and passage domain constraints through A*, and TEB achieves local smoothing and obstacle safety distance maintenance through pose-time joint optimization. The trailer motion constraints are written into the trajectory generation process in the form of soft and hard constraints of connection angle and connection angle change rate, so that the target trajectory is still feasible and executable under environmental changes or pose confidence fluctuations, providing a directly usable trajectory input for subsequent traction control command generation.

[0132] In step S5, the core task is to convert the target trajectory into executable traction control commands based on the fused pose information and traction connection angle information, and control the movement of the traction robot. When safety conditions such as slippage or idling are triggered, the system promptly switches to a more conservative control strategy and, if necessary, generates parking commands to control the traction robot to park, ensuring the controllability and safety of the traction parking process. The traction control commands may include the desired linear velocity. Expected acceleration Desired angular velocity Or equivalent left and right track / wheel speed commands; fuse pose information to calculate trajectory tracking error, and use traction connection angle information to constrain trailer attitude and suppress folding risk.

[0133] Specifically, trajectory tracking error can be defined as lateral deviation. deviation from heading ,in The normal distance from the current position of the traction robot to the nearest point on the target trajectory. , The heading is the closest point to the target trajectory; the traction connection angle deviation can be defined as... ,in The target connection angle (e.g., 0) or a reference angle that varies with the trajectory curvature can be used. Speed ​​control can employ:

[0134] Angular velocity control can be achieved by:

[0135] in The reference velocity and reference angular velocity are given for the target trajectory. To control the gain, For amplitude limiting; acceleration commands can be generated by Given. If the traction robot uses differential drive, it can... Mapped to left and right drive equivalent speed , ,in It is the equivalent wheel track or track center distance.

[0136] In some embodiments of this application, safety conditions focus on slippage and idling under deteriorated adhesion, and are quantifiable triggers achieved through the deviation between desired and actual motion. Specifically, safety conditions can include slippage and idling. Desired velocity and displacement can be generated based on wheel speed information and motor speed information, combined with traction robot parameters. Actual velocity and displacement are generated based on fused pose information. The velocity deviation between the desired and actual velocities, and the displacement deviation between the desired and actual displacements, are calculated. A slippage or idling state is determined when the velocity or displacement deviation meets preset judgment conditions. Wheel speed information can provide the equivalent angular velocity of the wheel / track, and motor speed information can provide the motor output shaft speed. Combined with traction robot parameters (e.g., wheel radius)... Reduction ratio The desired velocity and desired displacement can be derived from these parameters.

[0137] Specifically, the desired speed can be defined as , The angular velocity of the motor is given; the desired displacement can be defined as... The actual velocity can be calculated from the positional changes of the fused pose in adjacent time steps: ,in, The actual displacement can be defined as: Speed ​​deviation is The displacement deviation is Based on this, when or And continue If the time exceeds a certain threshold, it can be determined that the engine has entered a slipping or idling state. The deviation threshold, The duration threshold is used to distinguish between slippage and idling. This can also be achieved by combining the trend of motor load current or wheel speed changes, but it does not affect the basic closed loop of "safety condition triggering".

[0138] In some embodiments of this application, the system does not stop immediately after a safety condition is triggered. Instead, the control strategy is switched to angle suppression to prioritize stabilizing the traction connection angle and reduce the risk of folding. During this process, lateral deviation and heading deviation can be generated based on the target trajectory and fused pose information, connection angle deviation can be generated based on the traction connection angle information, and a control strategy identifier including a conventional tracking strategy identifier and an angle suppression strategy identifier can be generated based on the lateral deviation, heading deviation, and connection angle deviation. The lateral deviation and heading deviation can be respectively taken as... and Connecting angle deviation is taken Manipulation strategy identifiers can be discrete variables. express, Corresponding to the standard tracking strategy identifier, Corresponding to the angle suppression strategy identifier; in the conventional tracking strategy, the control law minimizes... In the angle suppression strategy, the control law aims to minimize... And the main focus is on tightening constraints.

[0139] Furthermore, angle suppression not only adjusts the control gain but also requires updating the boundaries of speed, acceleration, and steering rate of change, and simultaneously tightening the permissible range in the trailer motion constraints. This reduces the overall control capability to a lower order to maintain a safety margin when adhesion deteriorates. In slippage or idling states, the handling strategy identifier can be set as the angle suppression strategy identifier, and the upper bounds of speed and acceleration in the traction control command can be updated. Constraints can be applied to the upper bound of the steering rate of change in the traction control command, and the permissible range of the traction connection angle in the trailer motion constraints can be tightened. Specifically, the speed upper bound can be updated to... The upper bound of acceleration is updated to , The tightening coefficient; the upper bound of the rate of change of direction can be obtained by... Constraint fulfillment , The permissible range for the traction connection angle can be tightened to: ,in , , To tighten the margin, and can be tightened in conjunction with other measures. The permissible interval is used to suppress the tendency for rapid folding. The control law under the angle suppression strategy can be rewritten as:

[0140] in, To enhance the suppression of connection angles.

[0141] Furthermore, the triggering of safety conditions can employ continuous determination to avoid false triggering due to short-term noise. That is, the safety condition is only triggered when a preset continuous determination condition is consistently met during slippage or idling. The preset continuous determination condition can be defined as: [condition with a length of...]. Within the time window, satisfy or The proportion is no less than ,in This is the proportional threshold; or defined as continuous. Each cycle meets the deviation threshold. Upon triggering, the "Generate Parking Command" link can be entered to execute a safe stop.

[0142] In some embodiments of this application, the parking command is not a single command, but a state transition process driven by a set of control states. This ensures a consistent and traceable degradation path under different fault levels and maintains a safe stop until the unlocking conditions are met. During this process, a set of control states, including automatic control state, degraded control state, and safe stop state, can be generated and transitioned between them. The control state set can utilize state variables. express, In automatic control mode, The control state has been downgraded. The state is a safe parking state; the state transition conditions can be jointly determined by the safety condition trigger, the control consistency verification result, and the duration determination.

[0143] Specifically, in automatic control mode, trajectory control commands can be generated based on the target trajectory, and safety conditions can be used as parallel constraints to generate safety control commands, so that consistency checks can capture conflicts when they occur. The trajectory control commands can be selected from... or Safety control commands can be in the form of "permitted" or "limited" when no safety condition is triggered, and in the form of "strong limit / parking preparation" after the safety condition is triggered. For example, [the command could be set to...]. The clamp is set to 0, and the electronic parking actuator is instructed to enter the ready state. For example, when the gradient is greater than 15% or the battery level is less than 15%, the safety control command can be set to a more conservative limit or return / stop strategy, but the state machine output should be taken into account to maintain the logic closure.

[0144] Furthermore, to avoid conflicting commands from trajectory control and safety control within the same cycle, a consistency check can be performed on the trajectory control and safety control commands, generating a check result. If the check result meets a preset failure condition, a degraded control state is entered. This consistency check may include: velocity direction consistency (the consistency between the two...). The signs are consistent or the difference does not exceed the threshold. ), and the consistency of the turning trend ( The difference does not exceed the threshold ) and the consistency of the connection angle boundary (implied by trajectory control) (Predictions do not violate the tightened permit range). For example, a preset failure condition can be defined as: or Or predict the connection angle to go out of bounds.

[0145] After entering the degraded control state, the upper limit of speed, the upper limit of acceleration, and the permissible range of traction connection angle are tightened in the manner described above. The control strategy when the pose confidence is insufficient can be switched to short-term tracking that relies more on the continuity of the odometer, so as to reduce the direct impact of external positioning jitter on the control.

[0146] In some embodiments of this application, when the degraded control state meets the preset exit conditions, or when the safety conditions continue to exist, it is necessary to enter the safe parking state and issue a parking command, while maintaining the parking command until the preset unlocking conditions are met, thereby ensuring that movement does not resume before user intervention or fault resolution.

[0147] For example, preset exit conditions may include: the security condition lasting for more than [time period missing]. The pose confidence level is consistently lower than Or, consistency checks fail for more than [number] consecutive times. Next; parking instructions may include... , It will trigger the electronic or mechanical parking actuator to lock, and may also trigger an audible and visual alarm or remote prompt; preset unlocking conditions may include: the safety condition being released and continuing Time and pose confidence recovery to The above conditions apply, and the rate of change in the environment is below the threshold, and the conditions for manual unlocking or remote confirmation are met.

[0148] Thus, step S5, through the closed loop of "trajectory tracking control - safety condition determination - angle suppression and amplitude limiting - consistency verification and degradation - safe parking maintenance", ensures that the traction robot can still achieve controlled traction and safe parking under complex occlusion, attachment changes and dynamic obstacle conditions, forming a reproducible and engineering-applicable intelligent perception and control process.

[0149] In summary, this method integrates satellite positioning, ultra-wideband positioning, and inertial wheel speed odometer for consistency verification and confidence weighting, and performs degraded fusion that can be eliminated when pose confidence decreases. It combines laser point cloud and image semantics to construct an environmental representation that includes dynamic prediction. Under the constraints of trailer motion, it drives the generation of traction control commands through target trajectory optimization and real-time updates. This enables robust handling of obstructions, multipaths, slippage, and dynamic obstacles in towed scenarios. It can maintain a 0.3m safety margin and controllable boundary of connection angle in narrow camp parking spaces, reduce the risk of trajectory jitter and folding, reduce the probability of multiple manual corrections and parking failures, and improve the success rate of automatic parking, traffic safety, and user experience consistency.

[0150] It should be noted that, although the embodiments in this application are based on... Figure 1 Steps S1 to S5 are described sequentially, but this does not mean that steps S1 to S5 must be performed in a strict order. The reason this embodiment follows this order is... Figure 1 The order in which steps S1 to S5 are described is provided to facilitate understanding of the technical solutions of the embodiments of this application by those skilled in the art. In other words, in the embodiments of this application, the order of steps S1 to S5 can be appropriately adjusted according to actual needs.

[0151] In some embodiments of this application, a towing robot intelligent perception and control method is applied to the automatic parking scenario of a campervan at a campground. The towing robot collects satellite positioning, ultra-wideband positioning, inertial measurement, wheel speed / motor speed, laser point cloud and image semantics, and stores them in a global coordinate system. Lower output fused pose pose confidence With traction connection angle This completes a full towing and parking process from the starting position to the target parking position.

[0152] The complete implementation process may include the following steps: Step 1: Receive target parking information Load the parameters of the traction robot and the trailer and initialize them. and .

[0153] Step 2: Generate trailer motion constraints based on trailer parameters ,Sure and .

[0154] Step 3, within the time window Internal computation Construction residuals And obtained Normalization yields ,renew And generate ;when Remove at time The information sources were then re-integrated.

[0155] Step 4: Generate an obstacle set based on laser point cloud and image semantics. , construct environment representation ,in Used to generate static risk; performs data correlation on adjacent time points and updates dynamic obstacle predictions, and writes them. .

[0156] Step 5, in Generate and optimize target trajectory under environmental representation constraints and in the context of change or Update when changes meet the conditions .

[0157] Step Six, based on , and Generate traction control commands And execute; with , Determine if slippage / idle spin occurs and trigger the angle suppression strategy to tighten. and If necessary, a parking command will be generated to complete the safe parking process.

[0158] Through the complete implementation process described above, this method uses consistency verification and confidence weighting fusion to suppress positioning anomalies caused by occlusion, multipath, and attachment changes. It also uses pose confidence-driven degradation fusion to avoid distortion sources contaminating the control input. Combined with the environmental representation written by dynamic obstacle prediction and trajectory optimization and angle suppression control under traction connection angle constraints, this method can maintain a safety margin and controllable boundary of connection angle even in narrow spaces, reducing the risk of folding and parking failures, reducing the number of manual corrections, and improving the success rate of automatic parking, process stability, and operational safety.

[0159] It should be understood that the step numbers identified by "Step 1, Step 2" and other similar forms in the above embodiments are only used to distinguish different steps and do not limit the steps to be executed in the order of these numbers. The specific execution order of each step can be adjusted according to its functional requirements and the inherent logic in the actual application scenario. The above step numbers should not be interpreted as a limitation on the implementation process of the embodiments of this application.

[0160] like Figure 2 As shown, the following is an embodiment of an intelligent sensing control system for a traction robot provided by this disclosure. This intelligent sensing control system for a traction robot belongs to the same inventive concept as the intelligent sensing control methods for traction robots in the above embodiments. For details not described in detail in the embodiments of the intelligent sensing control system for a traction robot, please refer to the embodiments of the intelligent sensing control methods for traction robots described above.

[0161] Based on the same concept, another embodiment of this application provides an intelligent perception and control system for a traction robot, comprising: The constraint generation unit 1 is used to acquire the parameters of the traction robot, the parameters of the trailer, and the traction connection angle information, and to generate the motion constraints of the trailer. The pose fusion unit 2 is used to collect satellite positioning information and ultra-wideband positioning information, collect inertial measurement information and wheel speed information and generate odometer information, perform consistency checks on satellite positioning information, ultra-wideband positioning information and odometer information, calculate satellite positioning confidence, ultra-wideband positioning confidence and odometer confidence, and fuse them to obtain fused pose information and pose confidence. When the pose confidence is lower than the first preset threshold, downgrade fusion is performed. Downgrade fusion includes removing positioning information with confidence lower than the second preset threshold and re-fusing. Environment construction unit 3 is used to generate obstacle information and construct environment representation based on laser point cloud information and image semantic information, determine and update dynamic obstacle information based on obstacle information at adjacent acquisition times, and write dynamic obstacle information into environment representation; The trajectory generation unit 4 is used to generate a target trajectory based on fused pose information under environmental representation and trailer motion constraints, and to update the target trajectory when the environmental representation changes or the pose confidence changes. The control output unit 5 is used to generate traction control commands and control the movement of the traction robot based on the target trajectory, fused pose information and traction connection angle information, and to generate parking commands and control the traction robot to park when safety conditions are triggered.

[0162] By adopting the above technical solution, the constraint generation unit 1 acquires the parameters of the traction robot, the trailer, and the traction connection angle information, and generates the trailer motion constraints. This ensures that the target trajectory generation and traction control command output are executed within the kinematic boundaries of the trailer, improving the feasibility and attitude stability of the combined motion of the traction robot and the trailer, and reducing the operational risks caused by traction connection angle exceeding the boundary or attitude divergence. The pose fusion unit 2 performs consistency checks on satellite positioning information, ultra-wideband positioning information, and odometer information, quantifies their respective credibility, and then fuses them to obtain fused pose information and pose confidence. This gives the pose output a measurable reliability characterization, facilitating planning and control to respond to changes in positioning quality. At the same time, when the pose confidence is below the threshold, a degradation fusion mechanism is implemented to remove low-confidence information and re-fuse it, improving the continuity and anti-interference capability of the pose output under conditions such as occlusion, multipath, and short-term drift, and reducing the impact of single positioning source anomalies on the system control link.

[0163] Furthermore, the environment construction unit 3 generates obstacle information and constructs an environment representation based on laser point cloud information and image semantic information. It also updates dynamic obstacle information by combining obstacle information from adjacent acquisition times and writes it into the environment representation, enabling the environment representation to reflect dynamic target state changes and improving the timeliness and consistency of obstacle constraints. The trajectory generation unit 4 updates the target trajectory when the environment representation changes or the pose confidence changes, enabling the target trajectory to be adaptively updated with changes in environment and positioning reliability, reducing the risk of planning deviation under trajectory lag or positioning degradation conditions and improving the stability of risk margin. The control output unit 5 generates traction control commands based on the target trajectory, fused pose information and traction connection angle information, and controls the movement of the traction robot, so that the traction control is simultaneously constrained by the trajectory and connection angle state to maintain closed-loop consistency. When safety conditions are triggered, a parking command is generated and parking is controlled, enabling the system to have a closed-loop safety mechanism of "planning-control-safe handling" linkage, improving the consistency of controlled shutdown and overall safety under abnormal working conditions.

[0164] In this embodiment of the application, the system may further include a security determination unit, used for: Based on wheel speed information and motor speed information, combined with traction robot parameters, the desired speed and desired displacement are generated. Based on the fused pose information, the actual speed and actual displacement are generated. The speed deviation between the desired speed and the actual speed, as well as the displacement deviation between the desired displacement and the actual displacement, are calculated. When the speed deviation or displacement deviation meets the preset judgment conditions, the slippage state or the freewheeling state is determined. Lateral deviation and heading deviation are generated based on the target trajectory and fused pose information. Connection angle deviation is generated based on the traction connection angle information. Control strategy identifiers are generated based on lateral deviation, heading deviation and connection angle deviation. Control strategy identifiers include conventional tracking strategy identifiers and angle suppression strategy identifiers. In the slipping or idling state, the control strategy identifier is determined as the angle suppression strategy identifier, and the constraint update of the upper limit of speed, upper limit of acceleration and upper limit of steering change rate of the traction control command is triggered. At the same time, the tightening of the allowable range of traction connection angle of trailer motion constraint is triggered.

[0165] In some embodiments of this application, the trajectory generation unit 4 can also be used for: The current pose of the traction robot is determined based on the fused pose information, and the target pose is determined based on the target parking information. Generate accessible area information and obstacle constraint information based on environmental representation; Under the constraints of passable area information, obstacle constraint information, and trailer motion constraint, a set of candidate trajectories from the current pose to the target pose is generated, and the feasibility of the candidate trajectory set is checked. The trajectory cost is calculated for the candidate trajectories that pass the feasibility check and the target trajectory is selected. The trajectory cost includes the deviation cost between the candidate trajectory termination pose and the target pose, and the risk cost of the candidate trajectory in the environmental representation. When changes in environmental representation or pose confidence meet preset update conditions, the latest laser point cloud information and image semantic information are collected and the environmental representation is updated. The latest satellite positioning information, ultra-wideband positioning information and odometer information are collected and the fused pose information and pose confidence are updated. The current pose of the traction robot is re-determined, and a new set of candidate trajectories from the current pose to the target pose is generated and the target trajectory is updated.

[0166] In summary, this system, through the collaborative efforts of constraint generation unit 1, pose fusion unit 2, environment construction unit 3, trajectory generation unit 4, and control output unit 5, completes the following: generating trailer motion constraints; verifying and fusing consistency between satellite positioning / ultra-wideband / odometer data; constructing environment representation based on point cloud and image semantics and updating dynamic obstacles; and adaptively updating the target trajectory under environmental changes or pose confidence changes. This enables constrained planning and closed-loop traction control of the traction-trailer combination in confined spaces. It improves the reliability and continuity of pose output and reduces the impact of single positioning source anomalies on the control link, while also enhancing the timeliness of obstacle constraints and the stability of trajectory risk margin in dynamic scenarios. Furthermore, by triggering angle suppression strategies and updating speed, acceleration, and steering rate of change constraints through slippage / idleness judgment, it further improves controllability and consistency of safe handling under abnormal conditions, thereby enhancing the stability and safety of traction displacement and parking operations.

[0167] The above-disclosed embodiments are merely preferred embodiments of the present invention, but the present invention is not limited thereto. Any non-creative variations that can be conceived by those skilled in the art, as well as any improvements and modifications made without departing from the principles of the present invention, should fall within the protection scope of the present invention.

Claims

1. A method for intelligent perception and control of a traction robot, characterized in that, include: Acquire the parameters of the traction robot, the trailer parameters, and the traction connection angle information, and generate the trailer motion constraints; Satellite positioning information and ultra-wideband positioning information are collected, as well as inertial measurement information and wheel speed information, and odometer information is generated. Consistency checks are performed on the satellite positioning information, ultra-wideband positioning information, and odometer information. The reliability of satellite positioning, ultra-wideband positioning, and odometer information are calculated, and fused to obtain fused pose information and pose confidence. When the pose confidence is lower than the first preset threshold, downgraded fusion is performed. Downgraded fusion includes removing positioning information with confidence lower than the second preset threshold and re-fusing. Obstacle information is generated based on laser point cloud information and image semantic information, and an environment representation is constructed. Dynamic obstacle information is determined and updated based on obstacle information at adjacent acquisition times, and the dynamic obstacle information is written into the environment representation. Based on fused pose information, a target trajectory is generated under environmental representation and trailer motion constraints, and the target trajectory is updated when the environmental representation changes or the pose confidence changes. Based on the target trajectory, fused pose information and traction connection angle information, traction control commands are generated and the traction robot is moved. When safety conditions are triggered, parking commands are generated and the traction robot is parked. The steps for generating trailer motion constraints include: Determine the geometric relationships of the traction connection point and the trailer geometry based on the trailer parameters; Based on the traction connection angle information, a geometric consistency check is performed on the geometric relationship of the traction connection point and the trailer geometric relationship. If the geometric consistency check fails, the geometric relationship of the traction connection point or the trailer geometric relationship is corrected to a geometric relationship that meets the geometric consistency check requirements according to the preset fault tolerance rules. Based on the geometric relationship of the traction connection point after the geometric consistency check is passed, the geometric relationship of the trailer and the parameters of the traction robot are used to calculate the permissible range of the traction connection angle and the permissible range of the rate of change of the traction connection angle, and to generate the motion constraints of the trailer.

2. The intelligent perception and control method for a traction robot as described in claim 1, characterized in that, The steps of performing consistency checks on satellite positioning information, ultra-wideband positioning information, and odometry information, calculating the confidence levels of satellite positioning, ultra-wideband positioning, and odometry, and fusing them to obtain fused pose information and pose confidence include: Within a preset time window, the odometer displacement increment and odometer heading increment are calculated based on odometer information, the satellite displacement increment and satellite heading increment are calculated based on satellite positioning information, and the ultra-wideband displacement increment and ultra-wideband heading increment are calculated based on ultra-wideband positioning information. The difference between the odometer displacement increment and the satellite displacement increment is calculated and denoted as the first displacement residual. The difference between the odometer heading increment and the satellite heading increment is calculated and denoted as the first heading residual. Based on the first displacement residual and the first heading residual, the satellite consistency test result is generated and the satellite positioning reliability is determined. The difference between the odometer displacement increment and the ultra-wideband displacement increment is calculated and denoted as the second displacement residual. The difference between the odometer heading increment and the ultra-wideband heading increment is calculated and denoted as the second heading residual. Based on the second displacement residual and the second heading residual, the ultra-wideband consistency test result is generated and the reliability of ultra-wideband positioning is determined. Based on the first displacement residual, the second displacement residual, the first heading residual, and the second heading residual, the odometer consistency test results are generated and the odometer reliability is determined. The satellite positioning reliability, ultra-wideband positioning reliability, and odometer reliability are normalized to obtain the satellite fusion weight, ultra-wideband fusion weight, and odometer fusion weight. The satellite displacement increment, ultra-wideband displacement increment, and odometer displacement increment are weighted and fused to obtain the fused displacement increment. The satellite heading increment, ultra-wideband heading increment, and odometer heading increment are weighted and fused to obtain the fused heading increment. The fused pose information is updated based on the fused displacement increment and fused heading increment. The pose confidence is generated based on the first displacement residual, the second displacement residual, the first heading residual, the second heading residual, the satellite positioning confidence, the ultra-wideband positioning confidence, and the odometer confidence.

3. The intelligent perception and control method for a traction robot as described in claim 1, characterized in that, The steps of determining and updating dynamic obstacle information based on obstacle information at adjacent acquisition times, and writing the dynamic obstacle information into the environment representation, include: Perform data association on obstacle information from adjacent acquisition times and generate obstacle association results; Based on the obstacle association results, the obstacle motion state is generated, and dynamic obstacle information is determined and updated. Motion prediction information is generated based on dynamic obstacle information. The dynamic obstacle information and motion prediction information are written into the environmental representation and participate in the triggering determination of environmental representation changes.

4. The intelligent perception and control method for a traction robot as described in claim 1, characterized in that, The steps for generating a target trajectory based on fused pose information, under environmental representation and trailer motion constraints, and updating the target trajectory when the environmental representation changes or the pose confidence changes, include: The current pose of the traction robot is determined based on the fused pose information, and the target pose is determined based on the target parking information. Generate accessible area information and obstacle constraint information based on environmental representation; Under the constraints of passable area information, obstacle constraint information, and trailer motion constraint, a set of candidate trajectories from the current pose to the target pose is generated, and the feasibility of the candidate trajectory set is checked. The trajectory cost is calculated for the candidate trajectories that pass the feasibility check and the target trajectory is selected. The trajectory cost includes the deviation cost between the candidate trajectory termination pose and the target pose, and the risk cost of the candidate trajectory in the environmental representation. When changes in environmental representation or pose confidence meet preset update conditions, the latest laser point cloud information and image semantic information are collected and the environmental representation is updated. The latest satellite positioning information, ultra-wideband positioning information and odometer information are collected and the fused pose information and pose confidence are updated. The current pose of the traction robot is re-determined, and a new set of candidate trajectories from the current pose to the target pose is generated and the target trajectory is updated.

5. The intelligent perception and control method for a traction robot as described in claim 1, characterized in that, The safety conditions include slippage and freewheeling, and the determination and triggering of the safety conditions include: Based on wheel speed information and motor speed information, combined with traction robot parameters, the desired speed and desired displacement are generated. Based on the fused pose information, the actual speed and actual displacement are generated. The speed deviation between the desired speed and the actual speed, as well as the displacement deviation between the desired displacement and the actual displacement, are calculated. When the speed deviation or displacement deviation meets the preset judgment conditions, the slippage state or the freewheeling state is determined. Lateral deviation and heading deviation are generated based on the target trajectory and fused pose information. Connection angle deviation is generated based on the traction connection angle information. Control strategy identifiers are generated based on lateral deviation, heading deviation and connection angle deviation. Control strategy identifiers include conventional tracking strategy identifiers and angle suppression strategy identifiers. In the slipping or idling state, the control strategy identifier is determined as the included angle suppression strategy identifier and the upper limit of speed and acceleration of the traction control command is updated. The upper limit of the steering rate of change of the traction control command is constrained and the allowable range of the traction connection angle of the trailer motion constraint is tightened. The safety condition is triggered when the preset continuous judgment condition is met continuously in the slipping or idling state.

6. The intelligent perception and control method for a traction robot as described in claim 5, characterized in that, The steps for generating a parking command include: A set of control states is generated and state transitions are performed. The set of control states includes automatic control state, degraded control state, and safe stop state. In automatic control mode, trajectory control commands are generated based on the target trajectory, and safety control commands are generated based on safety conditions; The trajectory control command and safety control command are subjected to consistency verification and verification results are generated. When the verification results meet the preset failure conditions, the control state is degraded and the upper limit of speed, upper limit of acceleration and permissible range of traction connection angle are tightened. When the preset exit conditions are met in the downgraded control state, the vehicle enters the safe parking state and generates a parking command. The parking command is maintained in the safe parking state until the preset unlocking conditions are met.

7. An intelligent sensing and control system for a traction robot, characterized in that, include: The constraint generation unit is used to acquire the parameters of the traction robot, the parameters of the trailer, and the traction connection angle information, and to generate the motion constraints of the trailer. The pose fusion unit is used to collect satellite positioning information and ultra-wideband positioning information, collect inertial measurement information and wheel speed information and generate odometer information, perform consistency checks on satellite positioning information, ultra-wideband positioning information and odometer information, calculate satellite positioning confidence, ultra-wideband positioning confidence and odometer confidence, and fuse them to obtain fused pose information and pose confidence. When the pose confidence is lower than a first preset threshold, downgraded fusion is performed. Downgraded fusion includes removing positioning information with confidence lower than a second preset threshold and re-fusing. The environment construction unit is used to generate obstacle information and construct an environment representation based on laser point cloud information and image semantic information, determine and update dynamic obstacle information based on obstacle information at adjacent acquisition times, and write the dynamic obstacle information into the environment representation. The trajectory generation unit is used to generate a target trajectory based on fused pose information, under environmental representation and trailer motion constraints, and to update the target trajectory when the environmental representation changes or the pose confidence changes. The control output unit is used to generate traction control commands and control the movement of the traction robot based on the target trajectory, fused pose information and traction connection angle information, and to generate parking commands and control the traction robot to park when safety conditions are triggered. The generated trailer motion constraints include: Determine the geometric relationships of the traction connection point and the trailer geometry based on the trailer parameters; Based on the traction connection angle information, a geometric consistency check is performed on the geometric relationship of the traction connection point and the trailer geometric relationship. If the geometric consistency check fails, the geometric relationship of the traction connection point or the trailer geometric relationship is corrected to a geometric relationship that meets the geometric consistency check requirements according to the preset fault tolerance rules. Based on the geometric relationship of the traction connection point after the geometric consistency check is passed, the geometric relationship of the trailer and the parameters of the traction robot are used to calculate the permissible range of the traction connection angle and the permissible range of the rate of change of the traction connection angle, and to generate the motion constraints of the trailer.

8. The intelligent perception and control system for the traction robot as described in claim 7, characterized in that, The system further includes a security determination unit, which is used for: Based on wheel speed information and motor speed information, combined with traction robot parameters, the desired speed and desired displacement are generated. Based on the fused pose information, the actual speed and actual displacement are generated. The speed deviation between the desired speed and the actual speed, as well as the displacement deviation between the desired displacement and the actual displacement, are calculated. When the speed deviation or displacement deviation meets the preset judgment conditions, the slippage state or the freewheeling state is determined. Lateral deviation and heading deviation are generated based on the target trajectory and fused pose information. Connection angle deviation is generated based on the traction connection angle information. Control strategy identifiers are generated based on lateral deviation, heading deviation and connection angle deviation. Control strategy identifiers include conventional tracking strategy identifiers and angle suppression strategy identifiers. In the slipping or idling state, the control strategy identifier is determined as the angle suppression strategy identifier, and the constraint update of the upper limit of speed, upper limit of acceleration and upper limit of steering change rate of the traction control command is triggered. At the same time, the tightening of the allowable range of traction connection angle of trailer motion constraint is triggered.

9. The intelligent perception and control system for the traction robot as described in claim 7, characterized in that, The trajectory generation unit is also used for: The current pose of the traction robot is determined based on the fused pose information, and the target pose is determined based on the target parking information. Generate accessible area information and obstacle constraint information based on environmental representation; Under the constraints of passable area information, obstacle constraint information, and trailer motion constraint, a set of candidate trajectories from the current pose to the target pose is generated, and the feasibility of the candidate trajectory set is checked. The trajectory cost is calculated for the candidate trajectories that pass the feasibility check and the target trajectory is selected. The trajectory cost includes the deviation cost between the candidate trajectory termination pose and the target pose, and the risk cost of the candidate trajectory in the environmental representation. When changes in environmental representation or pose confidence meet preset update conditions, the latest laser point cloud information and image semantic information are collected and the environmental representation is updated. The latest satellite positioning information, ultra-wideband positioning information and odometer information are collected and the fused pose information and pose confidence are updated. The current pose of the traction robot is re-determined, and a new set of candidate trajectories from the current pose to the target pose is generated and the target trajectory is updated.

Images