Navigation control method for factory unmanned tractor

Abstract

The application discloses a navigation control method of a factory unmanned tractor, comprising the following steps: S1, constructing a two-dimensional grid map of a work area based on a laser SLAM technology; S2, marking exclusive semantic information of the tractor in the two-dimensional grid map; S3, acquiring initial state information of the tractor; S4, inputting a starting point and an ending point in the two-dimensional grid map, planning a running path of the tractor, and guiding the tractor to run; S5, in the running process of the tractor, a data sensing device collects multi-source data, analyzes and optimizes the running path; and S6, judging whether the tractor reaches the ending point, if not, returning to the step S5, if yes, executing unloading or waiting for a new task instruction, and returning to the step S3 after receiving the new task. The application realizes zero-reformation deployment, significantly improves positioning robustness, load adaptability and man-machine mixed running safety, and solves technical problems of traditional tractors, such as environment reformation dependence, easy rule violation and overload, and hook hanging and damage.

Description

Technical Field

[0001] This invention relates to the field of autonomous driving and intelligent manufacturing logistics equipment technology, and more specifically, to a navigation and control method for an unmanned tractor for factory use. Background Technology

[0002] To adapt to rapidly changing end-market demands, factories need to establish flexible production capabilities that allow for quick production line switching and the provision of small-batch, multi-variety production. In this process, efficient parts delivery is a key supporting element for achieving production flexibility and rapid response capabilities.

[0003] Currently, most factories still primarily use a delivery method where parts are placed on different trailers and multiple trailers are towed by manually driven tractors. This method requires repetitive, simple labor 24 / 7, leading to extremely high labor intensity and easy fatigue for workers. More seriously, there are numerous safety risks on-site due to non-compliance with safe delivery regulations. For example, in order to carry more goods in a single trip, some factories illegally tow more trailers than the safety standard, resulting in severely oversized tractor trains and significant blind spots and the risk of inner wheel collisions when turning.

[0004] Furthermore, existing manually driven tractors and conventional unmanned guided vehicles (AGVs) suffer from the following technical deficiencies: First, some existing industrial AGVs rely heavily on magnetic strips or QR codes affixed to the ground or walls for navigation indoors, requiring extensive on-site environmental modifications and exhibiting poor flexibility, making them unsuitable for the frequent layout adjustments required by modern dynamic factory environments. Second, factory environments involve frequent switching between lightly loaded (empty) and heavily loaded (e.g., fully loaded with 2 tons of goods), often resulting in frequent start-stop operations. Conventional tractors have simplistic control logic, making them prone to tail-wagging, poor path tracking accuracy, and insufficient driving stability when turning under heavy loads. Third, the dynamic industrial environment is characterized by the interplay and interference of personnel, forklifts, and temporarily placed materials. Conventional tractors lack predictive multi-source perception fusion mechanisms, resulting in untimely obstacle avoidance and severely compromised safety when operating alongside humans. Fourth, most existing hook-and-unhook mechanisms are rigid cylinder connections. When faced with load fluctuations, uneven ground causing the trailer hook to tilt or misalign, they are prone to mechanical jamming and uneven force distribution, ultimately leading to deformation and damage to the traction mechanisms of the tractor and trailer, resulting in extremely low reliability and versatility in logistics operations. Summary of the Invention

[0005] The purpose of this invention is to provide a navigation and control method for unmanned towing vehicles in factories, aiming to solve the technical problems existing in current factory logistics and distribution, such as personnel shortage, heavy reliance on environmental modifications, poor path tracking stability, easy jamming during hooking and unhooking, low safety due to mixed human and machine traffic, and difficulty in controlling illegal and excessive towing.

[0006] To achieve the above objectives, the present invention provides a navigation control method for an unmanned tractor used in a factory, comprising the following steps: Step S1: Construct a two-dimensional grid map of the work area based on laser SLAM technology; Step S2: Label the tractor's specific semantic information onto the two-dimensional grid map; Step S3: Obtain the initial status information of the tractor. Step S4: Input the starting point and the ending point into the two-dimensional grid map, plan the running path of the tractor based on the tractor's specific semantic information and the tractor's initial state information, and then guide the tractor to run according to the planned running path. Step S5: During the operation of the tractor, the data sensing device installed on the tractor collects multi-source data, analyzes the multi-source data, and optimizes the tractor's operating path. Step S6: Determine whether the tractor has reached the destination. If it has not reached the destination, return to step S5. If it has reached the destination, perform unloading or wait for a new task instruction, and return to step S3 after receiving the new task.

[0007] Preferably, in step S2, the tractor's specific semantic information includes docking location, restricted area, and speed limit area.

[0008] Preferably, in step S3, the initial state information of the tractor includes: the location information of the tractor in the two-dimensional grid map, whether the tractor is currently carrying a trailer, and the battery power status of the tractor.

[0009] 4. The navigation control method according to claim 3, characterized in that, when the status information of the tractor vehicle not currently carrying a trailer is obtained, and a task to tract a trailer to the docking position is received, the tractor vehicle performs an automatic trailer attachment / unattachment step, specifically as follows: Step S31: Guide the tractor to the docking position; Step S32: When the position detection switch on the tractor detects the position signal of the trailer hook ring, the mechanical connection action between the tractor's hook-and-unhook mechanism and the trailer is executed. Step S33: When the position feedback of the traction pin in the hook-and-unhook mechanism is fully extended and the locking buckle position switch triggers the locked signal, the connection is confirmed and proceed to step S4.

[0010] More preferably, step S32 further includes acquiring the feedback value of the pressure sensor installed on the hook-and-unhook mechanical mechanism in real time, and determining whether the feedback value is greater than the first jamming threshold. If the feedback value is less than or equal to the first jamming threshold, continue to execute the mechanical connection action until the trailer is loaded. If the feedback value is greater than the first jamming threshold, the mechanical connection action is stopped, and a flexible adjustment anti-jamming procedure is performed, specifically as follows: Step S321: Control the tractor to move forward and backward, and simultaneously control the floating base of the hook-and-unhook mechanism to perform horizontal displacement compensation, and control the tractor to perform yaw angle swing adjustment. Step S322: Repeatedly execute the fine-tuning action of step S321 and retry the mechanical connection until the feedback value is less than or equal to the second jamming threshold and remains stable. Then exit the flexible adjustment anti-jamming step and return to step S32. The first stagnation threshold is greater than the second stagnation threshold.

[0011] Preferably, the specific steps for planning the running path in step S4 are as follows: Step S41: Receive the start point and the end point; Step S42, using an improved The algorithm introduces turning costs and reversing penalties to generate an initial running path that satisfies the minimum turning radius.

[0012] Preferably, the data sensing device includes a lidar and an industrial vision camera mounted on the tractor. In step S5, the specific steps for optimizing the running path are as follows: Step S501: During the operation of the tractor, the point cloud data obtained by the LiDAR scan is matched with the pre-built high-precision two-dimensional grid map to provide a global stable pose reference. Step S502: Use an industrial vision camera to extract the natural inherent environmental features in the work area and construct a multi-scale semantic feature library; add new natural inherent environmental features in real time to dynamically update the two-dimensional grid map. Step S503: By using an improved adaptive ORB feature matching algorithm, integrating dynamic illumination suppression and motion blur compensation mechanisms, and combining multi-level RANSAC geometric verification to eliminate dynamic interference, visual observation and laser odometry are tightly coupled and nonlinearly optimized to improve the running path, correct laser drift, and output positioning information.

[0013] Preferably, the data sensing device includes an inertial measurement unit and a wheel speed-odometer; Step S501 also establishes a mechanism for a multi-source fusion Kalman filter framework, specifically as follows: Step S5011: Real-time monitoring of the number of feature matches and the confidence level of the localization covariance output by the lidar; Step S5012: When the number of feature matches is lower than the first preset threshold and the confidence level of the positioning covariance is lower than the second preset threshold, it is determined that the laser is short-term failure and the system switches to the redundant calculation mode that integrates vision, inertial measurement unit and wheel speed odometer. Step S5013: Using the high-frequency angular velocity and acceleration data of the inertial measurement unit as input, the kinematic model is used to perform time updates, and the prior state and covariance of the next moment are calculated. Step S5014 introduces multi-source observations for measurement updates: the visual odometry solves relative pose changes through feature matching and provides lateral position constraints to correct cumulative drift; the wheel speed odometry combines the steering angle model to provide longitudinal displacement and absolute scale information to suppress error divergence caused by slippage; and the inertial measurement unit continuously provides attitude angle and angular velocity observations under high dynamic conditions.

[0014] Preferably, step S5 further includes a step of monitoring whether the trailer attached to the tractor is overloaded, specifically: Step S511: Invoke the pre-established trailer self-learning model based on the object detection architecture; Step S512: Receive images acquired in real time by an industrial vision camera, output trailer bounding boxes and category confidence scores; combine multi-target tracking algorithms, use Kalman filtering to predict trajectories and use the Hungarian algorithm to match targets in previous and next frames, and identify and count the number of trailers currently being towed in real time. Step S513: When the number of currently towed trailers exceeds the set number threshold in multiple consecutive frames, a warning state is triggered, the alarm device is activated to issue a warning and execute intervention control.

[0015] Preferably, step S5 further includes a hierarchical human-machine hybrid safety control step based on a predictive model, specifically: a spatiotemporal synchronization fusion mechanism based on timestamp alignment and coordinate transformation is used to fuse two-dimensional environmental scanning data from lidar, obstacle category data from industrial vision cameras, and near-range blind spot detection data from ultrasonic sensors; multi-source confidence is fused using DS evidence theory to output comprehensive obstacle information; a safety model is established that includes a trajectory prediction model, a risk assessment model, and a decision model; a long short-term memory network is used to learn the movement patterns of abnormally approaching personnel or objects and predict their movement trajectories within a set time period in the future; hierarchical predictive safety control is executed: wherein, a first distance threshold is greater than a second distance threshold; when the predicted relative distance is greater than the first distance threshold, the vehicle speed is dynamically adjusted according to the movement trajectory; when the predicted relative distance is between the first and second distance thresholds, a detour path is planned based on the movement trajectory; when the predicted relative distance is less than the second distance threshold, the emergency braking of the tractor is triggered, and the main contactor is disconnected to achieve power-off braking.

[0016] Compared with the prior art, the present invention has the following significant advantages: 1. "Zero-modification" deployment and extremely high positioning robustness: This invention extracts inherent environmental features such as ground wear marks and shelf uprights for ORB feature matching and correction, completely eliminating the huge cost of laying manual markings (magnetic strips / QR codes), and flexibly adapting to dynamic factory areas. Simultaneously, in extreme conditions where lasers fail, it innovatively introduces an extended Kalman filter estimation mechanism combining vision, IMU, and wheel speed odometer, exhibiting strong anti-drift and anti-feature sparsity capabilities, maintaining high-precision and stable operation even in complex scenarios.

[0017] 2. Intelligent Control of Safety Risks: A pioneering visual closed-loop recognition and control system for the number of overloaded trailers in the industrial vehicle sector. Through deep learning and multi-target tracking algorithms, the unmanned vehicle can autonomously determine the number of vehicles being towed behind it. Once it detects personnel illegally exceeding the permitted length of the trailer, it immediately issues an alarm and intervenes, physically preventing serious safety hazards caused by heavy-duty long convoys sweeping around during turns.

[0018] 3. Excellent load versatility and stability: This invention establishes a longitudinal-lateral coupled dynamic model that takes into account parameters such as empty / full load mass and wheelbase. Combined with the hierarchical control of the Dynamic Window Method (DWA), the 2-ton tractor can adaptively adjust acceleration and deceleration under high-frequency switching of different trailers and light and heavy loads, completely solving the problems of sudden start and stop and heavy load tailing.

[0019] 4. Predictive human-machine mixed traffic safety protection: It integrates multi-source perception such as lidar, vision and ultrasound, and predicts the future movement trajectory of obstacles through LSTM network to realize a three-level active safety strategy of "long-distance speed adjustment, medium-distance detour and close-range power failure emergency stop", which comprehensively improves the absolute safety in human-machine mixed traffic and forklift intermittent scenarios.

[0020] 5. Adaptive mechanism for anti-jamming during hook-up and unhooking: Through the feedback closed loop of physical position switch and pressure sensor, the shortcomings of the traditional "hard-on-hard" hook-up mechanism are completely changed, enabling the vehicle to have flexible probing and adjustment capabilities, which greatly improves the docking success rate and service life of the mechanism in uneven ground environments. Attached Figure Description

[0021] Figure 1 This is a flowchart illustrating the automatic hooking and unhooking steps of the present invention. Detailed Implementation

[0022] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to specific embodiments. It should be understood that the specific embodiments described herein are merely illustrative of the invention and are not intended to limit the scope of protection of the invention. Example

[0023] The navigation and control method for an unmanned tractor used in a factory disclosed in this embodiment is developed based on an existing L4 autonomous driving chassis.

[0024] The L4 autonomous driving chassis is equipped with a highly adaptable AGV caster set. The selection of the caster set strictly adheres to the following core physical indicators: Installation height: The casters, drive wheels, and steering wheels must be able to bear force on the same plane, with the installation height difference controlled within 2 mm. An adjustable bracket structure is used, and height fine-tuning is achieved through precision shims and threaded pairs to ensure even load distribution across all four wheels, avoiding slippage and positioning drift caused by single-wheel overload or suspension. Load capacity: The rated load of a single wheel is calculated using the formula: the rated load of a single wheel is greater than or equal to the product of the maximum total mass and gravitational acceleration, divided by the number of casters, and then multiplied by 1.2, with a 20% safety margin reserved to cope with start-stop impacts, uneven loads, and uneven ground conditions. Wheel diameter and obstacle crossing ability: The wheel diameter and obstacle crossing height satisfy the empirical relationship hmax is approximately equal to D divided by 3 to D divided by 4. Rotation radius: The rotation radius of the omnidirectional casters meets R... swivel Equals D wheel Divide by 2 plus H mount Add R clearance D wheel H is the diameter of the wheel body. mount For installation height, R clearance For safety clearance; material and dynamic performance indicators, the casters use high-elasticity polyurethane tread with a Shore hardness of 85A, built-in roller bearings to cope with heavy load impacts, and are equipped with a steering damping mechanism with a damping torque of 1 Nm to suppress directional sway during high-speed travel and prevent the trailer from fishtailing when turning under heavy load.

[0025] The tractor in this embodiment adopts a four-wheel layout, using polyurethane-coated casters with a rated single-wheel load of 800 kg. In this embodiment, casters with a wheel diameter of 200 mm are selected, with a theoretical obstacle clearance height of 50 to 65 mm, which can cross common factory ground joints and small obstacles. At the same time, the large wheel diameter design effectively reduces rolling resistance. The rotation radius of this embodiment is 15 mm. Through three-dimensional modeling and kinematic simulation, it is verified that the minimum clearance between the wheel sets is not less than 20 mm at the steering limit position, avoiding interference with the chassis frame and the hook-and-unhook mechanism.

[0026] The control method in this embodiment is implemented through an additional independent control chip, instead of directly using the chassis's native control chip. The control chip in this embodiment uses the NVIDIA Jetson AGX Orin industrial-grade edge computing module, platform model: 900-13701-0050-000, which has 275 TOPS of AI computing power and can simultaneously handle multiple tasks such as laser SLAM mapping, YOLOv8 target detection inference, and LSTM trajectory prediction.

[0027] The specific operation steps of the navigation control method in this embodiment are as follows: Step S1: Construct a two-dimensional grid map of the work area based on laser SLAM technology.

[0028] When initially deployed in the factory, the tractor unit is controlled to travel at low speed across the factory's work area. A large-scale point cloud data is acquired using the lidar mounted on the tractor unit, and a high-precision two-dimensional grid map of the work area is constructed using lidar SLAM algorithm technology.

[0029] Step S2: Label the tractor's specific semantic information onto the two-dimensional grid map.

[0030] In this preferred embodiment, the tractor's specific semantic information includes docking location, restricted area, and speed limit area.

[0031] Among them: docking position refers to the specific coordinates and required orientation of the hook-up and unhooking operation; restricted area refers to densely populated passages, where the system will set up a virtual wall outside the area; speed limit area includes curves, intersections with obstructed views, etc.

[0032] Step S3: Obtain the initial status information of the tractor.

[0033] When a tractor unit is ready to perform a logistics dispatching task, it first obtains its own initial status information.

[0034] In this preferred embodiment, the initial state information includes: the initial high-precision position information of the tractor in the two-dimensional grid map, whether the trailer has been attached to the rear of the tractor, the current battery power status of the tractor, and the self-test readiness status of each communication link and safety circuit. Subsequent operational steps are only permitted when the battery is sufficient, the trailer has been successfully attached, and there are no abnormal alarms from other systems.

[0035] Further, see Figure 1 When step S3 obtains the status information that the tractor is currently not attached to a trailer, before executing step S4, an automatic hook-and-unhook step is also included, specifically: Step S31: When it is learned that the tractor is not currently carrying a trailer and a task to tow a trailer to a certain docking position is received, the control chip guides the tractor to move towards the docking position.

[0036] In step S32, when the rear of the tractor unit approaches the target trailer, the position detection switch on the tractor unit detects the physical collision signal of the trailer hook. The position detection switch adopts a dual redundancy design of mechanical limit switch and inductive proximity switch, with a detection tolerance of ±3cm. The two signals are confirmed to be valid after being judged by "AND logic". After receiving the signal, the control chip controls the tractor unit's hook-and-unhook mechanism to perform the mechanical connection action. The hook-and-unhook mechanism adopts an electric push rod driven traction pin structure, including a 30mm diameter alloy steel traction pin, a servo motor, a ball screw electric push rod, a spring damper floating base, and an electromagnetic drive locking buckle. The front end of the traction pin is a 15mm tapered guide head with a tapered angle of 30°; the ball screw electric push rod has a stroke of 100mm, a thrust of 2000N, and a position repeatability of ±0.1mm; the spring damper floating base has a horizontal float of ±10mm and a pitch / tilt float of ±5°.

[0037] Furthermore, step S32 also includes real-time acquisition of feedback values ​​from pressure sensors mounted on the hook-and-unhook mechanical mechanism, and determination of whether the feedback value exceeds a first jamming threshold. During the execution of this mechanical connection action, the control chip acquires feedback values ​​from strain gauge pressure sensors connected in series on the actuator in real-time at high frequency. The pressure sensor has a range of 0-5000N and a sampling frequency of 100Hz. If there are potholes on the ground causing the hook ring to tilt and misalign, the traction pin will be jammed. At this time, the pressure will rise sharply, and the control chip will determine whether the feedback value exceeds a preset first jamming threshold. In this embodiment, the first jamming threshold is set to 2800N.

[0038] If the feedback value is less than or equal to the first jamming threshold, continue to execute the mechanical connection action until the trailer is loaded. If the feedback value exceeds the first jamming threshold, the current mechanical connection action is immediately paused, and a flexible adjustment anti-jamming procedure is executed, specifically: Step S321: First, control the tractor to move back and forth at a low speed of 0.1m / s, with a movement range of ±5cm, and simultaneously detect the pressure change trend. If the pressure does not decrease, control the floating base to perform ±5mm horizontal displacement compensation. If the jamming is still not resolved, control the tractor to perform ±3° heading angle swing adjustment.

[0039] Step S322: Repeatedly execute the fine-tuning action of step S321 and retry the mechanical connection. After each round of adjustment, retry advancing the traction pin at a low speed of 20mm / s until the feedback value of the pressure sensor drops below the second jamming threshold and remains stable for more than 1 second. Then exit the flexible adjustment anti-jamming step and return to step S32. The second jamming threshold is 1500N. By setting the second jamming threshold to be much lower than the first jamming threshold, a buffering effect can be achieved, allowing the flexible adjustment anti-jamming step to be fully implemented and to get out of the jamming state, avoiding frequent switching between jamming and non-jamming.

[0040] Step S33: When the traction pin position feedback in the hook-and-unhook mechanism is "fully extended", the locking buckle position switch triggers the "locked" signal, and the feedback value is less than or equal to the jamming threshold, if all three conditions are met, the connection is confirmed to be complete, and then proceed to step S4.

[0041] This detection mechanism, combined with the precise force control of the electric push rod and the attitude self-adaptation capability of the floating base, solves the problems of existing hook-and-unhook mechanisms being prone to jamming, uneven force, and damage to the traction mechanism under load fluctuations, uneven ground, and alignment deviations, thus avoiding damage to the mechanism caused by forced pushing and pulling.

[0042] If it is found that the tractor is currently attached to a trailer, proceed directly to step S4.

[0043] Step S4: Input the start and end points into the 2D grid map. Based on the tractor's specific semantic information and initial state information, plan the tractor's running path, and then guide the tractor to run according to the planned path. Here, the tractor's initial state information is its position information.

[0044] Furthermore, the method for generating the running path is as follows: In step S41, the control chip receives the start and end points of the logistics scheduling task.

[0045] Step S42, using an improved The algorithm generates an initial path. The traditional A* algorithm only considers the shortest distance, while this embodiment uses an improved method. The algorithm expands the state space to (x, y, θ), which includes position, heading angle, and turn angle information. Its heuristic function is f(n) = g(n) + d. euclid +C turn +C reverse The turning cost C is forcibly introduced. turn (Proportional to the change in heading angle |Δθ|, example coefficient 0.5m / rad) and reversing penalty C reverse(Additional 2.0m cost). The algorithm starts from the starting point, establishing a node structure containing the state space. During initialization, the starting point is added to the Open List. The main loop selects the node with the smallest f value from the Open List. If the distance between this node and the destination is less than 0.3m and the heading angle deviation is less than 5°, the path is extracted; otherwise, it is moved to the Closed List, and neighboring nodes are expanded based on the bicycle kinematics model. Neighboring nodes must satisfy the minimum turning radius constraint R. min =2.5m (steering angle constraint is |φ|≤arctan(L / R)) min (L is the wheelbase), with an example step size of 0.5m. In reverse mode, a reversing penalty is accumulated. Collision detection is performed on each neighboring node, using a 2D grid map to determine if it is located in a restricted area or if the distance to an obstacle is less than a safe threshold; if a collision occurs, the node is discarded. If a neighboring node is not in the Open List, its g-value is calculated and added; if it already exists, the smaller g-value is retained and the parent node pointer is updated. This process is repeated until the destination is found or the Open List is empty. The final output path is fitted with a B-spline curve to generate a smooth running path that satisfies the minimum turning radius, avoids restricted areas, prioritizes forward movement, and has a gradual curvature change.

[0046] Step S5: During the operation of the tractor, the data sensing device installed on the tractor collects multi-source data, analyzes the multi-source data, and optimizes the tractor's operating path.

[0047] In this embodiment, the data sensing device includes a lidar, an industrial vision camera, and an ultrasonic sensor mounted on the tractor. The lidar is a SICKTiM 571 lidar with a scanning frequency of 15Hz and an angular resolution of 0.33°, used for 360-degree horizontal scanning to acquire data. The industrial vision camera is a Baslerac A1920-155uc industrial vision camera with a resolution of 1920×1200 and a frame rate of 155fps, used for forward and rear-view image acquisition. The ultrasonic device acquires information about the surroundings of the tractor.

[0048] In this embodiment, the optimization of the tractor's running path is achieved through a dynamic map update mechanism. During the tractor's operation, multi-source data collected by LiDAR and industrial vision cameras is used to determine whether there are any newly added, non-moving obstacles at a given coordinate point. For example, if a new temporary shelving unit is erected, the system will mark it on the two-dimensional grid map in real time to dynamically update the map, enabling it to adapt to adjustments in the factory layout.

[0049] Furthermore, the specific steps for optimizing the tractor's operating path are as follows: Step S501: During the operation of the tractor, the point cloud data obtained by the LiDAR scan is matched with the pre-built high-precision two-dimensional grid map to provide a global stable pose reference. Step S502: Use industrial vision cameras to extract natural inherent environmental features in the work area at high frequency, such as ground wear marks, floor joints, and shelf column outlines, and construct a multi-scale semantic feature library; add new natural inherent environmental features in real time to dynamically update the two-dimensional grid map. Step S503: By using an improved adaptive ORB feature matching algorithm, integrating dynamic illumination suppression and motion blur compensation mechanisms, and combining multi-level RANSAC geometric verification to eliminate dynamic interference, visual observation and laser odometry are tightly coupled and nonlinearly optimized to improve the running path, correct laser drift, and output positioning information.

[0050] Unlike existing technologies that rely on deploying numerous QR codes on-site, this invention employs a "zero-modification" natural feature fusion positioning method. This approach not only effectively eliminates the cumulative drift of a single sensor but also overcomes computing power bottlenecks through a parallel computing acceleration strategy. This enables centimeter-level positioning correction even under complex conditions, maintaining a stable positioning output accuracy better than ±10mm, with a positioning update frequency ≥50Hz.

[0051] Furthermore, to address the issue of instantaneous lidar failure caused by long, straight corridors or strong outdoor light, leading to inaccurate positioning, this embodiment also establishes a mechanism for a multi-source fusion Kalman filter framework, specifically as follows: Step S5011: The control chip monitors the number of feature matches and the confidence level of the positioning covariance output by the lidar in real time.

[0052] Step S5012: When the number of feature matches is lower than the first preset threshold and the confidence level of the positioning covariance is lower than the second preset threshold, it is determined that the lidar has experienced a short-term failure, and automatically and seamlessly switches to the redundant calculation mode that fuses vision, inertial measurement unit (IMU) and wheel speed odometer. Step S5013: First, a 15-dimensional state vector containing position, velocity, attitude, and sensor bias is constructed. The high-frequency angular velocity and acceleration data of the inertial measurement unit are used as input, and the kinematic model is used for time update to calculate the prior state and covariance of the next moment. This step is the prediction step. Step S5014 involves introducing multi-source observations for measurement updates: the visual odometry calculates relative pose changes through feature matching, providing lateral position constraints to correct cumulative drift; the wheel speed odometry, combined with the steering angle model, provides longitudinal displacement and absolute scale information, effectively suppressing error divergence caused by slippage; and the inertial measurement unit continuously provides attitude angle and angular velocity observations under high dynamic conditions; this step is a verification step. The aforementioned multi-source heterogeneous data are tightly coupled and fused using an extended Kalman filter (EKF) framework: a first-order Taylor expansion is used to calculate the Jacobian matrix in the nonlinear stage to linearize the observation equations, and the weights of each sensor are dynamically adjusted based on the adaptive noise covariance matrix to finally output the optimal estimated pose. This closed-loop mechanism ensures that even under extreme conditions such as lidar failure or drastic environmental changes, a stable high-frequency output of ≥50Hz can still be maintained, and the short-term cumulative positioning error is strictly controlled within ±10mm.

[0053] When the number of laser feature matches returns to normal, the system automatically switches back to primary fusion positioning and corrects minor accumulated errors generated during the calculation using a global map. If both industrial vision cameras fail simultaneously, the system switches to pure IMU and wheel speed odometer calculation mode and triggers speed limit protection. It automatically switches back to primary positioning once either sensor recovers.

[0054] Step S6: Determine if the tractor has reached the destination. If not, return to step S5; if it has, execute unloading or wait for a new task instruction, and return to step S3 after receiving a new task. Specifically, the tractor travels smoothly and safely through continuous sensing, positioning, prediction, and control cycles. When the local planner and high-precision positioning match and confirm that the vehicle coordinates coincide with the target destination workstation coordinates, it is determined that the destination has been reached, and the single delivery task is completed. Subsequently, the vehicle performs an automatic unhooking action to unload the goods and enters a waiting state until a new task instruction is received. After that, the initial state information is reacquired and the process returns to step S3; if the destination has not been reached, state adjustment and path optimization continue in step S5. Example

[0055] This embodiment is a preferred solution for step S5 in Embodiment 1. If the trailer attached to the tractor is overloaded, it will pose a safety risk to the tractor during operation. This safety risk arises because, in a factory work environment, operators may arbitrarily attach trailers at any location along the tractor's path (such as temporary stops, intersection waiting areas, or loading / unloading stations) to attempt to carry more goods in a single trip. Therefore, this embodiment also establishes a step in step S5 to identify whether the attached trailer is overloaded based on rearward visual information. This step performs real-time identification throughout the tractor's operation. Detection only at the starting point cannot cover dynamic illegal attachment behaviors during operation. This function can identify overload in real-time throughout the entire operation without requiring the vehicle to be stationary.

[0056] The specific identification steps are as follows: Step S511: First, in the learning phase, trailer detection models are built based on the YOLOv8n architecture using trailer images of different specifications collected at the factory site. Transfer learning is performed on the basis of COCO pre-trained weights, the backbone network parameters are frozen, only the detection head is trained, and online incremental learning is supported. New models only need 50-100 samples to be updated.

[0057] In step S512, during the running phase, the model receives images captured by an industrial vision camera in a backward position, outputs bounding boxes and class confidence scores, and performs multi-target tracking using the DeepSORT algorithm: Kalman filtering is used to predict the trailer's trajectory, the Hungarian algorithm is used to match targets in the preceding and following frames, the IoU matrix is ​​calculated to achieve data association, and the number of independent trailing trailers is counted in real time.

[0058] Step S513: When the number of trailers detected exceeds the set threshold (e.g., 2 if specified, 3 if actual) for 3 consecutive frames, the system triggers a warning state, drives the vehicle body warning lights and sound module to alarm, and links the chassis to perform intervention control. The intervention control can lock the vehicle to prevent high-speed driving until the excess trailers are unloaded.

[0059] This mechanism complements the automatic coupling and uncoupling system: the automatic coupling and uncoupling system is responsible for compliant connections in the standard operating procedures, while the visual counting function specifically monitors and handles manual unauthorized coupling behavior throughout the operation. Together, they ensure traction safety. Example

[0060] This embodiment is a preferred solution for step S5 in Embodiment 1.

[0061] In complex workshops where humans and machines coexist, this embodiment breaks through the traditional passive distance-measuring braking mode. Emergency braking of the tractor is achieved by establishing a spatiotemporal synchronization fusion mechanism. The specific emergency braking steps are as follows: Step S521: The 360° horizontal scanning data from the SICKTiM571 lidar, the semantic segmentation data provided by the Baslerac A1920-155uc camera, and the near-field blind zone data detected by the HC-SR04 ultrasonic sensor are unified into the vehicle's rear axle center coordinate system through hardware timestamp alignment and hand-eye calibration matrix transformation. Step S522: The confidence of multi-source perception is fused using the DS evidence theory: First, a basic confidence function is assigned to the obstacle existence probability output by each sensor, and then the joint confidence is calculated by the Dempster combination rule to output comprehensive obstacle information including location, category and motion state. Step S523: Building upon step S522, a safety model is constructed that includes trajectory prediction, risk assessment, and decision-making. The first-layer trajectory prediction model is based on a Long Short-Term Memory (LSTM) network. The input layer receives the historical trajectory sequence of obstacles (position coordinates within the past 2 seconds), the hidden layer uses 128-dimensional state units to capture motion patterns, and the output layer predicts trajectory points within the next 3 seconds. This model is trained using datasets of worker movement and forklift maneuvering collected from the factory, and can identify abnormal behaviors such as acceleration, deceleration, turning, and hesitation. The second-layer risk assessment module calculates the time-to-collision (TTC) between the predicted trajectory and the vehicle's planned path. When the TTC is less than 3 seconds, a warning is triggered. Finally, proactive hierarchical predictive safety control is implemented. Long-distance response (prediction distance > 5m): LSTM continuously predicts the trajectory within the next 3 seconds at a frequency of 50 Hz. When it is inferred that an obstacle will cross the path, the system dynamically adjusts the vehicle speed to 0.5m / s and triggers a buzzer warning. The prediction period is matched with the braking response time of 0.3 seconds to ensure that deceleration is completed before the obstacle enters the 2-meter range.

[0062] Mid-range response (2m < predicted distance ≤ 5m): The local path planner uses cubic B-spline curves to generate detour trajectories in real time while meeting the channel width requirements. The trajectory curvature constraint is 0.1m⁻¹ to ensure stability.

[0063] Close-range response (predicted distance ≤ 2m): If a person is detected suddenly running out, the system will instantly trigger emergency braking, reducing the vehicle speed to 0 within 0.3 seconds, and simultaneously disconnecting the main contactor to achieve power-off mechanical braking, ensuring absolute safety. Example

[0064] This embodiment is a preferred solution for step S5 in Embodiment 1.

[0065] This embodiment takes a 2-ton tractor as an example. To address the problem of the huge difference in inertia when switching between light and heavy loads, a longitudinal-lateral coupled dynamic model is established at the bottom layer.

[0066] In the longitudinal motion equation, the traction force F t Subject to acceleration and deceleration constraints, its expression is F t =ma+F roll +F gradeThe system includes unloaded / fully loaded mass variables. The lateral motion equations are based on a bicycle model, considering wheelbase, track width, and minimum turning radius constraints. During operation, the controller dynamically estimates the current actual load mass *m* by real-time sampling of the ratio of the traction motor's drive current to the vehicle's acceleration. In the local path optimization layer, the controller dynamically adjusts the speed and position loop gains of the servo motors based on the estimated load mass. Especially when the vehicle is determined to be under heavy load (with a significant increase in moment of inertia), the speed sampling space is reduced online in real-time using a dynamic window method, prioritizing local trajectories with extremely gentle curvature changes, while strictly limiting the maximum angular velocity during turning and the acceleration at startup. Through this adaptive PID control, the material drop caused by sudden starts and stops under heavy loads and the severe tail-wagging deviation of the trailer train during turns are completely eliminated.

[0067] The effective execution of the aforementioned layered motion control relies on the highly adaptable design of the chassis's running gear. This embodiment employs a modular omnidirectional caster assembly, whose selection criteria are closely matched to the longitudinal-lateral coupled dynamic model: the caster installation height is coplanar with the drive wheel and steering wheel, with a height difference of no more than 2 mm. Fine-tuning is achieved through adjustable brackets and precision shims to ensure uniform force distribution across all four wheels under both unloaded and fully loaded conditions. The single-wheel load capacity is designed with a 20% safety margin based on a 2-ton full load, using polyurethane-coated casters with a single-wheel rated load of 800 kg to meet heavy-load impact conditions. The 200 mm wheel diameter is determined based on obstacle-crossing requirements, where hmax is approximately equal to D divided by 3 to D divided by 4, allowing it to traverse 50 to 65 mm ground obstacles and reduce rolling resistance. The rotation radius is verified through kinematic simulation to ensure no interference with the chassis and hook-and-unhook mechanism during steering. The casters are equipped with a steering damping mechanism with a damping torque of 1.0 Nm, which, in conjunction with the dynamic window method, suppresses high-speed swaying, assisting in eliminating tail-wagging from a mechanical structure perspective, forming a closed-loop optimization mechanism between the control algorithm and hardware design.

[0068] In summary, this invention achieves highly efficient, stable, and fully safe and controllable unmanned flexible delivery of 2-ton tractors in complex and dynamic factory areas through multi-dimensional perception fusion and predictive control, supplemented by sophisticated dynamic models and visual monitoring methods.

[0069] Although the present invention has been described in detail above, those skilled in the art should understand that the above are merely preferred embodiments of the present invention and do not limit the scope of protection of the present invention. Any modifications, equivalent substitutions, and improvements made without departing from the spirit and scope of the present invention should be included within the scope of protection of the present invention.

Claims

1. A navigation and control method for an unmanned tractor vehicle used in a factory, characterized in that, Includes the following steps: Step S1: Construct a two-dimensional grid map of the work area based on laser SLAM technology; Step S2: Label the tractor's specific semantic information onto the two-dimensional grid map; Step S3: Obtain the initial status information of the tractor. Step S4: Input the starting point and the ending point into the two-dimensional grid map, plan the running path of the tractor based on the tractor's specific semantic information and the tractor's initial state information, and then guide the tractor to run according to the planned running path. Step S5: During the operation of the tractor, the data sensing device installed on the tractor collects multi-source data, analyzes the multi-source data, and optimizes the tractor's operating path. Step S6: Determine whether the tractor has reached the destination. If it has not reached the destination, return to step S5. If it has reached the destination, perform unloading or wait for a new task instruction, and return to step S3 after receiving the new task.

2. The navigation control method according to claim 1, characterized in that, In step S2, the tractor's specific semantic information includes docking location, restricted area, and speed limit area.

3. The navigation control method according to claim 1, characterized in that, In step S3, the initial status information of the tractor includes: the location information of the tractor in the two-dimensional grid map, whether the tractor is currently carrying a trailer, and the battery power status of the tractor.

4. The navigation control method according to claim 3, characterized in that, When the tractor unit receives information indicating that it is currently not carrying a trailer and receives a task to tow a trailer to the docking position, it executes the automatic trailer attachment and unattachment process, specifically as follows: Step S31: Guide the tractor to the docking position; Step S32: When the position detection switch on the tractor detects the position signal of the trailer hook ring, the mechanical connection action between the tractor's hook-and-unhook mechanism and the trailer is executed. Step S33: When the position feedback of the traction pin in the hook-and-unhook mechanism is fully extended and the locking buckle position switch triggers the locked signal, the connection is confirmed and proceed to step S4.

5. The navigation control method according to claim 4, characterized in that, Step S32 also includes acquiring the feedback value of the pressure sensor installed on the hook-and-unhook mechanical mechanism in real time, and determining whether the feedback value is greater than the first jamming threshold. If the feedback value is less than or equal to the first jamming threshold, continue to execute the mechanical connection action until the trailer is loaded. If the feedback value is greater than the first jamming threshold, the mechanical connection action is stopped, and a flexible adjustment anti-jamming procedure is performed, specifically as follows: Step S321: Control the tractor to move forward and backward, and simultaneously control the floating base of the hook-and-unhook mechanism to perform horizontal displacement compensation, and control the tractor to perform yaw angle swing adjustment. Step S322: Repeatedly execute the fine-tuning action of step S321 and retry the mechanical connection until the feedback value is less than or equal to the second jamming threshold and remains stable. Then exit the flexible adjustment anti-jamming step and return to step S32. The first stagnation threshold is greater than the second stagnation threshold.

6. The navigation control method according to claim 1, characterized in that, The specific steps for planning the running path in step S4 are as follows: Step S41: Receive the start point and the end point; Step S42, using an improved The algorithm introduces turning costs and reversing penalties to generate an initial running path that satisfies the minimum turning radius.

7. The navigation control method according to claim 1, characterized in that, Data sensing devices include lidar and industrial vision cameras mounted on the tractor unit; In step S5, the specific steps for optimizing the running path are as follows: Step S501: During the operation of the tractor, the point cloud data obtained by the LiDAR scan is matched with the pre-built high-precision two-dimensional grid map to provide a global stable pose reference. Step S502: Use an industrial vision camera to extract the natural inherent environmental features in the work area and construct a multi-scale semantic feature library; add new natural inherent environmental features in real time to dynamically update the two-dimensional grid map. Step S503: By using an improved adaptive ORB feature matching algorithm, integrating dynamic illumination suppression and motion blur compensation mechanisms, and combining multi-level RANSAC geometric verification to eliminate dynamic interference, visual observation and laser odometry are tightly coupled and nonlinearly optimized to improve the running path, correct laser drift, and output positioning information.

8. The navigation control method according to claim 7, characterized in that, The data sensing device includes an inertial measurement unit and a wheel speed-odometer; Step S501 also establishes a mechanism for a multi-source fusion Kalman filter framework, specifically as follows: Step S5011: Real-time monitoring of the number of feature matches and the confidence level of the localization covariance output by the lidar; Step S5012: When the number of feature matches is lower than the first preset threshold and the confidence level of the positioning covariance is lower than the second preset threshold, it is determined that the laser is short-term failure and the system switches to the redundant calculation mode that integrates vision, inertial measurement unit and wheel speed odometer. Step S5013: Using the high-frequency angular velocity and acceleration data of the inertial measurement unit as input, the kinematic model is used to perform time updates, and the prior state and covariance of the next moment are calculated. Step S5014 introduces multi-source observations for measurement updates: the visual odometry solves relative pose changes through feature matching and provides lateral position constraints to correct cumulative drift; the wheel speed odometry combines the steering angle model to provide longitudinal displacement and absolute scale information to suppress error divergence caused by slippage; and the inertial measurement unit continuously provides attitude angle and angular velocity observations under high dynamic conditions.

9. The navigation control method according to claim 1, characterized in that, Step S5 also includes a step of monitoring whether the trailer attached to the tractor is overloaded, specifically: Step S511: Invoke the pre-established trailer self-learning model based on the object detection architecture; Step S512: Receive images acquired in real time by an industrial vision camera, output trailer bounding boxes and category confidence scores; combine multi-target tracking algorithms, use Kalman filtering to predict trajectories and use the Hungarian algorithm to match targets in previous and next frames, and identify and count the number of trailers currently being towed in real time. Step S513: When the number of currently towed trailers exceeds the set number threshold in multiple consecutive frames, a warning state is triggered, the alarm device is activated to issue a warning and execute intervention control.

10. The method according to claim 1, characterized in that, Step S5 also includes a hierarchical human-machine hybrid safety control step based on a predictive model, specifically: a spatiotemporal synchronization fusion mechanism based on timestamp alignment and coordinate transformation is used to fuse two-dimensional environmental scanning data from lidar, obstacle category data from industrial vision cameras, and near-range blind spot detection data from ultrasonic sensors; multi-source confidence is fused using DS evidence theory to output comprehensive obstacle information; a safety model is established that includes a trajectory prediction model, a risk assessment model, and a decision model; a long short-term memory network is used to learn the movement patterns of abnormally approaching personnel or objects and predict their movement trajectories within a set time period in the future; hierarchical predictive safety control is executed: where the first distance threshold is greater than the second distance threshold; when the predicted relative distance is greater than the first distance threshold, the vehicle speed is dynamically adjusted according to the movement trajectory; when the predicted relative distance is between the first and second distance thresholds, a detour path is planned based on the movement trajectory; when the predicted relative distance is less than the second distance threshold, the emergency braking of the tractor is triggered, and the main contactor is disconnected to achieve power-off braking.

Images